diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 22cba75..429860c 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -5,7 +5,7 @@ rotate_planner_name: PNKXRotateLocalPlanner base_local_planner: LocalPlannerAdapter base_global_planner: CustomPlanner -robot_base_frame: base_footprint +robot_base_frame: base_link transform_tolerance: 1.0 obstacle_range: 3.0 #mark_threshold: 1 diff --git a/config/costmap_global_params.yaml b/config/costmap_global_params.yaml index 5c88dc6..811ab13 100755 --- a/config/costmap_global_params.yaml +++ b/config/costmap_global_params.yaml @@ -1,6 +1,6 @@ global_costmap: library_path: libplugins - robot_base_frame: base_footprint + robot_base_frame: base_link global_frame: map update_frequency: 1.0 publish_frequency: 1.0 diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml index 6af3ebb..ecd9e9d 100755 --- a/config/costmap_local_params.yaml +++ b/config/costmap_local_params.yaml @@ -1,7 +1,7 @@ local_costmap: library_path: libplugins global_frame: odom - robot_base_frame: base_footprint + robot_base_frame: base_link update_frequency: 6.0 publish_frequency: 6.0 rolling_window: true diff --git a/config/mprim/ekf.yaml b/config/mprim/ekf.yaml index 4bf871d..ff05f12 100755 --- a/config/mprim/ekf.yaml +++ b/config/mprim/ekf.yaml @@ -62,7 +62,7 @@ publish_acceleration: false # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified -base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified +base_link_frame: $(arg tf_prefix)base_link # Defaults to "base_link" if unspecified world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (robot_nav_msgs/Odometry, diff --git a/config/mprim/localization.yaml b/config/mprim/localization.yaml index 677cb96..2ed52b8 100644 --- a/config/mprim/localization.yaml +++ b/config/mprim/localization.yaml @@ -25,7 +25,7 @@ Amcl: update_min_d: 0.05 update_min_a: 0.05 odom_frame_id: odom - base_frame_id: base_footprint + base_frame_id: base_link global_frame_id: map resample_interval: 1 transform_tolerance: 0.2 diff --git a/config/mprim/ros_diff_drive_controller.yaml b/config/mprim/ros_diff_drive_controller.yaml index e498438..90b2db6 100755 --- a/config/mprim/ros_diff_drive_controller.yaml +++ b/config/mprim/ros_diff_drive_controller.yaml @@ -25,7 +25,7 @@ wheel_radius_multiplier : 1.0 # default: 1.0 cmd_vel_timeout: 1.0 # frame_ids (same as real MiR platform) -base_frame_id: base_footprint # default: base_link base_footprint +base_frame_id: base_link # default: base_link base_link odom_frame_id: odom # default: odom # Velocity and acceleration limits diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 9c205af..9b3aa9d 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -1,4 +1,4 @@ -yaw_goal_tolerance: 0.017 +yaw_goal_tolerance: 0.03 xy_goal_tolerance: 0.02 min_approach_linear_velocity: 0.05 @@ -41,8 +41,8 @@ PNKXRotateLocalPlanner: LimitedAccelGenerator: library_path: libmkt_plugins_standard_traj_generator - max_vel_x: 1.2 - min_vel_x: -1.2 + max_vel_x: 0.2 + min_vel_x: -0.2 max_vel_y: 0.0 # diff drive robot min_vel_y: 0.0 # diff drive robot diff --git a/examples/NavigationExample/NavigationAPI.cs b/examples/NavigationExample/NavigationAPI.cs new file mode 100644 index 0000000..e6e87b3 --- /dev/null +++ b/examples/NavigationExample/NavigationAPI.cs @@ -0,0 +1,292 @@ +using System; +using System.Runtime.InteropServices; +using NavigationExample; + +namespace NavigationExample +{ + /// + /// C# P/Invoke wrapper for Navigation C API + /// + public class NavigationAPI + { + private const string DllName = "libnav_c_api.so"; // Linux + // For Windows: "nav_c_api.dll" + // For macOS: "libnav_c_api.dylib" + + // ============================================================================ + // Enums + // ============================================================================ + public enum NavigationState + { + Pending = 0, + Active = 1, + Preempted = 2, + Succeeded = 3, + Aborted = 4, + Rejected = 5, + Preempting = 6, + Recalling = 7, + Recalled = 8, + Lost = 9, + Planning = 10, + Controlling = 11, + Clearing = 12, + Paused = 13 + } + + [StructLayout(LayoutKind.Sequential)] + public struct NavFeedback + { + public NavigationState navigation_state; + public IntPtr feed_back_str; // char*; free with nav_c_api_free_string + public Pose2D current_pose; + [MarshalAs(UnmanagedType.I1)] + public bool goal_checked; + [MarshalAs(UnmanagedType.I1)] + public bool is_ready; + } + + + /// Planner data output (plan, costmap, footprint). + [StructLayout(LayoutKind.Sequential)] + public struct PlannerDataOutput + { + public Path2D plan; + public OccupancyGrid costmap; + public OccupancyGridUpdate costmap_update; + [MarshalAs(UnmanagedType.I1)] + public bool is_costmap_updated; + public PolygonStamped footprint; + } + + [StructLayout(LayoutKind.Sequential)] + public struct NavigationHandle + { + public IntPtr ptr; + } + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern Header header_create(string frame_id); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern Header header_set_data( + uint seq, + uint sec, + uint nsec, + string frame_id); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern Time time_create(); + + /// Free a string allocated by the API (strdup). + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void nav_c_api_free_string(IntPtr str); + + /// Convert NavigationState to string; caller must free with nav_c_api_free_string. + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern IntPtr navigation_state_to_string(NavigationState state); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback); + + + /// Helper: copy unmanaged char* to managed string; does not free the pointer. + public static string MarshalString(IntPtr p) + { + if (p == IntPtr.Zero) return string.Empty; + return Marshal.PtrToStringAnsi(p) ?? string.Empty; + } + + /// Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done. + public static void navigation_free_feedback(ref NavFeedback feedback) + { + if (feedback.feed_back_str != IntPtr.Zero) + { + nav_c_api_free_string(feedback.feed_back_str); + feedback.feed_back_str = IntPtr.Zero; + } + } + + // ============================================================================ + // Navigation Handle Management + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern NavigationHandle navigation_create(); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_destroy(NavigationHandle handle); + + /// Initialize navigation using an existing tf3 buffer (from libtf3). Caller owns the buffer and must call tf3_buffer_destroy after navigation_destroy. + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_initialize(NavigationHandle handle, IntPtr tf3_buffer); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count); + + /// Send a goal for the robot to navigate to (global frame). + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal); + + /// Navigate using an Order message (graph nodes/edges). Order must be built or obtained from native side; call order_free when done if it was allocated by native. + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal); + + /// Goal is passed by reference to match C API: navigation_dock_to_order(..., const PoseStamped &goal). + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, ref PoseStamped goal); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance); + + /// Rotate in place to align with target orientation (radians). + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_rotate_to(NavigationHandle handle, double goal_yaw); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_pause(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_resume(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_cancel(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist); + + // ============================================================================ + // Navigation Data Management + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_point_cloud(NavigationHandle handle, string point_cloud_name, PointCloud point_cloud); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_point_cloud2(NavigationHandle handle, string point_cloud2_name, PointCloud2 point_cloud2); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_laser_scan(NavigationHandle handle, string laser_scan_name, ref LaserScan out_scan); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_point_cloud(NavigationHandle handle, string point_cloud_name, ref PointCloud out_cloud); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_point_cloud2(NavigationHandle handle, string point_cloud2_name, ref PointCloud2 out_cloud); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_static_map(NavigationHandle handle, string map_name); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_laser_scan(NavigationHandle handle, string laser_scan_name); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_point_cloud(NavigationHandle handle, string point_cloud_name); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_point_cloud2(NavigationHandle handle, string point_cloud2_name); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_static_maps(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_laser_scans(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_point_clouds(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_point_cloud2s(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_data(NavigationHandle handle); + + /// Get all static maps. out_maps must be pre-allocated; use navigation_get_all_static_maps_count or similar to get count first if needed. + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_all_static_maps(NavigationHandle handle, [Out] NamedOccupancyGrid[] out_maps, ref UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_all_laser_scans(NavigationHandle handle, [Out] NamedLaserScan[] out_scans, ref UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_all_point_clouds(NavigationHandle handle, [Out] NamedPointCloud[] out_clouds, ref UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_all_point_cloud2s(NavigationHandle handle, [Out] NamedPointCloud2[] out_clouds, ref UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_global_data(NavigationHandle handle, ref PlannerDataOutput out_data); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_local_data(NavigationHandle handle, ref PlannerDataOutput out_data); + } +} \ No newline at end of file diff --git a/examples/NavigationExample/NavigationExample.csproj b/examples/NavigationExample/NavigationExample.csproj index 48dbb2c..ad63abd 100644 --- a/examples/NavigationExample/NavigationExample.csproj +++ b/examples/NavigationExample/NavigationExample.csproj @@ -1,7 +1,7 @@ Exe - net6.0 + net10.0 linux-x64 true diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 3b1a84b..ceedc3e 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -6,6 +6,8 @@ using System.Runtime.CompilerServices; using System.Text.RegularExpressions; using SixLabors.ImageSharp; using SixLabors.ImageSharp.PixelFormats; +using NavigationExample; + namespace NavigationExample { @@ -19,366 +21,6 @@ namespace NavigationExample public double OccupiedThresh; public double FreeThresh; } - /// - /// C# P/Invoke wrapper for Navigation C API - /// - public class NavigationAPI - { - private const string DllName = "libnav_c_api.so"; // Linux - // For Windows: "nav_c_api.dll" - // For macOS: "libnav_c_api.dylib" - - // ============================================================================ - // Enums - // ============================================================================ - public enum NavigationState - { - Pending = 0, - Active = 1, - Preempted = 2, - Succeeded = 3, - Aborted = 4, - Rejected = 5, - Preempting = 6, - Recalling = 7, - Recalled = 8, - Lost = 9, - Planning = 10, - Controlling = 11, - Clearing = 12, - Paused = 13 - } - - // ============================================================================ - // Structures - // ============================================================================ - [StructLayout(LayoutKind.Sequential)] - public struct Point - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Pose2D - { - public double x; - public double y; - public double theta; - } - - [StructLayout(LayoutKind.Sequential)] - public struct NavFeedback - { - public NavigationState navigation_state; - public IntPtr feed_back_str; // char*; free with nav_c_api_free_string - public Pose2D current_pose; - [MarshalAs(UnmanagedType.I1)] - public bool goal_checked; - [MarshalAs(UnmanagedType.I1)] - public bool is_ready; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Twist2D - { - public double x; - public double y; - public double theta; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Quaternion - { - public double x; - public double y; - public double z; - public double w; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Position - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Pose - { - public Position position; - public Quaternion orientation; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct Vector3 - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Twist - { - public Vector3 linear; - public Vector3 angular; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Header - { - public uint seq; - public uint sec; - public uint nsec; - public IntPtr frame_id; // char* - } - - [StructLayout(LayoutKind.Sequential)] - public struct PoseStamped - { - public Header header; - public Pose pose; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Twist2DStamped - { - public Header header; - public Twist2D velocity; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct NavigationHandle - { - public IntPtr ptr; - } - - [StructLayout(LayoutKind.Sequential)] - public struct TFListenerHandle - { - public IntPtr ptr; - } - - [StructLayout(LayoutKind.Sequential)] - public struct LaserScan - { - public Header header; - public float angle_min; - public float angle_max; - public float angle_increment; - public float time_increment; - public float scan_time; - public float range_min; - public float range_max; - public IntPtr ranges; - public UIntPtr ranges_count; - public IntPtr intensities; - public UIntPtr intensities_count; - } - - [StructLayout(LayoutKind.Sequential)] - public struct PoseWithCovariance - { - public Pose pose; - public IntPtr covariance; - public UIntPtr covariance_count; - } - - [StructLayout(LayoutKind.Sequential)] - public struct TwistWithCovariance { - public Twist twist; - public IntPtr covariance; - public UIntPtr covariance_count; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Odometry - { - public Header header; - public IntPtr child_frame_id; - public PoseWithCovariance pose; - public TwistWithCovariance twist; - } - - [StructLayout(LayoutKind.Sequential)] - public struct OccupancyGrid - { - public Header header; - public MapMetaData info; - public IntPtr data; - public UIntPtr data_count; - } - - [StructLayout(LayoutKind.Sequential)] - public struct MapMetaData - { - public Time map_load_time; - public float resolution; - public uint width; - public uint height; - public Pose origin; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Time - { - public uint sec; - public uint nsec; - } - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - public static extern Header header_create(string frame_id); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - public static extern Header header_set_data( - uint seq, - uint sec, - uint nsec, - string frame_id); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern Time time_create(); - - /// Free a string allocated by the API (strdup). - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void nav_c_api_free_string(IntPtr str); - - /// Convert NavigationState to string; caller must free with nav_c_api_free_string. - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - public static extern IntPtr navigation_state_to_string(NavigationState state); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback); - - - /// Helper: copy unmanaged char* to managed string; does not free the pointer. - public static string MarshalString(IntPtr p) - { - if (p == IntPtr.Zero) return string.Empty; - return Marshal.PtrToStringAnsi(p) ?? string.Empty; - } - - /// Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done. - public static void navigation_free_feedback(ref NavFeedback feedback) - { - if (feedback.feed_back_str != IntPtr.Zero) - { - nav_c_api_free_string(feedback.feed_back_str); - feedback.feed_back_str = IntPtr.Zero; - } - } - - // ============================================================================ - // TF Listener Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern TFListenerHandle tf_listener_create(); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void tf_listener_destroy(TFListenerHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool tf_listener_set_static_transform( - TFListenerHandle tf_handle, - string parent_frame, - string child_frame, - double x, double y, double z, - double qx, double qy, double qz, double qw); - - // ============================================================================ - // Navigation Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern NavigationHandle navigation_create(); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_destroy(NavigationHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_rotate_to(NavigationHandle handle, PoseStamped goal, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_pause(NavigationHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_resume(NavigationHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_cancel(NavigationHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist); - // ============================================================================ - // Navigation Data Management - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid); - - } // ============================================================================ // Example Usage @@ -497,69 +139,53 @@ namespace NavigationExample static void Main(string[] args) { - // Create TF listener (required for costmap and local planner; must be valid when calling navigation_initialize) - NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create(); - if (tfHandle.ptr == IntPtr.Zero) + // Create tf3 buffer (replaces TF listener; used for all static transforms and navigation init) + IntPtr tf3Buffer = TF3API.tf3_buffer_create(10); + if (tf3Buffer == IntPtr.Zero) { - LogError("Failed to create TF listener"); + LogError("Failed to create tf3 buffer (libtf3.so may be missing)"); return; } - Console.WriteLine($"[NavigationExample] TF listener created, handle = 0x{tfHandle.ptr.ToInt64():X16}"); + Console.WriteLine($"[NavigationExample] TF3 buffer created, handle = 0x{tf3Buffer.ToInt64():X16}"); + string version = Marshal.PtrToStringAnsi(TF3API.tf3_get_version()) ?? "?"; + Console.WriteLine($"[TF3] {version}"); - // Inject a static TF so costmap can immediately canTransform(map <-> base_link). - // If you already publish TF from localization/odometry, you can remove this call. - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "map", "odom", - 0, 0, 0, - 0, 0, 0, 1)) + // Inject static transforms: map -> odom -> base_footprint -> base_link + var tMapOdom = TF3API.CreateStaticTransform("map", "odom", 0, 0, 0, 0, 0, 0, 1); + var tOdomFoot = TF3API.CreateStaticTransform("odom", "base_footprint", 0, 0, 0, 0, 0, 0, 1); + var tFootLink = TF3API.CreateStaticTransform("base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1); + if (!TF3API.tf3_set_transform(tf3Buffer, ref tMapOdom, "NavigationExample", true) || + !TF3API.tf3_set_transform(tf3Buffer, ref tOdomFoot, "NavigationExample", true) || + !TF3API.tf3_set_transform(tf3Buffer, ref tFootLink, "NavigationExample", true)) { - LogError("Failed to inject static TF map -> odom"); - NavigationAPI.tf_listener_destroy(tfHandle); + LogError("Failed to set static TF"); + TF3API.tf3_buffer_destroy(tf3Buffer); return; } - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_footprint", - 0, 0, 0, - 0, 0, 0, 1)) - { - LogError("Failed to inject static TF map -> base_link"); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "base_footprint", "base_link", - 0, 0, 0, - 0, 0, 0, 1)) - { - LogError("Failed to inject static TF map -> base_link"); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - // Create navigation instance + // Create navigation instance and initialize with tf3 buffer NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create(); if (navHandle.ptr == IntPtr.Zero) { LogError("Failed to create navigation instance"); - NavigationAPI.tf_listener_destroy(tfHandle); + TF3API.tf3_buffer_destroy(tf3Buffer); return; } - // Initialize navigation (passes TF to move_base; navigation keeps its own copy, so tfHandle can be destroyed later) - if (!NavigationAPI.navigation_initialize(navHandle, tfHandle)) + if (!NavigationAPI.navigation_initialize(navHandle, tf3Buffer)) { - LogError("Failed to initialize navigation (check native log for 'Invalid TF listener' or 'tf is nullptr')"); + LogError("Failed to initialize navigation with tf3 buffer"); NavigationAPI.navigation_destroy(navHandle); - NavigationAPI.tf_listener_destroy(tfHandle); + TF3API.tf3_buffer_destroy(tf3Buffer); return; } - while(true) + while (true) { - // Get navigation feedback NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) { - if(feedback.is_ready) + if (feedback.is_ready) { Console.WriteLine("Navigation is ready"); break; @@ -571,22 +197,23 @@ namespace NavigationExample } System.Threading.Thread.Sleep(100); } - Console.WriteLine("[NavigationExample] Navigation initialized with TF successfully"); + Console.WriteLine("[NavigationExample] Navigation initialized successfully"); + // Set robot footprint - NavigationAPI.Point[] footprint = new NavigationAPI.Point[] + Point[] footprint = new Point[] { - new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 }, - new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 }, - new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 }, - new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 } + new Point { x = 0.3, y = -0.2, z = 0.0 }, + new Point { x = 0.3, y = 0.2, z = 0.0 }, + new Point { x = -0.3, y = 0.2, z = 0.0 }, + new Point { x = -0.3, y = -0.2, z = 0.0 } }; NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan"); - NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId)); - NavigationAPI.LaserScan fscanHandle; + Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId)); + LaserScan fscanHandle; fscanHandle.header = fscanHeader; fscanHandle.angle_min = -1.57f; fscanHandle.angle_max = 1.57f; @@ -604,8 +231,8 @@ namespace NavigationExample NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan", fscanHandle); IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan"); - NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId)); - NavigationAPI.LaserScan bscanHandle; + Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId)); + LaserScan bscanHandle; bscanHandle.header = bscanHeader; bscanHandle.angle_min = 1.57f; bscanHandle.angle_max = -1.57f; @@ -623,8 +250,8 @@ namespace NavigationExample NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan", bscanHandle); IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom"); - NavigationAPI.Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId)); - NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry(); + Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId)); + Odometry odometryHandle = new Odometry(); odometryHandle.header = odometryHeader; IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint"); odometryHandle.child_frame_id = childFrameId; @@ -695,13 +322,13 @@ namespace NavigationExample Console.WriteLine("maze.yaml not found, using default map 3x10"); } - NavigationAPI.Time mapLoadTime = NavigationAPI.time_create(); - NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData(); + Time mapLoadTime = NavigationAPI.time_create(); + MapMetaData mapMetaData = new MapMetaData(); mapMetaData.map_load_time = mapLoadTime; mapMetaData.resolution = mapConfig.Resolution; mapMetaData.width = (uint)mapWidth; mapMetaData.height = (uint)mapHeight; - mapMetaData.origin = new NavigationAPI.Pose(); + mapMetaData.origin = new Pose(); mapMetaData.origin.position.x = mapConfig.OriginX; mapMetaData.origin.position.y = mapConfig.OriginY; mapMetaData.origin.position.z = mapConfig.OriginZ; @@ -709,7 +336,7 @@ namespace NavigationExample mapMetaData.origin.orientation.y = 0.0; mapMetaData.origin.orientation.z = 0.0; mapMetaData.origin.orientation.w = 1.0; - NavigationAPI.OccupancyGrid occupancyGrid = new NavigationAPI.OccupancyGrid(); + OccupancyGrid occupancyGrid = new OccupancyGrid(); IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map"); occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId)); occupancyGrid.info = mapMetaData; @@ -718,26 +345,148 @@ namespace NavigationExample occupancyGrid.data_count = new UIntPtr((uint)data.Length); Console.WriteLine("data length: {0} {1}", data.Length, occupancyGrid.data_count); Console.WriteLine("C# OccupancyGrid sizeof={0} data_off={1} data_count_off={2}", - Marshal.SizeOf(), - Marshal.OffsetOf("data"), - Marshal.OffsetOf("data_count")); + Marshal.SizeOf(), + Marshal.OffsetOf("data"), + Marshal.OffsetOf("data_count")); NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid); - NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped(); + Twist2DStamped twist = new Twist2DStamped(); if (NavigationAPI.navigation_get_twist(navHandle, ref twist)) { Console.WriteLine( "Twist: {0}, {1}, {2}, {3}", NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta); - } - // Cleanup - NavigationAPI.navigation_move_straight_to(navHandle, 1.0); + } + + // // Build order (thao cách bom order): header + nodes + edges giống C++ + // Order order = new Order(); + // order.headerId = 1; + // order.timestamp = Marshal.StringToHGlobalAnsi("2026-02-28 10:00:00"); + // order.version = Marshal.StringToHGlobalAnsi("1.0.0"); + // order.manufacturer = Marshal.StringToHGlobalAnsi("Manufacturer"); + // order.serialNumber = Marshal.StringToHGlobalAnsi("Serial Number"); + // order.orderId = Marshal.StringToHGlobalAnsi("Order ID"); + // order.orderUpdateId = 1; + + // // Nodes: giống for (auto node : order.nodes) { node_msg.nodeId = ...; node_msg.nodePosition.x = ...; order_msg.nodes.push_back(node_msg); } + // int nodeCount = 1; + // order.nodes = Marshal.AllocHGlobal(Marshal.SizeOf() * nodeCount); + // order.nodes_count = new UIntPtr((uint)nodeCount); + // Node node1 = new Node(); + // node1.nodeId = Marshal.StringToHGlobalAnsi("node-1"); + // node1.sequenceId = 0; + // node1.nodeDescription = Marshal.StringToHGlobalAnsi("Goal node"); + // node1.released = 0; + // node1.nodePosition.x = 1.0; + // node1.nodePosition.y = 1.0; + // node1.nodePosition.theta = 0.0; + // node1.nodePosition.allowedDeviationXY = 0.1f; + // node1.nodePosition.allowedDeviationTheta = 0.05f; + // node1.nodePosition.mapId = Marshal.StringToHGlobalAnsi("map"); + // node1.nodePosition.mapDescription = Marshal.StringToHGlobalAnsi(""); + // node1.actions = IntPtr.Zero; + // node1.actions_count = UIntPtr.Zero; + // Marshal.StructureToPtr(node1, order.nodes, false); + + // // Edges: rỗng trong ví dụ này; nếu cần thì alloc và fill tương tự (edge_msg.edgeId, trajectory.controlPoints, ...) + // order.edges = IntPtr.Zero; + // order.edges_count = UIntPtr.Zero; + // order.zoneSetId = Marshal.StringToHGlobalAnsi(""); + + // PoseStamped goal = new PoseStamped(); + // goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId)); + // goal.pose.position.x = 1.0; + // goal.pose.position.y = 1.0; + // goal.pose.position.z = 0.0; + // goal.pose.orientation.x = 0.0; + // goal.pose.orientation.y = 0.0; + // goal.pose.orientation.z = 0.0; + // goal.pose.orientation.w = 1.0; + + // NavigationAPI.navigation_move_to_order(navHandle, order, goal); + + NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0); + NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2); + + // NavigationAPI.navigation_move_straight_to(navHandle, 1.0); + + // while (true) + // { + // System.Threading.Thread.Sleep(100); + // NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); + // if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) + // { + // if (feedback.navigation_state == NavigationAPI.NavigationState.Succeeded) + // { + // Console.WriteLine("Navigation is Succeeded"); + // break; + // } + // } + + // NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput(); + // if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData)) + // { + // int n = (int)(uint)globalData.plan.poses_count; + // int poseSize = Marshal.SizeOf(); + // for (int i = 0; i < n; i++) + // { + // IntPtr posePtr = IntPtr.Add(globalData.plan.poses, i * poseSize); + // Pose2DStamped p = Marshal.PtrToStructure(posePtr); + // double p_x = p.pose.x; + // double p_y = p.pose.y; + // double p_theta = p.pose.theta; + // Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta); + // } + // if(globalData.is_costmap_updated) { + // for(int i = 0; i < (int)(uint)globalData.costmap.data_count; i++) { + // byte cellValue = Marshal.ReadByte(globalData.costmap.data, i); + // Console.WriteLine("Costmap: {0} {1}", i, cellValue); + // } + // } + // else { + // Console.WriteLine("Global Costmap is not updated"); + // } + // } + + // NavigationAPI.PlannerDataOutput localData = new NavigationAPI.PlannerDataOutput(); + // if(NavigationAPI.navigation_get_local_data(navHandle, ref localData)) + // { + // int n = (int)(uint)localData.plan.poses_count; + // int poseSize = Marshal.SizeOf(); + // for (int i = 0; i < n; i++) + // { + // IntPtr posePtr = IntPtr.Add(localData.plan.poses, i * poseSize); + // Pose2DStamped p = Marshal.PtrToStructure(posePtr); + // double p_x = p.pose.x; + // double p_y = p.pose.y; + // double p_theta = p.pose.theta; + // Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta); + // } + // if(localData.is_costmap_updated) { + // for(int i = 0; i < (int)(uint)localData.costmap.data_count; i++) { + // byte cellValue = Marshal.ReadByte(localData.costmap.data, i); + // Console.WriteLine("Costmap: {0} {1}", i, cellValue); + // } + // } + // else { + // Console.WriteLine("Local Costmap is not updated"); + // } + // } + // } + // Cleanup (destroy nav first, then tf3 buffer) NavigationAPI.navigation_destroy(navHandle); - NavigationAPI.tf_listener_destroy(tfHandle); + TF3API.tf3_buffer_destroy(tf3Buffer); Console.WriteLine("Press any key to exit..."); - Console.ReadKey(); + try + { + Console.ReadKey(intercept: true); + } + catch (InvalidOperationException) + { + // Running without a real console (e.g. redirected/automated run). + } } } } diff --git a/examples/NavigationExample/TF3API.cs b/examples/NavigationExample/TF3API.cs new file mode 100644 index 0000000..c059fa4 --- /dev/null +++ b/examples/NavigationExample/TF3API.cs @@ -0,0 +1,133 @@ +using System; +using System.Runtime.InteropServices; + +namespace NavigationExample +{ + + // ============================================================================ + // TF3 C API - P/Invoke wrapper for libtf3 (tf3 BufferCore) + // ============================================================================ + public static class TF3API + { + private const string Tf3DllName = "/usr/local/lib/libtf3.so"; // Linux; Windows: tf3.dll, macOS: libtf3.dylib + + public enum TF3_ErrorCode + { + TF3_OK = 0, + TF3_ERROR_LOOKUP = 1, + TF3_ERROR_CONNECTIVITY = 2, + TF3_ERROR_EXTRAPOLATION = 3, + TF3_ERROR_INVALID_ARGUMENT = 4, + TF3_ERROR_TIMEOUT = 5, + TF3_ERROR_UNKNOWN = 99 + } + + [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)] + public struct TF3_Transform + { + public long timestamp_sec; + public long timestamp_nsec; + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)] + public string frame_id; + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)] + public string child_frame_id; + public double translation_x; + public double translation_y; + public double translation_z; + public double rotation_x; + public double rotation_y; + public double rotation_z; + public double rotation_w; + } + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr tf3_buffer_create(int cache_time_sec); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void tf3_buffer_destroy(IntPtr buffer); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_set_transform( + IntPtr buffer, + ref TF3_Transform transform, + string authority, + [MarshalAs(UnmanagedType.I1)] bool is_static); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_lookup_transform( + IntPtr buffer, + string target_frame, + string source_frame, + long time_sec, + long time_nsec, + out TF3_Transform transform, + out TF3_ErrorCode error_code); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_lookup_transform_full( + IntPtr buffer, + string target_frame, + long target_time_sec, + long target_time_nsec, + string source_frame, + long source_time_sec, + long source_time_nsec, + string fixed_frame, + out TF3_Transform transform, + out TF3_ErrorCode error_code); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_can_transform( + IntPtr buffer, + string target_frame, + string source_frame, + long time_sec, + long time_nsec, + System.Text.StringBuilder error_msg, + int error_msg_len); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern int tf3_get_all_frame_names(IntPtr buffer, System.Text.StringBuilder frames, int frames_len); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_get_frame_tree(IntPtr buffer, System.Text.StringBuilder output, int output_len); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void tf3_clear(IntPtr buffer); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void tf3_get_current_time(out long sec, out long nsec); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_get_last_error(IntPtr buffer, System.Text.StringBuilder error_msg, int error_msg_len); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr tf3_get_version(); + + /// Helper: create TF3_Transform for static transform (identity or given pose). + public static TF3_Transform CreateStaticTransform(string parentFrame, string childFrame, + double tx = 0, double ty = 0, double tz = 0, + double qx = 0, double qy = 0, double qz = 0, double qw = 1) + { + var t = new TF3_Transform(); + t.timestamp_sec = 0; + t.timestamp_nsec = 0; + t.frame_id = parentFrame ?? ""; + t.child_frame_id = childFrame ?? ""; + t.translation_x = tx; + t.translation_y = ty; + t.translation_z = tz; + t.rotation_x = qx; + t.rotation_y = qy; + t.rotation_z = qz; + t.rotation_w = qw; + return t; + } + } +} \ No newline at end of file diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 8830579..fc78ee2 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/examples/NavigationExample/msgs/CommonMsgs.cs b/examples/NavigationExample/msgs/CommonMsgs.cs new file mode 100644 index 0000000..00c1311 --- /dev/null +++ b/examples/NavigationExample/msgs/CommonMsgs.cs @@ -0,0 +1,274 @@ +using System; +using System.Runtime.InteropServices; + +namespace NavigationExample +{ + // ============================================================================ + // Structures + // ============================================================================ + [StructLayout(LayoutKind.Sequential)] + public struct Point + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose2D + { + public double x; + public double y; + public double theta; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Twist2D + { + public double x; + public double y; + public double theta; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public double x; + public double y; + public double z; + public double w; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Position + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose + { + public Point position; + public Quaternion orientation; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Twist + { + public Vector3 linear; + public Vector3 angular; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Header + { + public uint seq; + public uint sec; + public uint nsec; + public IntPtr frame_id; // char* + } + + [StructLayout(LayoutKind.Sequential)] + public struct PoseStamped + { + public Header header; + public Pose pose; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Twist2DStamped + { + public Header header; + public Twist2D velocity; + } + + [StructLayout(LayoutKind.Sequential)] + public struct LaserScan + { + public Header header; + public float angle_min; + public float angle_max; + public float angle_increment; + public float time_increment; + public float scan_time; + public float range_min; + public float range_max; + public IntPtr ranges; + public UIntPtr ranges_count; + public IntPtr intensities; + public UIntPtr intensities_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct PointCloud + { + public Header header; + public IntPtr points; + public UIntPtr points_count; + public IntPtr channels; + public UIntPtr channels_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct PointCloud2 + { + public Header header; + public uint height; + public uint width; + public IntPtr fields; + public UIntPtr fields_count; + [MarshalAs(UnmanagedType.I1)] + public bool is_bigendian; + public uint point_step; + public uint row_step; + public IntPtr data; + public UIntPtr data_count; + [MarshalAs(UnmanagedType.I1)] + public bool is_dense; + } + + [StructLayout(LayoutKind.Sequential)] + public struct PoseWithCovariance + { + public Pose pose; + public IntPtr covariance; + public UIntPtr covariance_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct TwistWithCovariance { + public Twist twist; + public IntPtr covariance; + public UIntPtr covariance_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Odometry + { + public Header header; + public IntPtr child_frame_id; + public PoseWithCovariance pose; + public TwistWithCovariance twist; + } + + [StructLayout(LayoutKind.Sequential)] + public struct OccupancyGrid + { + public Header header; + public MapMetaData info; + public IntPtr data; + public UIntPtr data_count; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct MapMetaData + { + public Time map_load_time; + public float resolution; + public uint width; + public uint height; + public Pose origin; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Time + { + public uint sec; + public uint nsec; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Point32 + { + public float x; + public float y; + public float z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose2DStamped + { + public Header header; + public Pose2D pose; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Polygon + { + public IntPtr points; + public UIntPtr points_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct PolygonStamped + { + public Header header; + public Polygon polygon; + } + + [StructLayout(LayoutKind.Sequential)] + public struct OccupancyGridUpdate + { + public Header header; + public int x; + public int y; + public uint width; + public uint height; + public IntPtr data; + public UIntPtr data_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Path2D + { + public Header header; + public IntPtr poses; + public UIntPtr poses_count; + } + + /// Name (char*) + OccupancyGrid for static map list API. + [StructLayout(LayoutKind.Sequential)] + public struct NamedOccupancyGrid + { + public IntPtr name; + public OccupancyGrid grid; + } + + /// Name (char*) + LaserScan for laser scan list API. + [StructLayout(LayoutKind.Sequential)] + public struct NamedLaserScan + { + public IntPtr name; + public LaserScan scan; + } + + /// Name (char*) + PointCloud for point cloud list API. + [StructLayout(LayoutKind.Sequential)] + public struct NamedPointCloud + { + public IntPtr name; + public PointCloud cloud; + } + + /// Name (char*) + PointCloud2 for point cloud2 list API. + [StructLayout(LayoutKind.Sequential)] + public struct NamedPointCloud2 + { + public IntPtr name; + public PointCloud2 cloud; + } + + +} \ No newline at end of file diff --git a/examples/NavigationExample/msgs/ProtocolMsgsTypes.cs b/examples/NavigationExample/msgs/ProtocolMsgsTypes.cs new file mode 100644 index 0000000..4ff3bec --- /dev/null +++ b/examples/NavigationExample/msgs/ProtocolMsgsTypes.cs @@ -0,0 +1,109 @@ +using System; +using System.Runtime.InteropServices; + +namespace NavigationExample +{ + /// + /// C# struct layout cho protocol_msgs (robot_protocol_msgs C API). + /// Khớp với pnkx_nav_core/src/APIs/c_api/include/protocol_msgs/*.h + /// + [StructLayout(LayoutKind.Sequential)] + public struct ControlPoint + { + public double x; + public double y; + public double weight; + } + + [StructLayout(LayoutKind.Sequential)] + public struct ActionParameter + { + public IntPtr key; // char* + public IntPtr value; // char* + } + + [StructLayout(LayoutKind.Sequential)] + public struct NodePosition + { + public double x; + public double y; + public double theta; + public float allowedDeviationXY; + public float allowedDeviationTheta; + public IntPtr mapId; // char* + public IntPtr mapDescription; // char* + } + + [StructLayout(LayoutKind.Sequential)] + public struct Trajectory + { + public uint degree; + public IntPtr knotVector; // double* + public UIntPtr knotVector_count; + public IntPtr controlPoints; // ControlPoint* + public UIntPtr controlPoints_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Action + { + public IntPtr actionType; // char* + public IntPtr actionId; // char* + public IntPtr actionDescription;// char* + public IntPtr blockingType; // char* + public IntPtr actionParameters; // ActionParameter* + public UIntPtr actionParameters_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Node + { + public IntPtr nodeId; // char* + public int sequenceId; + public IntPtr nodeDescription; // char* + public byte released; + public NodePosition nodePosition; + public IntPtr actions; // Action* + public UIntPtr actions_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Edge + { + public IntPtr edgeId; // char* + public int sequenceId; + public IntPtr edgeDescription; // char* + public byte released; + public IntPtr startNodeId; // char* + public IntPtr endNodeId; // char* + public double maxSpeed; + public double maxHeight; + public double minHeight; + public double orientation; + public IntPtr orientationType; // char* + public IntPtr direction; // char* + public byte rotationAllowed; + public double maxRotationSpeed; + public Trajectory trajectory; + public double length; + public IntPtr actions; // Action* + public UIntPtr actions_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Order + { + public int headerId; + public IntPtr timestamp; // char* + public IntPtr version; // char* + public IntPtr manufacturer; // char* + public IntPtr serialNumber; // char* + public IntPtr orderId; // char* + public uint orderUpdateId; + public IntPtr nodes; // Node* + public UIntPtr nodes_count; + public IntPtr edges; // Edge* + public UIntPtr edges_count; + public IntPtr zoneSetId; // char* + } +} diff --git a/examples/NavigationExample/msgs/SensorMsgs.cs b/examples/NavigationExample/msgs/SensorMsgs.cs new file mode 100644 index 0000000..e69de29 diff --git a/pnkx_nav_core.sln b/pnkx_nav_core.sln new file mode 100644 index 0000000..76b51c8 --- /dev/null +++ b/pnkx_nav_core.sln @@ -0,0 +1,29 @@ +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.5.2.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "examples", "examples", "{B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "NavigationExample", "examples\NavigationExample\NavigationExample.csproj", "{995839D6-1E72-F444-6587-97EF24F93814}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.Build.0 = Debug|Any CPU + {995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.ActiveCfg = Release|Any CPU + {995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(NestedProjects) = preSolution + {995839D6-1E72-F444-6587-97EF24F93814} = {B36A84DF-456D-A817-6EDD-3EC3E7F6E11F} + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {1A56EED1-5E6A-48A6-9AEE-F39361B34305} + EndGlobalSection +EndGlobal diff --git a/robotapp.db b/robotapp.db new file mode 100644 index 0000000..e69de29 diff --git a/src/APIs/c_api/CMakeLists.txt b/src/APIs/c_api/CMakeLists.txt index efeb0da..a83d80c 100644 --- a/src/APIs/c_api/CMakeLists.txt +++ b/src/APIs/c_api/CMakeLists.txt @@ -21,6 +21,9 @@ set(PACKAGES_DIR robot_time robot_cpp geometry_msgs + angles + data_convert + robot_protocol_msgs ) # Thư mục include diff --git a/src/APIs/c_api/include/convertor.h b/src/APIs/c_api/include/convertor.h index 1be3c7c..6322a2c 100644 --- a/src/APIs/c_api/include/convertor.h +++ b/src/APIs/c_api/include/convertor.h @@ -13,6 +13,10 @@ #include "geometry_msgs/Pose2D.h" #include "nav_2d_msgs/Twist2D.h" #include "nav_2d_msgs/Twist2DStamped.h" +#include "nav_2d_msgs/Path2D.h" +#include "map_msgs/OccupancyGridUpdate.h" +#include "geometry_msgs/PolygonStamped.h" +#include "protocol_msgs/Order.h" // C++ #include @@ -24,6 +28,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -49,6 +57,21 @@ robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occu */ void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out); +/** + * @brief Convert C++ Path2D to C Path2D + */ +void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out); + +/** + * @brief Convert C++ OccupancyGridUpdate to C OccupancyGridUpdate + */ +void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out); + +/** + * @brief Convert C++ PolygonStamped to C PolygonStamped + */ +void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out); + /** * @brief Convert C++ LaserScan to C LaserScan * @param cpp C++ LaserScan @@ -150,4 +173,27 @@ robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped */ Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped); +/** + * @brief Convert C Order to C++ Order + * @param order C Order + * @return C++ Order + */ +robot_protocol_msgs::Order convert2CppOrder(const Order& order); + +/** + * @brief Convert C++ Order to C Order + * @param cpp_order C++ Order + * @return C Order (caller must call order_free when done to release memory) + */ +Order convert2COrder(const robot_protocol_msgs::Order& cpp_order); + +/** + * @brief Free dynamic memory held by an Order returned from convert2COrder + * @param order Order to free (pointers and counts are zeroed) + */ +#ifdef __cplusplus +extern "C" +#endif +void order_free(Order* order); + #endif // C_API_CONVERTOR_H \ No newline at end of file diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index 72f8548..1df99b7 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/src/APIs/c_api/include/nav_c_api.h @@ -24,22 +24,13 @@ extern "C" #include "sensor_msgs/PointCloud.h" #include "sensor_msgs/PointCloud2.h" #include "geometry_msgs/PolygonStamped.h" + #include "protocol_msgs/Order.h" typedef struct { char *name; OccupancyGrid grid; } NamedOccupancyGrid; typedef struct { char *name; LaserScan scan; } NamedLaserScan; typedef struct { char *name; PointCloud cloud; } NamedPointCloud; typedef struct { char *name; PointCloud2 cloud; } NamedPointCloud2; - - typedef struct - { - void *ptr; - } NavigationHandle; - - // typedef struct - // { - // void *ptr; - // } TFListenerHandle; - + typedef void* NavigationHandle; typedef void* TFListenerHandle; typedef enum @@ -100,53 +91,18 @@ extern "C" */ void navigation_destroy(NavigationHandle handle); - /** - * @brief Create a TF listener instance - * @return TF listener handle, or NULL on failure - */ - TFListenerHandle tf_listener_create(void); - - /** - * @brief Destroy a TF listener instance - * @param handle TF listener handle to destroy - */ - void tf_listener_destroy(TFListenerHandle handle); - - /** - * @brief Inject a static transform into the TF buffer. - * - * This is a convenience for standalone usage where no external TF publisher exists yet. - * It will create/ensure the frames exist and become transformable. - * - * @param tf_handle TF listener handle - * @param parent_frame Parent frame id (e.g. "map") - * @param child_frame Child frame id (e.g. "base_link") - * @param x Translation x (meters) - * @param y Translation y (meters) - * @param z Translation z (meters) - * @param qx Rotation quaternion x - * @param qy Rotation quaternion y - * @param qz Rotation quaternion z - * @param qw Rotation quaternion w - * @return true on success, false on failure - */ - bool tf_listener_set_static_transform(TFListenerHandle tf_handle, - const char *parent_frame, - const char *child_frame, - double x, double y, double z, - double qx, double qy, double qz, double qw); - // ============================================================================ // Navigation Interface Methods // ============================================================================ /** - * @brief Initialize the navigation system + * @brief Initialize the navigation system using an existing tf3 buffer (from libtf3 tf3_buffer_create). + * Caller retains ownership of the buffer and must call tf3_buffer_destroy when done (after navigation_destroy). * @param handle Navigation handle - * @param tf_handle TF listener handle + * @param tf3_buffer Pointer to tf3 BufferCore (TF3_BufferCore from libtf3) * @return true on success, false on failure */ - bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle); + bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf3_buffer); /** * @brief Set the robot's footprint (outline shape) @@ -171,51 +127,37 @@ extern "C" * @brief Send a goal for the robot to navigate to * @param handle Navigation handle * @param goal Target pose in the global frame - * @param xy_goal_tolerance Acceptable error in X/Y (meters) - * @param yaw_goal_tolerance Acceptable angular error (radians) * @return true if goal was accepted and sent successfully */ - bool navigation_move_to(NavigationHandle handle, const PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + bool navigation_move_to(NavigationHandle handle, const PoseStamped goal); - // /** - // * @brief Send a goal for the robot to navigate to - // * @param handle Navigation handle - // * @param order Order message - // * @param goal Target pose in the global frame - // * @param xy_goal_tolerance Acceptable error in X/Y (meters) - // * @param yaw_goal_tolerance Acceptable angular error (radians) - // * @return true if goal was accepted and sent successfully - // */ - // bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order, - // const PoseStamped &goal, - // double xy_goal_tolerance, double yaw_goal_tolerance); + /** + * @brief Send a goal for the robot to navigate to + * @param handle Navigation handle + * @param order Order message + * @param goal Target pose in the global frame + * @return true if goal was accepted and sent successfully + * @note If order was obtained from convert2COrder(), call order_free(&order) when done + */ + bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal); /** * @brief Send a docking goal to a predefined marker * @param handle Navigation handle * @param marker Marker name or ID (null-terminated string) * @param goal Target pose for docking - * @param xy_goal_tolerance Acceptable XY error (meters) - * @param yaw_goal_tolerance Acceptable heading error (radians) * @return true if docking command succeeded */ - bool navigation_dock_to(NavigationHandle handle, const char *marker, - const PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal); - // /** - // * @brief Send a docking goal to a predefined marker - // * @param handle Navigation handle - // * @param order Order message - // * @param goal Target pose for docking - // * @param xy_goal_tolerance Acceptable XY error (meters) - // * @param yaw_goal_tolerance Acceptable heading error (radians) - // * @return true if docking command succeeded - // */ - // bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order, - // const PoseStamped &goal, - // double xy_goal_tolerance, double yaw_goal_tolerance); + /** + * @brief Send a docking goal to a predefined marker + * @param handle Navigation handle + * @param order Order message + * @param goal Target pose for docking + * @return true if docking command succeeded + */ + bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal); /** * @brief Move straight toward the target position @@ -228,12 +170,10 @@ extern "C" /** * @brief Rotate in place to align with target orientation * @param handle Navigation handle - * @param goal Pose containing desired heading (only Z-axis used) - * @param yaw_goal_tolerance Acceptable angular error (radians) + * @param goal_yaw Desired heading (radians) * @return true if rotation command was sent successfully */ - bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal, - double yaw_goal_tolerance); + bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal); /** * @brief Pause the robot's movement diff --git a/src/APIs/c_api/include/protocol_msgs/Action.h b/src/APIs/c_api/include/protocol_msgs/Action.h new file mode 100644 index 0000000..08d2650 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Action.h @@ -0,0 +1,30 @@ +#ifndef C_API_PROTOCOL_MSGS_ACTION_H +#define C_API_PROTOCOL_MSGS_ACTION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/ActionParameter.h" + +#define PROTOCOL_MSGS_ACTION_BLOCKING_NONE "NONE" +#define PROTOCOL_MSGS_ACTION_BLOCKING_SOFT "SOFT" +#define PROTOCOL_MSGS_ACTION_BLOCKING_HARD "HARD" + +typedef struct +{ + char *actionType; + char *actionId; + char *actionDescription; + char *blockingType; + ActionParameter *actionParameters; + size_t actionParameters_count; +} Action; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ACTION_H diff --git a/src/APIs/c_api/include/protocol_msgs/ActionParameter.h b/src/APIs/c_api/include/protocol_msgs/ActionParameter.h new file mode 100644 index 0000000..423ed79 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/ActionParameter.h @@ -0,0 +1,21 @@ +#ifndef C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H +#define C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *key; + char *value; +} ActionParameter; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H diff --git a/src/APIs/c_api/include/protocol_msgs/ControlPoint.h b/src/APIs/c_api/include/protocol_msgs/ControlPoint.h new file mode 100644 index 0000000..17ec8dd --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/ControlPoint.h @@ -0,0 +1,22 @@ +#ifndef C_API_PROTOCOL_MSGS_CONTROLPOINT_H +#define C_API_PROTOCOL_MSGS_CONTROLPOINT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double weight; +} ControlPoint; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_CONTROLPOINT_H diff --git a/src/APIs/c_api/include/protocol_msgs/Edge.h b/src/APIs/c_api/include/protocol_msgs/Edge.h new file mode 100644 index 0000000..aeb791c --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Edge.h @@ -0,0 +1,42 @@ +#ifndef C_API_PROTOCOL_MSGS_EDGE_H +#define C_API_PROTOCOL_MSGS_EDGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/Trajectory.h" +#include "protocol_msgs/Action.h" + +#define PROTOCOL_MSGS_EDGE_ORIENTATION_GLOBAL "GLOBAL" +#define PROTOCOL_MSGS_EDGE_ORIENTATION_TANGENTIAL "TANGENTIAL" + +typedef struct +{ + char *edgeId; + int32_t sequenceId; + char *edgeDescription; + uint8_t released; + char *startNodeId; + char *endNodeId; + double maxSpeed; + double maxHeight; + double minHeight; + double orientation; + char *orientationType; + char *direction; + uint8_t rotationAllowed; + double maxRotationSpeed; + Trajectory trajectory; + double length; + Action *actions; + size_t actions_count; +} Edge; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_EDGE_H diff --git a/src/APIs/c_api/include/protocol_msgs/Error.h b/src/APIs/c_api/include/protocol_msgs/Error.h new file mode 100644 index 0000000..2a4b8f5 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Error.h @@ -0,0 +1,28 @@ +#ifndef C_API_PROTOCOL_MSGS_ERROR_H +#define C_API_PROTOCOL_MSGS_ERROR_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/ErrorReference.h" + +#define PROTOCOL_MSGS_ERROR_LEVEL_WARNING "WARNING" +#define PROTOCOL_MSGS_ERROR_LEVEL_FATAL "FATAL" + +typedef struct +{ + char *errorType; + ErrorReference *errorReferences; + size_t errorReferences_count; + char *errorDescription; + char *errorLevel; +} Error; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ERROR_H diff --git a/src/APIs/c_api/include/protocol_msgs/ErrorReference.h b/src/APIs/c_api/include/protocol_msgs/ErrorReference.h new file mode 100644 index 0000000..5bb3d78 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/ErrorReference.h @@ -0,0 +1,21 @@ +#ifndef C_API_PROTOCOL_MSGS_ERRORREFERENCE_H +#define C_API_PROTOCOL_MSGS_ERRORREFERENCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *referenceKey; + char *referenceValue; +} ErrorReference; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ERRORREFERENCE_H diff --git a/src/APIs/c_api/include/protocol_msgs/Info.h b/src/APIs/c_api/include/protocol_msgs/Info.h new file mode 100644 index 0000000..2657bd3 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Info.h @@ -0,0 +1,28 @@ +#ifndef C_API_PROTOCOL_MSGS_INFO_H +#define C_API_PROTOCOL_MSGS_INFO_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/InfoReference.h" + +#define PROTOCOL_MSGS_INFO_LEVEL_DEBUG "DEBUG" +#define PROTOCOL_MSGS_INFO_LEVEL_INFO "INFO" + +typedef struct +{ + char *infoType; + InfoReference *infoReferences; + size_t infoReferences_count; + char *infoDescription; + char *infoLevel; +} Info; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_INFO_H diff --git a/src/APIs/c_api/include/protocol_msgs/InfoReference.h b/src/APIs/c_api/include/protocol_msgs/InfoReference.h new file mode 100644 index 0000000..d6cd08b --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/InfoReference.h @@ -0,0 +1,21 @@ +#ifndef C_API_PROTOCOL_MSGS_INFOREFERENCE_H +#define C_API_PROTOCOL_MSGS_INFOREFERENCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *referenceKey; + char *referenceValue; +} InfoReference; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_INFOREFERENCE_H diff --git a/src/APIs/c_api/include/protocol_msgs/Information.h b/src/APIs/c_api/include/protocol_msgs/Information.h new file mode 100644 index 0000000..d0e9f4f --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Information.h @@ -0,0 +1,22 @@ +#ifndef C_API_PROTOCOL_MSGS_INFORMATION_H +#define C_API_PROTOCOL_MSGS_INFORMATION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/Info.h" + +typedef struct +{ + Info *information; + size_t information_count; +} Information; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_INFORMATION_H diff --git a/src/APIs/c_api/include/protocol_msgs/Node.h b/src/APIs/c_api/include/protocol_msgs/Node.h new file mode 100644 index 0000000..cede8ba --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Node.h @@ -0,0 +1,28 @@ +#ifndef C_API_PROTOCOL_MSGS_NODE_H +#define C_API_PROTOCOL_MSGS_NODE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/NodePosition.h" +#include "protocol_msgs/Action.h" + +typedef struct +{ + char *nodeId; + int32_t sequenceId; + char *nodeDescription; + uint8_t released; + NodePosition nodePosition; + Action *actions; + size_t actions_count; +} Node; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_NODE_H diff --git a/src/APIs/c_api/include/protocol_msgs/NodePosition.h b/src/APIs/c_api/include/protocol_msgs/NodePosition.h new file mode 100644 index 0000000..986c8b2 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/NodePosition.h @@ -0,0 +1,26 @@ +#ifndef C_API_PROTOCOL_MSGS_NODEPOSITION_H +#define C_API_PROTOCOL_MSGS_NODEPOSITION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double theta; + float allowedDeviationXY; + float allowedDeviationTheta; + char *mapId; + char *mapDescription; +} NodePosition; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_NODEPOSITION_H diff --git a/src/APIs/c_api/include/protocol_msgs/Order.h b/src/APIs/c_api/include/protocol_msgs/Order.h new file mode 100644 index 0000000..af45fbb --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Order.h @@ -0,0 +1,33 @@ +#ifndef C_API_PROTOCOL_MSGS_ORDER_H +#define C_API_PROTOCOL_MSGS_ORDER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/Node.h" +#include "protocol_msgs/Edge.h" + +typedef struct +{ + int32_t headerId; + char *timestamp; + char *version; + char *manufacturer; + char *serialNumber; + char *orderId; + uint32_t orderUpdateId; + Node *nodes; + size_t nodes_count; + Edge *edges; + size_t edges_count; + char *zoneSetId; +} Order; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ORDER_H diff --git a/src/APIs/c_api/include/protocol_msgs/Trajectory.h b/src/APIs/c_api/include/protocol_msgs/Trajectory.h new file mode 100644 index 0000000..bc1ec5b --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Trajectory.h @@ -0,0 +1,25 @@ +#ifndef C_API_PROTOCOL_MSGS_TRAJECTORY_H +#define C_API_PROTOCOL_MSGS_TRAJECTORY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/ControlPoint.h" + +typedef struct +{ + uint32_t degree; + double *knotVector; + size_t knotVector_count; + ControlPoint *controlPoints; + size_t controlPoints_count; +} Trajectory; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_TRAJECTORY_H diff --git a/src/APIs/c_api/src/convertor.cpp b/src/APIs/c_api/src/convertor.cpp index 216f950..4e3e76d 100644 --- a/src/APIs/c_api/src/convertor.cpp +++ b/src/APIs/c_api/src/convertor.cpp @@ -201,6 +201,70 @@ void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyG } } +static void convert2CPose2DStamped(const robot_nav_2d_msgs::Pose2DStamped& cpp, Pose2DStamped& out) +{ + out.header = convert2CHeader(cpp.header); + out.pose = convert2CPose2D(cpp.pose); +} + +void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out) +{ + out.header = convert2CHeader(cpp.header); + out.poses_count = cpp.poses.size(); + out.poses = nullptr; + if (out.poses_count > 0) + { + out.poses = static_cast(malloc(out.poses_count * sizeof(Pose2DStamped))); + if (out.poses) + { + for (size_t i = 0; i < out.poses_count; i++) + convert2CPose2DStamped(cpp.poses[i], out.poses[i]); + } + } +} + +void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out) +{ + out.header = convert2CHeader(cpp.header); + out.x = cpp.x; + out.y = cpp.y; + out.width = cpp.width; + out.height = cpp.height; + out.data_count = cpp.data.size(); + out.data = nullptr; + if (out.data_count > 0) + { + out.data = static_cast(malloc(out.data_count * sizeof(int8_t))); + if (out.data) + memcpy(out.data, cpp.data.data(), out.data_count * sizeof(int8_t)); + } +} + +static void convert2CPolygon(const robot_geometry_msgs::Polygon& cpp, Polygon& out) +{ + out.points_count = cpp.points.size(); + out.points = nullptr; + if (out.points_count > 0) + { + out.points = static_cast(malloc(out.points_count * sizeof(Point32))); + if (out.points) + { + for (size_t i = 0; i < out.points_count; i++) + { + out.points[i].x = cpp.points[i].x; + out.points[i].y = cpp.points[i].y; + out.points[i].z = cpp.points[i].z; + } + } + } +} + +void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out) +{ + out.header = convert2CHeader(cpp.header); + convert2CPolygon(cpp.polygon, out.polygon); +} + void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out) { out.header.seq = cpp.header.seq; @@ -361,3 +425,374 @@ robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c) memcpy(cpp.data.data(), c.data, c.data_count); return cpp; } + +// --- protocol_msgs helpers (C <-> C++ robot_protocol_msgs) --- + +static inline char* dup_cstr(const char* s) +{ + if (!s) return nullptr; + const size_t n = std::strlen(s) + 1; + char* p = static_cast(std::malloc(n)); + if (p) std::memcpy(p, s, n); + return p; +} + +static robot_protocol_msgs::ActionParameter convert2CppActionParameter(const ActionParameter& c) +{ + robot_protocol_msgs::ActionParameter cpp; + cpp.key = c.key ? c.key : ""; + cpp.value = c.value ? c.value : ""; + return cpp; +} + +static ActionParameter convert2CActionParameter(const robot_protocol_msgs::ActionParameter& cpp) +{ + ActionParameter c; + c.key = dup_cstr(cpp.key.c_str()); + c.value = dup_cstr(cpp.value.c_str()); + return c; +} + +static robot_protocol_msgs::ControlPoint convert2CppControlPoint(const ControlPoint& c) +{ + robot_protocol_msgs::ControlPoint cpp; + cpp.x = c.x; + cpp.y = c.y; + cpp.weight = c.weight; + return cpp; +} + +static ControlPoint convert2CControlPoint(const robot_protocol_msgs::ControlPoint& cpp) +{ + ControlPoint c; + c.x = cpp.x; + c.y = cpp.y; + c.weight = cpp.weight; + return c; +} + +static robot_protocol_msgs::NodePosition convert2CppNodePosition(const NodePosition& c) +{ + robot_protocol_msgs::NodePosition cpp; + cpp.x = c.x; + cpp.y = c.y; + cpp.theta = c.theta; + cpp.allowedDeviationXY = c.allowedDeviationXY; + cpp.allowedDeviationTheta = c.allowedDeviationTheta; + cpp.mapId = c.mapId ? c.mapId : ""; + cpp.mapDescription = c.mapDescription ? c.mapDescription : ""; + return cpp; +} + +static NodePosition convert2CNodePosition(const robot_protocol_msgs::NodePosition& cpp) +{ + NodePosition c; + c.x = cpp.x; + c.y = cpp.y; + c.theta = cpp.theta; + c.allowedDeviationXY = cpp.allowedDeviationXY; + c.allowedDeviationTheta = cpp.allowedDeviationTheta; + c.mapId = dup_cstr(cpp.mapId.c_str()); + c.mapDescription = dup_cstr(cpp.mapDescription.c_str()); + return c; +} + +static robot_protocol_msgs::Trajectory convert2CppTrajectory(const Trajectory& c) +{ + robot_protocol_msgs::Trajectory cpp; + cpp.degree = c.degree; + const size_t kv = c.knotVector ? c.knotVector_count : 0; + cpp.knotVector.resize(kv); + for (size_t i = 0; i < kv; i++) + cpp.knotVector[i] = c.knotVector[i]; + const size_t ncp = c.controlPoints ? c.controlPoints_count : 0; + cpp.controlPoints.resize(ncp); + for (size_t i = 0; i < ncp; i++) + cpp.controlPoints[i] = convert2CppControlPoint(c.controlPoints[i]); + return cpp; +} + +static Trajectory convert2CTrajectory(const robot_protocol_msgs::Trajectory& cpp) +{ + Trajectory c; + c.degree = cpp.degree; + c.knotVector_count = cpp.knotVector.size(); + c.knotVector = c.knotVector_count ? static_cast(std::malloc(c.knotVector_count * sizeof(double))) : nullptr; + if (c.knotVector) + for (size_t i = 0; i < c.knotVector_count; i++) + c.knotVector[i] = cpp.knotVector[i]; + c.controlPoints_count = cpp.controlPoints.size(); + c.controlPoints = c.controlPoints_count ? static_cast(std::malloc(c.controlPoints_count * sizeof(ControlPoint))) : nullptr; + if (c.controlPoints) + for (size_t i = 0; i < c.controlPoints_count; i++) + c.controlPoints[i] = convert2CControlPoint(cpp.controlPoints[i]); + return c; +} + +static robot_protocol_msgs::Action convert2CppAction(const Action& c) +{ + robot_protocol_msgs::Action cpp; + cpp.actionType = c.actionType ? c.actionType : ""; + cpp.actionId = c.actionId ? c.actionId : ""; + cpp.actionDescription = c.actionDescription ? c.actionDescription : ""; + cpp.blockingType = c.blockingType ? c.blockingType : ""; + const size_t n = c.actionParameters ? c.actionParameters_count : 0; + cpp.actionParameters.resize(n); + for (size_t i = 0; i < n; i++) + cpp.actionParameters[i] = convert2CppActionParameter(c.actionParameters[i]); + return cpp; +} + +static Action convert2CAction(const robot_protocol_msgs::Action& cpp) +{ + Action c; + c.actionType = dup_cstr(cpp.actionType.c_str()); + c.actionId = dup_cstr(cpp.actionId.c_str()); + c.actionDescription = dup_cstr(cpp.actionDescription.c_str()); + c.blockingType = dup_cstr(cpp.blockingType.c_str()); + c.actionParameters_count = cpp.actionParameters.size(); + c.actionParameters = c.actionParameters_count ? static_cast(std::malloc(c.actionParameters_count * sizeof(ActionParameter))) : nullptr; + if (c.actionParameters) + for (size_t i = 0; i < c.actionParameters_count; i++) + c.actionParameters[i] = convert2CActionParameter(cpp.actionParameters[i]); + return c; +} + +static robot_protocol_msgs::Node convert2CppNode(const Node& c) +{ + robot_protocol_msgs::Node cpp; + cpp.nodeId = c.nodeId ? c.nodeId : ""; + cpp.sequenceId = c.sequenceId; + cpp.nodeDescription = c.nodeDescription ? c.nodeDescription : ""; + cpp.released = c.released; + cpp.nodePosition = convert2CppNodePosition(c.nodePosition); + const size_t n = c.actions ? c.actions_count : 0; + cpp.actions.resize(n); + for (size_t i = 0; i < n; i++) + cpp.actions[i] = convert2CppAction(c.actions[i]); + return cpp; +} + +static Node convert2CNode(const robot_protocol_msgs::Node& cpp) +{ + Node c; + c.nodeId = dup_cstr(cpp.nodeId.c_str()); + c.sequenceId = cpp.sequenceId; + c.nodeDescription = dup_cstr(cpp.nodeDescription.c_str()); + c.released = cpp.released; + c.nodePosition = convert2CNodePosition(cpp.nodePosition); + c.actions_count = cpp.actions.size(); + c.actions = c.actions_count ? static_cast(std::malloc(c.actions_count * sizeof(Action))) : nullptr; + if (c.actions) + for (size_t i = 0; i < c.actions_count; i++) + c.actions[i] = convert2CAction(cpp.actions[i]); + return c; +} + +static robot_protocol_msgs::Edge convert2CppEdge(const Edge& c) +{ + robot_protocol_msgs::Edge cpp; + cpp.edgeId = c.edgeId ? c.edgeId : ""; + cpp.sequenceId = c.sequenceId; + cpp.edgeDescription = c.edgeDescription ? c.edgeDescription : ""; + cpp.released = c.released; + cpp.startNodeId = c.startNodeId ? c.startNodeId : ""; + cpp.endNodeId = c.endNodeId ? c.endNodeId : ""; + cpp.maxSpeed = c.maxSpeed; + cpp.maxHeight = c.maxHeight; + cpp.minHeight = c.minHeight; + cpp.orientation = c.orientation; + cpp.orientationType = c.orientationType ? c.orientationType : ""; + cpp.direction = c.direction ? c.direction : ""; + cpp.rotationAllowed = c.rotationAllowed; + cpp.maxRotationSpeed = c.maxRotationSpeed; + cpp.trajectory = convert2CppTrajectory(c.trajectory); + cpp.length = c.length; + const size_t n = c.actions ? c.actions_count : 0; + cpp.actions.resize(n); + for (size_t i = 0; i < n; i++) + cpp.actions[i] = convert2CppAction(c.actions[i]); + return cpp; +} + +static Edge convert2CEdge(const robot_protocol_msgs::Edge& cpp) +{ + Edge c; + c.edgeId = dup_cstr(cpp.edgeId.c_str()); + c.sequenceId = cpp.sequenceId; + c.edgeDescription = dup_cstr(cpp.edgeDescription.c_str()); + c.released = cpp.released; + c.startNodeId = dup_cstr(cpp.startNodeId.c_str()); + c.endNodeId = dup_cstr(cpp.endNodeId.c_str()); + c.maxSpeed = cpp.maxSpeed; + c.maxHeight = cpp.maxHeight; + c.minHeight = cpp.minHeight; + c.orientation = cpp.orientation; + c.orientationType = dup_cstr(cpp.orientationType.c_str()); + c.direction = dup_cstr(cpp.direction.c_str()); + c.rotationAllowed = cpp.rotationAllowed; + c.maxRotationSpeed = cpp.maxRotationSpeed; + c.trajectory = convert2CTrajectory(cpp.trajectory); + c.length = cpp.length; + c.actions_count = cpp.actions.size(); + c.actions = c.actions_count ? static_cast(std::malloc(c.actions_count * sizeof(Action))) : nullptr; + if (c.actions) + for (size_t i = 0; i < c.actions_count; i++) + c.actions[i] = convert2CAction(cpp.actions[i]); + return c; +} + +static void free_action_parameter(ActionParameter* p) +{ + if (!p) return; + std::free(p->key); + std::free(p->value); + p->key = nullptr; + p->value = nullptr; +} + +static void free_action(Action* p) +{ + if (!p) return; + std::free(p->actionType); + std::free(p->actionId); + std::free(p->actionDescription); + std::free(p->blockingType); + p->actionType = p->actionId = p->actionDescription = p->blockingType = nullptr; + if (p->actionParameters) { + for (size_t i = 0; i < p->actionParameters_count; i++) + free_action_parameter(&p->actionParameters[i]); + std::free(p->actionParameters); + p->actionParameters = nullptr; + } + p->actionParameters_count = 0; +} + +static void free_node_position(NodePosition* p) +{ + if (!p) return; + std::free(p->mapId); + std::free(p->mapDescription); + p->mapId = p->mapDescription = nullptr; +} + +static void free_trajectory(Trajectory* p) +{ + if (!p) return; + std::free(p->knotVector); + p->knotVector = nullptr; + p->knotVector_count = 0; + std::free(p->controlPoints); + p->controlPoints = nullptr; + p->controlPoints_count = 0; +} + +static void free_node(Node* p) +{ + if (!p) return; + std::free(p->nodeId); + std::free(p->nodeDescription); + p->nodeId = p->nodeDescription = nullptr; + free_node_position(&p->nodePosition); + if (p->actions) { + for (size_t i = 0; i < p->actions_count; i++) + free_action(&p->actions[i]); + std::free(p->actions); + p->actions = nullptr; + } + p->actions_count = 0; +} + +static void free_edge(Edge* p) +{ + if (!p) return; + std::free(p->edgeId); + std::free(p->edgeDescription); + std::free(p->startNodeId); + std::free(p->endNodeId); + std::free(p->orientationType); + std::free(p->direction); + p->edgeId = p->edgeDescription = p->startNodeId = p->endNodeId = nullptr; + p->orientationType = p->direction = nullptr; + free_trajectory(&p->trajectory); + if (p->actions) { + for (size_t i = 0; i < p->actions_count; i++) + free_action(&p->actions[i]); + std::free(p->actions); + p->actions = nullptr; + } + p->actions_count = 0; +} + +robot_protocol_msgs::Order convert2CppOrder(const Order& order) +{ + robot_protocol_msgs::Order cpp_order; + cpp_order.headerId = order.headerId; + cpp_order.timestamp = order.timestamp ? order.timestamp : ""; + cpp_order.version = order.version ? order.version : ""; + cpp_order.manufacturer = order.manufacturer ? order.manufacturer : ""; + cpp_order.serialNumber = order.serialNumber ? order.serialNumber : ""; + cpp_order.orderId = order.orderId ? order.orderId : ""; + cpp_order.orderUpdateId = order.orderUpdateId; + const size_t nn = order.nodes ? order.nodes_count : 0; + cpp_order.nodes.resize(nn); + for (size_t i = 0; i < nn; i++) + cpp_order.nodes[i] = convert2CppNode(order.nodes[i]); + const size_t ne = order.edges ? order.edges_count : 0; + cpp_order.edges.resize(ne); + for (size_t i = 0; i < ne; i++) + cpp_order.edges[i] = convert2CppEdge(order.edges[i]); + cpp_order.zoneSetId = order.zoneSetId ? order.zoneSetId : ""; + return cpp_order; +} + +Order convert2COrder(const robot_protocol_msgs::Order& cpp_order) +{ + Order order; + order.headerId = cpp_order.headerId; + order.timestamp = dup_cstr(cpp_order.timestamp.c_str()); + order.version = dup_cstr(cpp_order.version.c_str()); + order.manufacturer = dup_cstr(cpp_order.manufacturer.c_str()); + order.serialNumber = dup_cstr(cpp_order.serialNumber.c_str()); + order.orderId = dup_cstr(cpp_order.orderId.c_str()); + order.orderUpdateId = cpp_order.orderUpdateId; + order.nodes_count = cpp_order.nodes.size(); + order.nodes = order.nodes_count ? static_cast(std::malloc(order.nodes_count * sizeof(Node))) : nullptr; + if (order.nodes) + for (size_t i = 0; i < order.nodes_count; i++) + order.nodes[i] = convert2CNode(cpp_order.nodes[i]); + order.edges_count = cpp_order.edges.size(); + order.edges = order.edges_count ? static_cast(std::malloc(order.edges_count * sizeof(Edge))) : nullptr; + if (order.edges) + for (size_t i = 0; i < order.edges_count; i++) + order.edges[i] = convert2CEdge(cpp_order.edges[i]); + order.zoneSetId = dup_cstr(cpp_order.zoneSetId.c_str()); + return order; +} + +extern "C" void order_free(Order* order) +{ + if (!order) return; + std::free(order->timestamp); + std::free(order->version); + std::free(order->manufacturer); + std::free(order->serialNumber); + std::free(order->orderId); + std::free(order->zoneSetId); + order->timestamp = order->version = order->manufacturer = nullptr; + order->serialNumber = order->orderId = order->zoneSetId = nullptr; + if (order->nodes) { + for (size_t i = 0; i < order->nodes_count; i++) + free_node(&order->nodes[i]); + std::free(order->nodes); + order->nodes = nullptr; + } + order->nodes_count = 0; + if (order->edges) { + for (size_t i = 0; i < order->edges_count; i++) + free_edge(&order->edges[i]); + std::free(order->edges); + order->edges = nullptr; + } + order->edges_count = 0; +} \ No newline at end of file diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 9611e7b..fa212cf 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -1,6 +1,8 @@ #include #include "nav_c_api.h" #include "convertor.h" +#include +#include #include #include #include @@ -11,6 +13,14 @@ #include #include #include +#include +#include + +// Keep navigation instances alive even when returning raw pointers as C handles. +// Key is the raw BaseNavigation* (used as NavigationHandle), value owns the instance. +static std::mutex g_nav_instances_mutex; +static std::unordered_map>g_nav_instances; static NavFeedback convert2CNavFeedback(const robot::move_base_core::NavFeedback &cpp) { @@ -38,10 +48,6 @@ extern "C" void nav_c_api_free_string(char *str) free(str); } -// ============================================================================ -// Helper Functions -// ============================================================================ - extern "C" bool navigation_offset_goal_2d(const Pose2D &in_pose, const char *frame_id, double offset_distance, PoseStamped &out_goal) @@ -66,75 +72,9 @@ extern "C" bool navigation_offset_goal_stamped(const PoseStamped &in_pose, doubl return true; } -// ============================================================================ -// TF Listener Management -// ============================================================================ - -extern "C" TFListenerHandle tf_listener_create(void) -{ - try - { - auto tf = std::make_shared(); - return TFListenerHandle{new std::shared_ptr(tf)}; - } - catch (...) - { - return TFListenerHandle{nullptr}; - } -} - -extern "C" void tf_listener_destroy(TFListenerHandle handle) -{ - if (handle) - { - auto *tf_ptr = static_cast *>(handle); - delete tf_ptr; - } -} - -extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle, - const char *parent_frame, - const char *child_frame, - double x, double y, double z, - double qx, double qy, double qz, double qw) -{ - if (!tf_handle || !parent_frame || !child_frame) - { - return false; - } - - try - { - auto *tf_ptr = static_cast *>(tf_handle); - if (!tf_ptr || !(*tf_ptr)) - { - return false; - } - - tf3::TransformStampedMsg st; - st.header.stamp = tf3::Time::now(); - st.header.frame_id = parent_frame; - st.child_frame_id = child_frame; - st.transform.translation.x = x; - st.transform.translation.y = y; - st.transform.translation.z = z; - st.transform.rotation.x = qx; - st.transform.rotation.y = qy; - st.transform.rotation.z = qz; - st.transform.rotation.w = qw; - - return (*tf_ptr)->setTransform(st, "nav_c_api(static)", true /*is_static*/); - } - catch (...) - { - return false; - } -} - // ============================================================================ // Navigation Handle Management // ============================================================================ - extern "C" NavigationHandle navigation_create(void) { try @@ -146,27 +86,37 @@ extern "C" NavigationHandle navigation_create(void) auto move_base_loader = boost::dll::import_alias( path_file_so, "MoveBase", boost::dll::load_mode::append_decorations); auto move_base_ptr = move_base_loader(); - // Store shared_ptr in handle (same pattern as TFListenerHandle) - return NavigationHandle{new std::shared_ptr(move_base_ptr)}; + if (!move_base_ptr) + return nullptr; + + auto *raw = move_base_ptr.get(); + { + std::lock_guard lock(g_nav_instances_mutex); + g_nav_instances[raw] = std::move(move_base_ptr); + } + + // Return raw pointer as opaque C handle. + return static_cast(raw); } catch (const std::exception &e) { // Log error if possible (in production, use proper logging) - return NavigationHandle{nullptr}; + return nullptr; } catch (...) { - return NavigationHandle{nullptr}; + return nullptr; } } extern "C" void navigation_destroy(NavigationHandle handle) { - if (handle.ptr) - { - auto *nav_ptr = static_cast *>(handle.ptr); - delete nav_ptr; - } + if (!handle) + return; + + auto *raw = static_cast(handle); + std::lock_guard lock(g_nav_instances_mutex); + g_nav_instances.erase(raw); } // ============================================================================ @@ -175,26 +125,29 @@ extern "C" void navigation_destroy(NavigationHandle handle) extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) { - printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); - if (!handle.ptr || !tf_handle) + if (!handle || !tf_handle) { printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__); return false; } try { - printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + printf("[%s:%d]: Initialize navigation\n", __FILE__, __LINE__); + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto *tf_ptr = static_cast *>(tf_handle); - if (!tf_ptr || !(*tf_ptr)) + + std::shared_ptr tf_ptr = + std::shared_ptr(reinterpret_cast(tf_handle), [](::tf3::BufferCore *) {}); + if (!tf_ptr) { - printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); + printf("[%s:%d]\n Error: Invalid TF listener", __FILE__, __LINE__); return false; } - nav->initialize(*tf_ptr); + nav_ptr->initialize(tf_ptr); return true; } @@ -205,24 +158,25 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle } catch (...) { - printf("[%s:%d]\n Error: Failed to initialize navigation\n", __FILE__, __LINE__); return false; } } extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point *points, size_t point_count) { - if (!handle.ptr || !points || point_count == 0) + if (!handle || !points || point_count == 0) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + std::vector footprint; footprint.reserve(point_count); for (size_t i = 0; i < point_count; ++i) @@ -233,7 +187,7 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po pt.z = points[i].z; footprint.push_back(pt); } - nav->setRobotFootprint(footprint); + nav_ptr->setRobotFootprint(footprint); return true; } catch (...) @@ -244,18 +198,19 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - std::vector footprint = nav->getRobotFootprint(); + std::vector footprint = nav_ptr->getRobotFootprint(); if (footprint.empty()) { return false; @@ -276,22 +231,23 @@ extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point *o } } -extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance) +extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped goal) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); - return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + return nav_ptr->moveTo(cpp_goal); } catch (...) { @@ -299,39 +255,52 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped go } } -// extern "C" bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order, -// const PoseStamped& goal, -// double xy_goal_tolerance, double yaw_goal_tolerance) { -// if (!handle.ptr || !order.ptr || !goal) { -// return false; -// } -// try { -// auto* nav = static_cast(handle.ptr); -// auto* order_ptr = static_cast(order.ptr); -// robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); -// return nav->moveTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); -// } catch (...) { -// return false; -// } -// } - -extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, - const PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance) +extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal) { - if (!handle.ptr || !marker) + if (!handle) + { + return false; + } + try + { + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) + return false; + robot::log_error("navigation_move_to_order goal %f %f", goal.pose.position.x, goal.pose.position.y); + robot_protocol_msgs::Order cpp_order = convert2CppOrder(order); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav_ptr->moveTo(cpp_order, cpp_goal); + } + catch (const std::exception &e) + { + return false; + } + catch (...) + { + printf("[%s:%d]\n Error: Failed to move to order\n", __FILE__, __LINE__); + return false; + } +} + +extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal) +{ + if (!handle || !marker) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); - return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + return nav_ptr->dockTo(std::string(marker), cpp_goal); } catch (...) { @@ -339,39 +308,59 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, } } -// extern "C" bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order, -// const PoseStamped& goal, -// double xy_goal_tolerance, double yaw_goal_tolerance) { -// if (!handle.ptr || !order.ptr || !goal) { -// return false; -// } -// try { -// auto* nav = static_cast(handle.ptr); -// auto* order_ptr = static_cast(order.ptr); -// robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); -// return nav->dockTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); -// } catch (...) { -// return false; -// } +extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal) +{ + if (!handle) + { + return false; + } + try + { + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) + return false; + robot_protocol_msgs::Order cpp_order = convert2CppOrder(order); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav_ptr->dockTo(cpp_order, std::string(marker), cpp_goal); + } + catch (const std::exception &e) + { + return false; + } + catch (...) + { + printf("[%s:%d]\n Error: Failed to dock to order\n", __FILE__, __LINE__); + return false; + } +} extern "C" bool navigation_move_straight_to(NavigationHandle handle, const double distance) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::PoseStamped pose; - if (!nav->getRobotPose(pose)) + if (!nav_ptr->getRobotPose(pose)) return false; + robot_geometry_msgs::Pose2D pose2d; + pose2d.x = pose.pose.position.x; + pose2d.y = pose.pose.position.y; + pose2d.theta = data_convert::getYaw(pose.pose.orientation); + robot::log_error("pose2d: %f %f %f", pose2d.x, pose2d.y, pose2d.theta); robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance); - return nav->moveStraightTo(goal); + return nav_ptr->moveStraightTo(goal); } catch (...) { @@ -379,21 +368,22 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const doubl } } -extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal, double yaw_goal_tolerance) +extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); - return nav->rotateTo(cpp_goal, yaw_goal_tolerance); + return nav_ptr->rotateTo(cpp_goal); } catch (...) { @@ -403,13 +393,16 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped extern "C" void navigation_pause(NavigationHandle handle) { - if (handle.ptr) + if (handle) { try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (nav_ptr && *nav_ptr) - nav_ptr->get()->pause(); + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) + return; + nav_ptr->pause(); } catch (...) { @@ -420,13 +413,15 @@ extern "C" void navigation_pause(NavigationHandle handle) extern "C" void navigation_resume(NavigationHandle handle) { - if (handle.ptr) + if (handle) { try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (nav_ptr && *nav_ptr) - nav_ptr->get()->resume(); + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (nav_ptr) + nav_ptr->resume(); } catch (...) { @@ -437,13 +432,15 @@ extern "C" void navigation_resume(NavigationHandle handle) extern "C" void navigation_cancel(NavigationHandle handle) { - if (handle.ptr) + if (handle) { try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (nav_ptr && *nav_ptr) - nav_ptr->get()->cancel(); + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (nav_ptr) + nav_ptr->cancel(); } catch (...) { @@ -455,22 +452,24 @@ extern "C" void navigation_cancel(NavigationHandle handle) extern "C" bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::Vector3 linear; linear.x = linear_x; linear.y = linear_y; linear.z = linear_z; - return nav->setTwistLinear(linear); + return nav_ptr->setTwistLinear(linear); } catch (...) { @@ -481,22 +480,24 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle, extern "C" bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::Vector3 angular; angular.x = angular_x; angular.y = angular_y; angular.z = angular_z; - return nav->setTwistAngular(angular); + return nav_ptr->setTwistAngular(angular); } catch (...) { @@ -506,19 +507,21 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle, extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::PoseStamped cpp_pose; - if (nav->getRobotPose(cpp_pose)) + if (nav_ptr->getRobotPose(cpp_pose)) { out_pose = convert2CPoseStamped(cpp_pose); return true; @@ -533,19 +536,21 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::Pose2D cpp_pose; - if (nav->getRobotPose(cpp_pose)) + if (nav_ptr->getRobotPose(cpp_pose)) { out_pose = convert2CPose2D(cpp_pose); return true; @@ -560,18 +565,20 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &ou extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist(); + + robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav_ptr->getTwist(); out_twist = convert2CTwist2DStamped(cpp_twist); return true; } @@ -583,18 +590,20 @@ extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &ou extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot::move_base_core::NavFeedback *fb = nav->getFeedback(); + + robot::move_base_core::NavFeedback *fb = nav_ptr->getFeedback(); if (fb == nullptr) return false; out_feedback = convert2CNavFeedback(*fb); @@ -612,20 +621,22 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback &ou extern "C" bool navigation_add_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid occupancy_grid) { - if (!handle.ptr || !map_name) + if (!handle || !map_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot::log_info("occupancy_grid.data_count: %d", occupancy_grid.data_count); robot_nav_msgs::OccupancyGrid robot_occupancy_grid = convert2CppOccupancyGrid(occupancy_grid); - nav->addStaticMap(std::string(map_name), robot_occupancy_grid); + nav_ptr->addStaticMap(std::string(map_name), robot_occupancy_grid); return true; } catch (...) @@ -636,19 +647,20 @@ extern "C" bool navigation_add_static_map(NavigationHandle handle, const char *m extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan laser_scan) { - if (!handle.ptr || !laser_scan_name) + if (!handle || !laser_scan_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); robot_sensor_msgs::LaserScan robot_laser_scan = convert2CppLaserScan(laser_scan); - nav->addLaserScan(std::string(laser_scan_name), robot_laser_scan); + nav_ptr->addLaserScan(std::string(laser_scan_name), robot_laser_scan); return true; } catch (...) @@ -659,18 +671,20 @@ extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char *l extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud) { - if (!handle.ptr || !point_cloud_name) + if (!handle || !point_cloud_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud cpp_cloud = convert2CppPointCloud(point_cloud); - nav->addPointCloud(std::string(point_cloud_name), cpp_cloud); + nav_ptr->addPointCloud(std::string(point_cloud_name), cpp_cloud); return true; } catch (...) @@ -681,18 +695,20 @@ extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char * extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2) { - if (!handle.ptr || !point_cloud2_name) + if (!handle || !point_cloud2_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud2 cpp_cloud = convert2CppPointCloud2(point_cloud2); - nav->addPointCloud2(std::string(point_cloud2_name), cpp_cloud); + nav_ptr->addPointCloud2(std::string(point_cloud2_name), cpp_cloud); return true; } catch (...) @@ -703,20 +719,22 @@ extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char extern "C" bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry) { - if (!handle.ptr || !odometry_name) + if (!handle || !odometry_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) { return false; } - auto *nav = nav_ptr->get(); + robot_nav_msgs::Odometry robot_odometry = convert2CppOdometry(odometry); - nav->addOdometry(std::string(odometry_name), robot_odometry); + nav_ptr->addOdometry(std::string(odometry_name), robot_odometry); return true; } catch (...) @@ -727,17 +745,19 @@ extern "C" bool navigation_add_odometry(NavigationHandle handle, const char *odo extern "C" bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map) { - if (!handle.ptr || !map_name) + if (!handle || !map_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_nav_msgs::OccupancyGrid robot_map = nav->getStaticMap(std::string(map_name)); + + robot_nav_msgs::OccupancyGrid robot_map = nav_ptr->getStaticMap(std::string(map_name)); convert2COccupancyGrid(robot_map, out_map); return true; } @@ -749,17 +769,19 @@ extern "C" bool navigation_get_static_map(NavigationHandle handle, const char *m extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan) { - if (!handle.ptr || !laser_scan_name) + if (!handle || !laser_scan_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_sensor_msgs::LaserScan robot_scan = nav->getLaserScan(std::string(laser_scan_name)); + + robot_sensor_msgs::LaserScan robot_scan = nav_ptr->getLaserScan(std::string(laser_scan_name)); convert2CLaserScan(robot_scan, out_scan); return true; } @@ -771,17 +793,19 @@ extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char *l extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud) { - if (!handle.ptr || !point_cloud_name) + if (!handle || !point_cloud_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_sensor_msgs::PointCloud robot_cloud = nav->getPointCloud(std::string(point_cloud_name)); + + robot_sensor_msgs::PointCloud robot_cloud = nav_ptr->getPointCloud(std::string(point_cloud_name)); convert2CPointCloud(robot_cloud, out_cloud); return true; } @@ -793,17 +817,19 @@ extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char * extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud) { - if (!handle.ptr || !point_cloud2_name) + if (!handle || !point_cloud2_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_sensor_msgs::PointCloud2 robot_cloud = nav->getPointCloud2(std::string(point_cloud2_name)); + + robot_sensor_msgs::PointCloud2 robot_cloud = nav_ptr->getPointCloud2(std::string(point_cloud2_name)); convert2CPointCloud2(robot_cloud, out_cloud); return true; } @@ -815,17 +841,19 @@ extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count) { - if (!handle.ptr || !out_maps) + if (!handle || !out_maps) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto maps = nav->getAllStaticMaps(); + + auto maps = nav_ptr->getAllStaticMaps(); out_count = maps.size(); size_t i = 0; for (const auto &p : maps) @@ -844,17 +872,19 @@ extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOcc extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count) { - if (!handle.ptr || !out_scans) + if (!handle || !out_scans) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto scans = nav->getAllLaserScans(); + + auto scans = nav_ptr->getAllLaserScans(); if (scans.empty()) { out_count = 0; @@ -878,17 +908,19 @@ extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLas extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count) { - if (!handle.ptr || !out_clouds) + if (!handle || !out_clouds) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto clouds = nav->getAllPointClouds(); + + auto clouds = nav_ptr->getAllPointClouds(); if (clouds.empty()) { out_count = 0; @@ -912,17 +944,19 @@ extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPo extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count) { - if (!handle.ptr || !out_clouds) + if (!handle || !out_clouds) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto clouds = nav->getAllPointCloud2s(); + + auto clouds = nav_ptr->getAllPointCloud2s(); if (clouds.empty()) { out_count = 0; @@ -946,16 +980,18 @@ extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedP extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char *map_name) { - if (!handle.ptr || !map_name) + if (!handle || !map_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeStaticMap(std::string(map_name)); + return nav_ptr->removeStaticMap(std::string(map_name)); } catch (...) { @@ -965,16 +1001,18 @@ extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char *laser_scan_name) { - if (!handle.ptr || !laser_scan_name) + if (!handle || !laser_scan_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeLaserScan(std::string(laser_scan_name)); + return nav_ptr->removeLaserScan(std::string(laser_scan_name)); } catch (...) { @@ -984,16 +1022,18 @@ extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const char *point_cloud_name) { - if (!handle.ptr || !point_cloud_name) + if (!handle || !point_cloud_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removePointCloud(std::string(point_cloud_name)); + return nav_ptr->removePointCloud(std::string(point_cloud_name)); } catch (...) { @@ -1003,16 +1043,18 @@ extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const cha extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const char *point_cloud2_name) { - if (!handle.ptr || !point_cloud2_name) + if (!handle || !point_cloud2_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removePointCloud2(std::string(point_cloud2_name)); + return nav_ptr->removePointCloud2(std::string(point_cloud2_name)); } catch (...) { @@ -1022,16 +1064,18 @@ extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const ch extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllStaticMaps(); + return nav_ptr->removeAllStaticMaps(); } catch (...) { @@ -1041,16 +1085,18 @@ extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllLaserScans(); + return nav_ptr->removeAllLaserScans(); } catch (...) { @@ -1060,16 +1106,18 @@ extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllPointClouds(); + return nav_ptr->removeAllPointClouds(); } catch (...) { @@ -1079,16 +1127,18 @@ extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllPointCloud2s(); + return nav_ptr->removeAllPointCloud2s(); } catch (...) { @@ -1098,16 +1148,18 @@ extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) extern "C" bool navigation_remove_all_data(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllData(); + return nav_ptr->removeAllData(); } catch (...) { @@ -1116,35 +1168,85 @@ extern "C" bool navigation_remove_all_data(NavigationHandle handle) } // ============================================================================ -// Planner Data (disabled - PlannerDataOutput and related types not in public API) +// Planner Data // ============================================================================ -#if 0 +static void clear_planner_data(PlannerDataOutput *out) +{ + if (!out) + return; + // Free Path2D plan + if (out->plan.poses) + { + for (size_t i = 0; i < out->plan.poses_count; i++) + { + if (out->plan.poses[i].header.frame_id) + free(out->plan.poses[i].header.frame_id); + } + free(out->plan.poses); + out->plan.poses = nullptr; + out->plan.poses_count = 0; + } + if (out->plan.header.frame_id) + { + free(out->plan.header.frame_id); + out->plan.header.frame_id = nullptr; + } + // Free OccupancyGrid costmap + if (out->costmap.header.frame_id) + { + free(out->costmap.header.frame_id); + out->costmap.header.frame_id = nullptr; + } + if (out->costmap.data) + { + free(out->costmap.data); + out->costmap.data = nullptr; + } + // Free OccupancyGridUpdate + if (out->costmap_update.header.frame_id) + { + free(out->costmap_update.header.frame_id); + out->costmap_update.header.frame_id = nullptr; + } + if (out->costmap_update.data) + { + free(out->costmap_update.data); + out->costmap_update.data = nullptr; + } + // Free PolygonStamped footprint + if (out->footprint.header.frame_id) + { + free(out->footprint.header.frame_id); + out->footprint.header.frame_id = nullptr; + } + if (out->footprint.polygon.points) + { + free(out->footprint.polygon.points); + out->footprint.polygon.points = nullptr; + out->footprint.polygon.points_count = 0; + } +} + extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput *out_data) { - if (!handle.ptr || !out_data) - { + if (!handle || !out_data) return false; - } clear_planner_data(out_data); try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), + [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot::move_base_core::PlannerDataOutput data = nav->getGlobalData(); - out_data->plan = Path2DHandle{clone_message(data.plan)}; - out_data->costmap = OccupancyGridHandle{clone_message(data.costmap)}; - out_data->costmap_update = OccupancyGridUpdateHandle{clone_message(data.costmap_update)}; - out_data->footprint = PolygonStampedHandle{clone_message(data.footprint)}; - out_data->is_costmap_updated = data.is_costmap_updated; - if (!out_data->plan.ptr || !out_data->costmap.ptr || !out_data->costmap_update.ptr || - !out_data->footprint.ptr) - { - clear_planner_data(out_data); - return false; - } + robot::move_base_core::PlannerDataOutput data = nav_ptr->getGlobalData(); + convert2CPath2D(data.plan, out_data->plan); + convert2COccupancyGrid(data.costmap, out_data->costmap); + convert2COccupancyGridUpdate(data.costmap_update, out_data->costmap_update); + convert2CPolygonStamped(data.footprint, out_data->footprint); + out_data->is_costmap_updated = data.is_costmap_updated; return true; } catch (...) @@ -1156,30 +1258,24 @@ extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataO extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput *out_data) { - if (!handle.ptr || !out_data) - { + if (!handle || !out_data) return false; - } clear_planner_data(out_data); try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), + [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot::move_base_core::PlannerDataOutput data = nav->getLocalData(); - out_data->plan = Path2DHandle{clone_message(data.plan)}; - out_data->costmap = convert2CppOccupancyGrid(data.costmap); - out_data->costmap_update = OccupancyGridUpdateHandle{clone_message(data.costmap_update)}; - out_data->footprint = convert2CppPolygonStamped(data.footprint); - out_data->is_costmap_updated = data.is_costmap_updated; - if (!out_data->plan.ptr || !out_data->costmap.ptr || !out_data->costmap_update.ptr || - !out_data->footprint.ptr) - { - clear_planner_data(out_data); - return false; - } + robot::move_base_core::PlannerDataOutput data = nav_ptr->getLocalData(); + convert2CPath2D(data.plan, out_data->plan); + convert2COccupancyGrid(data.costmap, out_data->costmap); + convert2COccupancyGridUpdate(data.costmap_update, out_data->costmap_update); + convert2CPolygonStamped(data.footprint, out_data->footprint); + out_data->is_costmap_updated = data.is_costmap_updated; return true; } catch (...) @@ -1188,4 +1284,4 @@ extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOu return false; } } -#endif + diff --git a/src/Algorithms/Libraries/kalman/src/kalman.cpp b/src/Algorithms/Libraries/kalman/src/kalman.cpp index 30c5345..4c44312 100755 --- a/src/Algorithms/Libraries/kalman/src/kalman.cpp +++ b/src/Algorithms/Libraries/kalman/src/kalman.cpp @@ -24,7 +24,15 @@ KalmanFilter::KalmanFilter( I.setIdentity(); } -KalmanFilter::KalmanFilter() {} +KalmanFilter::KalmanFilter() + : m(0), + n(0), + t0(0.0), + t(0.0), + dt(0.0), + initialized(false) +{ +} void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) { x_hat = x0; diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h index 423a082..88d3dba 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h @@ -15,9 +15,9 @@ namespace mkt_algorithm class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory { public: - GoStraight() {}; + GoStraight(); - virtual ~GoStraight() {}; + virtual ~GoStraight(); /** * @brief Initialize parameters as needed diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index b7951bc..bab5fc9 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -30,9 +30,14 @@ namespace mkt_algorithm class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm { public: - PredictiveTrajectory() : initialized_(false), nav_stop_(false), - near_goal_heading_integral_(0.0), near_goal_heading_last_error_(0.0), near_goal_heading_was_active_(false) {}; + /** + * @brief Constructor + */ + PredictiveTrajectory(); + /** + * @brief Destructor + */ virtual ~PredictiveTrajectory(); // Standard ScoreAlgorithm Interface diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h index 4af845f..e888cfc 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h @@ -14,9 +14,9 @@ namespace mkt_algorithm class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory { public: - RotateToGoal() {}; + RotateToGoal(); - virtual ~RotateToGoal() {}; + virtual ~RotateToGoal(); /** * @brief Initialize parameters as needed diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index 029309f..d3351eb 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -2,6 +2,13 @@ #include #include +mkt_algorithm::diff::GoStraight::GoStraight() + : PredictiveTrajectory() +{ +} + +mkt_algorithm::diff::GoStraight::~GoStraight() {} + void mkt_algorithm::diff::GoStraight::initialize( robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 70b0ed5..7d57a13 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -1,6 +1,48 @@ #include #include + +mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory() + : initialized_(false), + nav_stop_(false), + x_direction_(0.0), + y_direction_(0.0), + theta_direction_(0.0), + use_velocity_scaled_lookahead_dist_(false), + lookahead_time_(0.0), + lookahead_dist_(0.0), + min_lookahead_dist_(0.0), + max_lookahead_dist_(0.0), + max_lateral_accel_(0.0), + use_rotate_to_heading_(false), + rotate_to_heading_min_angle_(0.0), + min_path_distance_(0.0), + max_path_distance_(0.0), + use_final_heading_alignment_(false), + final_heading_xy_tolerance_(0.0), + final_heading_angle_tolerance_(0.0), + final_heading_min_velocity_(0.0), + final_heading_kp_angular_(0.0), + final_heading_ki_angular_(0.0), + final_heading_kd_angular_(0.0), + near_goal_heading_integral_(0.0), + near_goal_heading_last_error_(0.0), + near_goal_heading_was_active_(false), + use_regulated_linear_velocity_scaling_(false), + min_approach_linear_velocity_(0.0), + regulated_linear_scaling_min_radius_(0.0), + regulated_linear_scaling_min_speed_(0.0), + use_cost_regulated_linear_velocity_scaling_(false), + inflation_cost_scaling_factor_(0.0), + cost_scaling_dist_(0.0), + cost_scaling_gain_(0.0), + control_duration_(0.0), + kf_(nullptr), + m_(0), + n_(0) +{ +} + mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {} void mkt_algorithm::diff::PredictiveTrajectory::initialize( diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp index c621c02..51670e9 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp @@ -4,6 +4,14 @@ #include #include + +mkt_algorithm::diff::RotateToGoal::RotateToGoal() + : PredictiveTrajectory() +{ +} + +mkt_algorithm::diff::RotateToGoal::~RotateToGoal() {} + void mkt_algorithm::diff::RotateToGoal::initialize( robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h index 253f43c..56aac68 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h @@ -15,6 +15,7 @@ namespace mkt_plugins { public: KinematicParameters(); + virtual ~KinematicParameters(); void initialize(const robot::NodeHandle &nh); /** diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h index 99ee362..5c0d783 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h @@ -12,6 +12,15 @@ namespace mkt_plugins class LimitedAccelGenerator : public StandardTrajectoryGenerator { public: + /** + * @brief Constructor + */ + LimitedAccelGenerator(); + /** + * @brief Destructor + */ + virtual ~LimitedAccelGenerator(); + /** * @brief Initialize the generator with parameters from NodeHandle * @param nh NodeHandle for loading acceleration_time parameter diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h index abe6e52..b7480d0 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h @@ -17,6 +17,16 @@ namespace mkt_plugins class StandardTrajectoryGenerator : public score_algorithm::TrajectoryGenerator { public: + + /** + * @brief Constructor + */ + StandardTrajectoryGenerator(); + + /** + * @brief Destructor + */ + virtual ~StandardTrajectoryGenerator(); /** * @brief Initialize the trajectory generator with parameters from NodeHandle * @param nh NodeHandle for loading configuration parameters diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h index d650f7d..37ecab8 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h @@ -21,7 +21,12 @@ namespace mkt_plugins * @brief Default constructor * Initializes all iterators to nullptr */ - XYThetaIterator() : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {} + XYThetaIterator(); + + /** + * @brief Destructor + */ + virtual ~XYThetaIterator(); /** * @brief Initialize the iterator with parameters and kinematics diff --git a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp index 6fa90d2..ae8b0c1 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp @@ -7,7 +7,12 @@ #include #include -mkt_plugins::GoalChecker::GoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25) +mkt_plugins::GoalChecker::GoalChecker() + : nh_(), + line_generator_(nullptr), + yaw_goal_tolerance_(0.25), + xy_goal_tolerance_(0.25), + old_xy_goal_tolerance_(0.0) { } diff --git a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp index f11251d..dfd45dd 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp @@ -32,12 +32,42 @@ namespace mkt_plugins nh.setParam(decel_param, -accel); } - KinematicParameters::KinematicParameters() : xytheta_direct_(), - min_vel_x_(0.0), min_vel_y_(0.0), max_vel_x_(0.0), max_vel_y_(0.0), max_vel_theta_(0.0), - min_speed_xy_(0.0), max_speed_xy_(0.0), min_speed_theta_(0.0), - acc_lim_x_(0.0), acc_lim_y_(0.0), acc_lim_theta_(0.0), - decel_lim_x_(0.0), decel_lim_y_(0.0), decel_lim_theta_(0.0), - min_speed_xy_sq_(0.0), max_speed_xy_sq_(0.0) + KinematicParameters::KinematicParameters() + : xytheta_direct_(), + min_vel_x_(0.0), + min_vel_y_(0.0), + max_vel_x_(0.0), + max_vel_y_(0.0), + max_vel_theta_(0.0), + min_speed_xy_(0.0), + max_speed_xy_(0.0), + min_speed_theta_(0.0), + acc_lim_x_(0.0), + acc_lim_y_(0.0), + acc_lim_theta_(0.0), + decel_lim_x_(0.0), + decel_lim_y_(0.0), + decel_lim_theta_(0.0), + min_speed_xy_sq_(0.0), + max_speed_xy_sq_(0.0), + original_min_vel_x_(0.0), + original_min_vel_y_(0.0), + original_max_vel_x_(0.0), + original_max_vel_y_(0.0), + original_max_vel_theta_(0.0), + original_min_speed_xy_(0.0), + original_max_speed_xy_(0.0), + original_min_speed_theta_(0.0), + original_acc_lim_x_(0.0), + original_acc_lim_y_(0.0), + original_acc_lim_theta_(0.0), + original_decel_lim_x_(0.0), + original_decel_lim_y_(0.0), + original_decel_lim_theta_(0.0) + { + } + + KinematicParameters::~KinematicParameters() { } diff --git a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp index af19d51..1b6e8c6 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp @@ -8,6 +8,16 @@ namespace mkt_plugins { +LimitedAccelGenerator::LimitedAccelGenerator() + : StandardTrajectoryGenerator(), + acceleration_time_(0.0) +{ +} + +LimitedAccelGenerator::~LimitedAccelGenerator() +{ +} + void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh) { StandardTrajectoryGenerator::initialize(nh); diff --git a/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp index c08fe66..fa26177 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp @@ -7,8 +7,13 @@ namespace mkt_plugins { -SimpleGoalChecker::SimpleGoalChecker() : - xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625) +SimpleGoalChecker::SimpleGoalChecker() + : nh_(), + xy_goal_tolerance_(0.25), + yaw_goal_tolerance_(0.25), + stateful_(true), + check_xy_(true), + xy_goal_tolerance_sq_(0.0625) { } diff --git a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp index c077009..581a835 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp @@ -13,6 +13,24 @@ using robot_nav_2d_utils::loadParameterWithDeprecation; namespace mkt_plugins { + + StandardTrajectoryGenerator::StandardTrajectoryGenerator() + : nh_kinematics_(), + kinematics_(nullptr), + velocity_iterator_(nullptr), + sim_time_(0.0), + discretize_by_time_(false), + time_granularity_(0.0), + linear_granularity_(0.0), + angular_granularity_(0.0), + include_last_point_(false) + { + } + + StandardTrajectoryGenerator::~StandardTrajectoryGenerator() + { + } + void StandardTrajectoryGenerator::initialize(const robot::NodeHandle &nh) { nh_kinematics_ = nh; diff --git a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp index ab7a690..c4de3c0 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp @@ -4,6 +4,21 @@ namespace mkt_plugins { +XYThetaIterator::XYThetaIterator() + : vx_samples_(0), + vy_samples_(0), + vtheta_samples_(0), + kinematics_(nullptr), + x_it_(nullptr), + y_it_(nullptr), + th_it_(nullptr) +{ +} + +XYThetaIterator::~XYThetaIterator() +{ +} + void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) { kinematics_ = kinematics; diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index f3829bf..eac6f13 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -13,7 +13,17 @@ namespace two_points_planner { - TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {} + TwoPointsPlanner::TwoPointsPlanner() + : initialized_(false), + lethal_obstacle_(0), + inscribed_inflated_obstacle_(0), + circumscribed_cost_(0), + allow_unknown_(false), + costmap_robot_(nullptr), + current_env_width_(0), + current_env_height_(0) + { + } TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) : initialized_(false), costmap_robot_(NULL) diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h index 6ac81c1..f05ccfa 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h @@ -79,7 +79,6 @@ namespace pnkx_local_planner */ robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; - bool is_ready_; }; } // namespace pnkx_local_planner diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index 74375cf..d030d04 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -14,7 +14,10 @@ #include pnkx_local_planner::PNKXDockingLocalPlanner::PNKXDockingLocalPlanner() - : start_docking_(false) + : PNKXLocalPlanner(), + start_docking_(false), + original_xy_goal_tolerance_(0.0), + original_yaw_goal_tolerance_(0.0) { } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp index 401e9b6..ddbc668 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp @@ -15,6 +15,7 @@ #include pnkx_local_planner::PNKXGoStraightLocalPlanner::PNKXGoStraightLocalPlanner() + : PNKXLocalPlanner() { } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index accc3b9..04358f5 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -17,7 +17,21 @@ #include pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner() - : initialized_(false) + : initialized_(false), + traj_generator_(nullptr), + nav_algorithm_(nullptr), + rotate_algorithm_(nullptr), + goal_checker_(nullptr), + tf_(nullptr), + costmap_(nullptr), + costmap_robot_(nullptr), + info_(), + update_costmap_before_planning_(false), + ret_angle_(false), + ret_nav_(false), + yaw_goal_tolerance_(0.0), + xy_goal_tolerance_(0.0), + lock_(false) { } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp index 429d802..0d4c5a5 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp @@ -14,6 +14,7 @@ #include pnkx_local_planner::PNKXRotateLocalPlanner::PNKXRotateLocalPlanner() + : PNKXLocalPlanner() { } diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index 49dbcb8..264f603 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -406,7 +406,8 @@ namespace robot * @param yaw_goal_tolerance Acceptable heading error (radians). * @return True if docking command succeeded. */ - virtual bool dockTo(const robot_protocol_msgs::Order &msg, + virtual bool dockTo(const robot_protocol_msgs::Order &msg, + const std::string &marker, const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) = 0; diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index 66a860f..52c46fa 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -303,6 +303,7 @@ namespace move_base * @return True if docking command succeeded. */ virtual bool dockTo(const robot_protocol_msgs::Order &msg, + const std::string &marker, const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) override; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index bff0768..57aed24 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -33,20 +33,50 @@ #include move_base::MoveBase::MoveBase() - : initialized_(false), - planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), - planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), - runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), - pause_ctr_(false), paused_(false) + : initialized_(false), + tf_(), + as_(NULL), + planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), + planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), + recovery_index_(0), + recovery_behavior_enabled_(false), + planner_frequency_(0.0), controller_frequency_(0.0), + inscribed_radius_(0.0), circumscribed_radius_(0.0), + planner_patience_(0.0), controller_patience_(0.0), + max_planning_retries_(0), planning_retries_(0), + conservative_reset_dist_(0.0), clearing_radius_(0.0), + shutdown_costmaps_(false), clearing_rotation_allowed_(false), + make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false), + oscillation_timeout_(0.0), oscillation_distance_(0.0), + state_(PLANNING), recovery_trigger_(PLANNING_R), + runPlanner_(false), planner_thread_(NULL), + setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), + pause_ctr_(false), paused_(false), cancel_ctr_(false), + original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0) { } move_base::MoveBase::MoveBase(robot::TFListenerPtr tf) - : initialized_(false), - planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), - planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), - runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), - pause_ctr_(false), paused_(false) + : initialized_(false), + tf_(), + as_(NULL), + planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), + planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), + recovery_index_(0), + recovery_behavior_enabled_(false), + planner_frequency_(0.0), controller_frequency_(0.0), + inscribed_radius_(0.0), circumscribed_radius_(0.0), + planner_patience_(0.0), controller_patience_(0.0), + max_planning_retries_(0), planning_retries_(0), + conservative_reset_dist_(0.0), clearing_radius_(0.0), + shutdown_costmaps_(false), clearing_rotation_allowed_(false), + make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false), + oscillation_timeout_(0.0), oscillation_distance_(0.0), + state_(PLANNING), recovery_trigger_(PLANNING_R), + runPlanner_(false), planner_thread_(NULL), + setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), + pause_ctr_(false), paused_(false), cancel_ctr_(false), + original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0) { initialize(tf); } @@ -134,6 +164,11 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__); } tf_ = tf; + if(tf_) + { + std::string all_frames_string = tf_->allFramesAsString(); + robot::log_info("[%s:%d]\n INFO: tf_ allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str()); + } as_ = new MoveBaseActionServer("move_base", std::bind(&MoveBase::executeCb, this, std::placeholders::_1), true); setupActionServerCallbacks(); @@ -341,8 +376,6 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) } initialized_ = true; setup_ = true; - if(tf_) - robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); robot::log_info("========== End: initialize() - SUCCESS =========="); } else @@ -717,37 +750,37 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na // robot::log_info("addOdometry: %s", odometry_name.c_str()); // robot::log_info("odometry header: %s", odometry.header.frame_id.c_str()); // robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str()); - if(tf_) - { - // try - // { - // robot_geometry_msgs::PoseStamped global_pose_stamped; - // tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose); - // robot_geometry_msgs::PoseStamped robot_pose; - // tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); - // robot_pose.header.frame_id = robot_base_frame_; - // robot_pose.header.stamp = robot::Time(); - // robot::Time current_time = robot::Time::now(); // save time for checking tf delay later + // if(tf_) + // { + // try + // { + // robot_geometry_msgs::PoseStamped global_pose_stamped; + // tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose); + // robot_geometry_msgs::PoseStamped robot_pose; + // tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); + // robot_pose.header.frame_id = robot_base_frame_; + // robot_pose.header.stamp = robot::Time(); + // robot::Time current_time = robot::Time::now(); // save time for checking tf delay later - // // Convert robot::Time to tf3::Time - // tf3::Time tf3_current_time = data_convert::convertTime(current_time); - // tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); + // // Convert robot::Time to tf3::Time + // tf3::Time tf3_current_time = data_convert::convertTime(current_time); + // tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); - // std::string error_msg; - // if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg)) - // { - // // Transform is available at current time - // tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time); - // tf3::doTransform(robot_pose, global_pose_stamped, transform); - // robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation)); - // } - // } - // catch (const tf3::TransformException &ex) - // { - // robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what()); - // } - robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); - } + // std::string error_msg; + // if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg)) + // { + // // Transform is available at current time + // tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time); + // tf3::doTransform(robot_pose, global_pose_stamped, transform); + // robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation)); + // } + // } + // catch (const tf3::TransformException &ex) + // { + // robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what()); + // } + // // robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); + // } // robot::log_info("odometry x: %f y: %f theta: %f", odometry.pose.pose.position.x, odometry.pose.pose.position.y, data_convert::getYaw(odometry.pose.pose.orientation)); // std::stringstream pose_covariance_str; // for(int i = 0; i < 36; i++) { @@ -770,7 +803,8 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na odometry_ = odometry; } -bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) +bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance, double yaw_goal_tolerance) { if (setup_) { @@ -816,12 +850,25 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d // Check pointers if (!tc_) { - throw std::runtime_error("Null 'tc_' pointer encountered"); + robot::log_error("[MoveBase::moveTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; } - if (!controller_costmap_robot_) { - throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); @@ -829,7 +876,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d // Swap planner try { - if(!tc_->swapPlanner(position_planner_name_)) + if (!tc_->swapPlanner(position_planner_name_)) { robot::log_error("[MoveBase::moveTo] Failed to swapPlanner"); return false; @@ -837,8 +884,9 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d } catch (const std::exception &e) { - std::cerr << e.what() << "\n"; - throw std::exception(e); + robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what()); + lock.unlock(); + return false; } // Update navigation feedback @@ -935,20 +983,33 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_)); robot::Duration(0.01).sleep(); - robot::log_info("[MoveBase] In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server."); if (!tc_) { - throw std::runtime_error("Null 'tc_' pointer encountered"); + robot::log_error("[MoveBase::moveTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; } if (!controller_costmap_robot_) { - throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); try { - if(!tc_->swapPlanner(position_planner_name_)) + if (!tc_->swapPlanner(position_planner_name_)) { robot::log_error("[MoveBase::moveTo] Failed to swapPlanner"); return false; @@ -967,49 +1028,76 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->goal_checked = false; } + else + { + robot::log_error("[MoveBase::moveTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } if (cancel_ctr_) cancel_ctr_ = false; - robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); - action_goal->header.stamp = robot::Time::now(); - action_goal->goal.target_pose = goal; - - // Generate unique goal ID using timestamp - robot::Time now = robot::Time::now(); - action_goal->goal_id.stamp = now; - std::ostringstream goal_id_stream; - goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; - action_goal->goal_id.id = goal_id_stream.str(); - - robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); - - // Store Order message for use in planThread + if (!as_) { - boost::unique_lock planner_lock(planner_mutex_); - planner_order_ = boost::make_shared(msg); - robot::log_info("[MoveBase::moveTo] Stored Order message for planning"); + robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + lock.unlock(); + return false; + } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + + // Store Order message for use in planThread + { + boost::unique_lock planner_lock(planner_mutex_); + planner_order_ = boost::make_shared(msg); + robot::log_info("[MoveBase::moveTo] Stored Order message for planning"); + } + if (!planner_order_) + { + robot::log_error("[MoveBase::moveTo] Failed to store Order message for planning"); + lock.unlock(); + return false; + } + + as_->processGoal(action_goal); + } + catch (const std::exception &e) + { + robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what()); + lock.unlock(); + return false; } - - as_->processGoal(action_goal); lock.unlock(); return true; } -bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal, +bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) { std::string maker_sources; - // private_nh_.param("maker_sources", maker_sources, std::string("")); + private_nh_.param("maker_sources", maker_sources, std::string("")); std::stringstream ss(maker_sources); std::string source; bool has_maker = false; while (ss >> source) { - if (maker == source) + if (marker == source) { - private_nh_.setParam("maker_name", maker); + private_nh_.setParam("maker_name", marker); has_maker = true; } } @@ -1019,7 +1107,120 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_ { nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; std::stringstream fb_ss; - fb_ss << "The system has not been '" << maker << "'"; + fb_ss << "The system has not been '" << marker << "'"; + nav_feedback_->feed_back_str = fb_ss.str(); + nav_feedback_->goal_checked = false; + } + return false; + } + + if (setup_) + { + swapPlanner(default_config_.base_global_planner); + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); + nav_feedback_->goal_checked = false; + } + return false; + } + + if (fabs(xy_goal_tolerance) > 0.001) + this->setXyGoalTolerance(fabs(xy_goal_tolerance)); + else + this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_)); + + if (fabs(yaw_goal_tolerance) > 0.001) + this->setYawGoalTolerance(fabs(yaw_goal_tolerance)); + else + this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_)); + + if (!tc_) + { + robot::log_error("[MoveBase::dockTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; + } + if (!controller_costmap_robot_) + { + robot::log_error("[MoveBase::dockTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; + } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + try + { + if (!tc_->swapPlanner(docking_planner_name_)) + { + robot::log_error("[MoveBase::dockTo] Failed to swapPlanner"); + return false; + } + } + catch (const std::exception &e) + { + robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what()); + lock.unlock(); + return false; + } + + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; + nav_feedback_->feed_back_str = std::string("Planning"); + nav_feedback_->goal_checked = false; + } + else + { + robot::log_error("[MoveBase::dockTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } + if (cancel_ctr_) + cancel_ctr_ = false; + + lock.unlock(); + return true; +} + +bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg, + const std::string &marker, + const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + std::string maker_sources; + private_nh_.param("maker_sources", maker_sources, std::string("")); + std::stringstream ss(maker_sources); + std::string source; + bool has_maker = false; + while (ss >> source) + { + if (marker == source) + { + private_nh_.setParam("maker_name", marker); + has_maker = true; + } + } + if (!has_maker) + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + std::stringstream fb_ss; + fb_ss << "The system has not been '" << marker << "'"; nav_feedback_->feed_back_str = fb_ss.str(); nav_feedback_->goal_checked = false; } @@ -1084,17 +1285,53 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_ if (cancel_ctr_) cancel_ctr_ = false; + if (!as_) + { + robot::log_error("[MoveBase::dockTo] as_ pointer is null!"); + lock.unlock(); + return false; + } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_dock_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::dockTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + + // Store Order message for use in planThread (same as moveTo with Order) + { + boost::unique_lock planner_lock(planner_mutex_); + planner_order_ = boost::make_shared(msg); + robot::log_info("[MoveBase::dockTo] Stored Order message for planning"); + } + if (!planner_order_) + { + robot::log_error("[MoveBase::dockTo] Failed to store Order message for planning"); + lock.unlock(); + return false; + } + + as_->processGoal(action_goal); + } + catch (const std::exception &e) + { + robot::log_error("[MoveBase::dockTo] Exception during processGoal: %s", e.what()); + lock.unlock(); + return false; + } + lock.unlock(); return true; } -bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg, - const robot_geometry_msgs::PoseStamped &goal, - double xy_goal_tolerance, double yaw_goal_tolerance) -{ - return false; -} - bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance) { robot::log_info("[MoveBase::moveStraightTo] Entry"); @@ -1117,13 +1354,28 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped this->setXyGoalTolerance(fabs(xy_goal_tolerance)); else this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_)); + if (!tc_) { - throw std::runtime_error("Null 'tc_' pointer encountered"); + robot::log_error("[MoveBase::moveStraightTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; } if (!controller_costmap_robot_) { - throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); @@ -1131,17 +1383,10 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped try { robot::log_info("[MoveBase::moveStraightTo] Entry swapPlanner"); - if(!tc_->swapPlanner(go_straight_planner_name_)) + if (!tc_->swapPlanner(go_straight_planner_name_)) { - if(tf_ == nullptr) - { - robot::log_error("[MoveBase::moveTo] tf_ pointer is null!"); - throw std::runtime_error("Null 'tf_' pointer encountered"); - } - else - { - robot::log_info("[MoveBase::moveTo] tf_ pointer is not null!"); - } + robot::log_error("[MoveBase::moveStraightTo] Failed to swapPlanner"); + lock.unlock(); return false; } } @@ -1158,13 +1403,18 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->goal_checked = false; } + else + { + robot::log_error("[MoveBase::moveStraightTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } if (cancel_ctr_) cancel_ctr_ = false; - // Check if action server exists if (!as_) { - robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + robot::log_error("[MoveBase::moveStraightTo] as_ pointer is null!"); lock.unlock(); return false; } @@ -1174,31 +1424,24 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); action_goal->header.stamp = robot::Time::now(); action_goal->goal.target_pose = goal; - - // Generate unique goal ID using timestamp + robot::Time now = robot::Time::now(); action_goal->goal_id.stamp = now; std::ostringstream goal_id_stream; goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; action_goal->goal_id.id = goal_id_stream.str(); - + robot::log_info("[MoveBase::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); - robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld", + robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld", action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); - - // Clear Order message since this is a non-Order moveTo call + + // Clear Order message since this is a non-Order moveStraightTo call { boost::unique_lock planner_lock(planner_mutex_); planner_order_.reset(); } - - robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server..."); - if(controller_costmap_robot_ == nullptr) - { - robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ is null!"); - return false; - } + robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server..."); as_->processGoal(action_goal); robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server"); } @@ -1240,19 +1483,34 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, if (!tc_) { - throw std::runtime_error("Null 'tc_' pointer encountered"); + robot::log_error("[MoveBase::rotateTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; } if (!controller_costmap_robot_) { - throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + robot::log_error("[MoveBase::rotateTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); try { - if(!tc_->swapPlanner(rotate_planner_name_)) + if (!tc_->swapPlanner(rotate_planner_name_)) { robot::log_error("[MoveBase::rotateTo] Failed to swapPlanner"); + lock.unlock(); return false; } } @@ -1269,29 +1527,34 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->goal_checked = false; } + else + { + robot::log_error("[MoveBase::rotateTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } if (cancel_ctr_) cancel_ctr_ = false; - // Check if action server exists if (!as_) { - robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + robot::log_error("[MoveBase::rotateTo] as_ pointer is null!"); lock.unlock(); return false; } try { - robot_geometry_msgs::Pose2D pose; if (!this->getRobotPose(pose)) { if (nav_feedback_) { nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; - nav_feedback_->feed_back_str = std::string("Coudn't get robot pose"); + nav_feedback_->feed_back_str = std::string("Couldn't get robot pose"); nav_feedback_->goal_checked = false; } + lock.unlock(); return false; } @@ -1302,23 +1565,22 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, action_goal->goal.target_pose.pose.position.x = pose.x + distance * cos(pose.theta); action_goal->goal.target_pose.pose.position.y = pose.y + distance * sin(pose.theta); - // Generate unique goal ID using timestamp robot::Time now = robot::Time::now(); action_goal->goal_id.stamp = now; std::ostringstream goal_id_stream; goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; action_goal->goal_id.id = goal_id_stream.str(); - + robot::log_info("[MoveBase::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); - robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld", + robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld", action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); - - // Clear Order message since this is a non-Order moveTo call + + // Clear Order message since this is a non-Order rotateTo call { boost::unique_lock planner_lock(planner_mutex_); planner_order_.reset(); } - + robot::log_info("[MoveBase::rotateTo] Processing goal through action server..."); as_->processGoal(action_goal); robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server"); @@ -1326,7 +1588,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, catch (const std::exception &e) { lock.unlock(); - robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what()); + robot::log_error("[MoveBase::rotateTo] Exception during processGoal: %s", e.what()); return false; } @@ -1459,7 +1721,6 @@ bool move_base::MoveBase::setTwistLinear(const robot_geometry_msgs::Vector3 &lin { if (tc_->islock()) return true; - return tc_->setTwistLinear(linear); } else if (tc_ && cancel_ctr_) @@ -1878,7 +2139,7 @@ void move_base::MoveBase::planThread() while (wait_for_wake || !runPlanner_) { // if we should not be running the planner then suspend this thread - std::cout << "Planner thread is suspending" << std::endl; + robot::log_info("Planner thread is suspending"); planner_cond_.wait(lock); wait_for_wake = false; } @@ -1888,7 +2149,7 @@ void move_base::MoveBase::planThread() robot_geometry_msgs::PoseStamped temp_goal = planner_goal_; boost::shared_ptr temp_order = planner_order_; lock.unlock(); - std::cout << "Planning..." << std::endl; + robot::log_info("Planning..."); // run planner planner_plan_->clear(); // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y); @@ -1910,7 +2171,7 @@ void move_base::MoveBase::planThread() if (gotPlan) { - std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl; + robot::log_info("Got Plan with %d points!", planner_plan_->size()); // pointer swap the plans under mutex (the controller will pull from latest_plan_) std::vector *temp_plan = planner_plan_; @@ -1921,7 +2182,7 @@ void move_base::MoveBase::planThread() planning_retries_ = 0; new_global_plan_ = true; - std::cout << "Generated a plan from the base_global_planner" << std::endl; + robot::log_info("Generated a plan from the base_global_planner"); // make sure we only start the controller if we still haven't reached the goal if (runPlanner_) @@ -1937,7 +2198,7 @@ void move_base::MoveBase::planThread() // if we didn't get a plan and we are in the planning state (the robot isn't moving) else if (state_ == move_base::PLANNING) { - std::cout << "No Plan..." << std::endl; + robot::log_info("No Plan..."); robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_); // check if we've tried to make a plan for over our time limit or our maximum number of retries