diff --git a/examples/CSharpExample.cs b/examples/CSharpExample.cs deleted file mode 100644 index 5d60a56..0000000 --- a/examples/CSharpExample.cs +++ /dev/null @@ -1,620 +0,0 @@ -using System; -using System.Runtime.InteropServices; -using System.Runtime.CompilerServices; - -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 - } - - // ============================================================================ - // 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 Position position; - public Quaternion orientation; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Header - { - public uint seq; - public long 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 Vector3 - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct NavFeedback - { - public NavigationState navigation_state; - public IntPtr feed_back_str; // char* - public Pose2D current_pose; - [MarshalAs(UnmanagedType.I1)] - public bool goal_checked; - [MarshalAs(UnmanagedType.I1)] - public bool is_ready; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct NamedOccupancyGrid - { - public IntPtr name; // char* - public IntPtr map; // OccupancyGridHandle - } - - [StructLayout(LayoutKind.Sequential)] - public struct NamedLaserScan - { - public IntPtr name; // char* - public IntPtr scan; // LaserScanHandle - } - - [StructLayout(LayoutKind.Sequential)] - public struct NamedPointCloud - { - public IntPtr name; // char* - public IntPtr cloud; // PointCloudHandle - } - - [StructLayout(LayoutKind.Sequential)] - public struct NamedPointCloud2 - { - public IntPtr name; // char* - public IntPtr cloud; // PointCloud2Handle - } - - [StructLayout(LayoutKind.Sequential)] - public struct PlannerDataOutput - { - public IntPtr plan; // Path2DHandle - public IntPtr costmap; // OccupancyGridHandle - public IntPtr costmap_update; // OccupancyGridUpdateHandle - [MarshalAs(UnmanagedType.I1)] - public bool is_costmap_updated; - public IntPtr footprint; // PolygonStampedHandle - } - - // ============================================================================ - // String Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void nav_c_api_free_string(IntPtr str); - - - // ============================================================================ - // Complex Message Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_occupancy_grid(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_occupancy_grid_update(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_laser_scan(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_point_cloud(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_point_cloud2(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_odometry(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_path2d(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_polygon_stamped(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_order(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_occupancy_grids(IntPtr maps, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_laser_scans(IntPtr scans, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_point_clouds(IntPtr clouds, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_point_cloud2s(IntPtr clouds, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_planner_data(ref PlannerDataOutput data); - - // ============================================================================ - // State Conversion - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - public static extern IntPtr navigation_state_to_string(NavigationState state); - - // ============================================================================ - // Helper Functions - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_offset_goal_2d( - double pose_x, double pose_y, double pose_theta, - string frame_id, double offset_distance, - ref PoseStamped out_goal); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_offset_goal_stamped( - ref PoseStamped in_pose, double offset_distance, - ref PoseStamped out_goal); - - // ============================================================================ - // Navigation Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern IntPtr navigation_create(); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_destroy(IntPtr handle); - - // ============================================================================ - // TF Listener Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern IntPtr tf_listener_create(); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void tf_listener_destroy(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool tf_listener_set_static_transform( - IntPtr tf_handle, - string parent_frame, - string child_frame, - double x, double y, double z, - double qx, double qy, double qz, double qw); - - // ============================================================================ - // Navigation Interface Methods - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_robot_footprint( - IntPtr handle, Point[] points, UIntPtr point_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_footprint( - IntPtr handle, out IntPtr out_points, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_points(IntPtr points); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to( - IntPtr handle, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to_order( - IntPtr handle, IntPtr order, ref 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( - IntPtr handle, string marker, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_dock_to_order( - IntPtr handle, IntPtr order, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_straight_to( - IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_rotate_to( - IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_pause(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_resume(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_cancel(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_linear( - IntPtr handle, double linear_x, double linear_y, double linear_z); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_angular( - IntPtr handle, double angular_x, double angular_y, double angular_z); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_stamped( - IntPtr handle, ref PoseStamped out_pose); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_2d( - IntPtr handle, ref Pose2D out_pose); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_twist( - IntPtr handle, ref Twist2DStamped out_twist); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_feedback( - IntPtr handle, ref NavFeedback out_feedback); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_feedback(ref NavFeedback feedback); - - - // ============================================================================ - // Navigation Data Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_static_map(IntPtr handle, string map_name, IntPtr map); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_laser_scan(IntPtr handle, string laser_scan_name, IntPtr laser_scan); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_point_cloud(IntPtr handle, string point_cloud_name, IntPtr point_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_point_cloud2(IntPtr handle, string point_cloud2_name, IntPtr point_cloud2); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_odometry(IntPtr handle, string odometry_name, IntPtr odometry); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_static_map(IntPtr handle, string map_name, out IntPtr out_map); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_laser_scan(IntPtr handle, string laser_scan_name, out IntPtr out_scan); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_point_cloud(IntPtr handle, string point_cloud_name, out IntPtr out_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_point_cloud2(IntPtr handle, string point_cloud2_name, out IntPtr out_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_static_maps(IntPtr handle, out IntPtr out_maps, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_laser_scans(IntPtr handle, out IntPtr out_scans, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_point_clouds(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_point_cloud2s(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_static_map(IntPtr handle, string map_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_laser_scan(IntPtr 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(IntPtr 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(IntPtr handle, string point_cloud2_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_static_maps(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_laser_scans(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_point_clouds(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_point_cloud2s(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_data(IntPtr handle); - - - // ============================================================================ - // Planner Data - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_global_data(IntPtr handle, ref PlannerDataOutput out_data); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data); - - // ============================================================================ - // Helper Methods for String Conversion - // ============================================================================ - public static string MarshalString(IntPtr ptr) - { - if (ptr == IntPtr.Zero) - return string.Empty; - return Marshal.PtrToStringAnsi(ptr); - } - } - - // ============================================================================ - // Example Usage - // ============================================================================ - class Program - { - // Helper method để hiển thị file và line number tự động - static void LogError(string message, - [CallerFilePath] string filePath = "", - [CallerLineNumber] int lineNumber = 0, - [CallerMemberName] string memberName = "") - { - // Lấy tên file từ đường dẫn đầy đủ - string fileName = System.IO.Path.GetFileName(filePath); - Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}"); - } - - static void Main(string[] args) - { - // Create TF listener - IntPtr tfHandle = NavigationAPI.tf_listener_create(); - if (tfHandle == IntPtr.Zero) - { - LogError("Failed to create TF listener"); - return; - } - - // 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)) - { - LogError("Failed to inject static TF map -> odom"); - NavigationAPI.tf_listener_destroy(tfHandle); - 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 - IntPtr navHandle = NavigationAPI.navigation_create(); - if (navHandle == IntPtr.Zero) - { - LogError("Failed to create navigation instance"); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - // Initialize navigation - if (!NavigationAPI.navigation_initialize(navHandle, tfHandle)) - { - LogError("Failed to initialize navigation"); - NavigationAPI.navigation_destroy(navHandle); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - // Set robot footprint - NavigationAPI.Point[] footprint = new NavigationAPI.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 } - }; - NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); - - // Get robot pose - NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D(); - if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose)) - { - Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}"); - } - - // Get navigation feedback - NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); - if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) - { - string stateStr = NavigationAPI.MarshalString( - NavigationAPI.navigation_state_to_string(feedback.navigation_state)); - string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str); - Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}"); - NavigationAPI.navigation_free_feedback(ref feedback); - } - - - // Get global planner data (opaque handles) - NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput(); - if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData)) - { - Console.WriteLine($"Global data received (costmap_updated={globalData.is_costmap_updated})"); - NavigationAPI.navigation_free_planner_data(ref globalData); - } - - // Get all static maps (names + opaque handles) - IntPtr mapsPtr; - UIntPtr mapsCount; - if (NavigationAPI.navigation_get_all_static_maps(navHandle, out mapsPtr, out mapsCount)) - { - ulong count = mapsCount.ToUInt64(); - Console.WriteLine($"Static maps: {count}"); - if (mapsPtr != IntPtr.Zero && count > 0) - { - int itemSize = Marshal.SizeOf(); - for (ulong i = 0; i < count; i++) - { - IntPtr itemPtr = IntPtr.Add(mapsPtr, checked((int)(i * (ulong)itemSize))); - var item = Marshal.PtrToStructure(itemPtr); - string name = NavigationAPI.MarshalString(item.name); - Console.WriteLine($"- {name}"); - } - NavigationAPI.navigation_free_named_occupancy_grids(mapsPtr, (UIntPtr)count); - } - } - - // Cleanup - NavigationAPI.navigation_destroy(navHandle); - NavigationAPI.tf_listener_destroy(tfHandle); - } - } -} - diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 2c363c6..768c885 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -53,6 +53,18 @@ namespace NavigationExample 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 { @@ -85,11 +97,27 @@ namespace NavigationExample 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 long sec; + public uint sec; public uint nsec; public IntPtr frame_id; // char* } @@ -108,384 +136,232 @@ namespace NavigationExample public Twist2D velocity; } + [StructLayout(LayoutKind.Sequential)] - public struct Vector3 + public struct NavigationHandle { - public double x; - public double y; - public double z; + public IntPtr ptr; } [StructLayout(LayoutKind.Sequential)] - public struct NavFeedback + public struct TFListenerHandle { - public NavigationState navigation_state; - public IntPtr feed_back_str; // char* - public Pose2D current_pose; - [MarshalAs(UnmanagedType.I1)] - public bool goal_checked; - [MarshalAs(UnmanagedType.I1)] - public bool is_ready; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct NamedOccupancyGrid - { - public IntPtr name; // char* - public IntPtr map; // OccupancyGridHandle + public IntPtr ptr; } [StructLayout(LayoutKind.Sequential)] - public struct NamedLaserScan + public struct LaserScan { - public IntPtr name; // char* - public IntPtr scan; // LaserScanHandle + 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 NamedPointCloud + public struct PoseWithCovariance { - public IntPtr name; // char* - public IntPtr cloud; // PointCloudHandle + public Pose pose; + public IntPtr covariance; + public UIntPtr covariance_count; } [StructLayout(LayoutKind.Sequential)] - public struct NamedPointCloud2 - { - public IntPtr name; // char* - public IntPtr cloud; // PointCloud2Handle + public struct TwistWithCovariance { + public Twist twist; + public IntPtr covariance; + public UIntPtr covariance_count; } [StructLayout(LayoutKind.Sequential)] - public struct PlannerDataOutput + public struct Odometry { - public IntPtr plan; // Path2DHandle - public IntPtr costmap; // OccupancyGridHandle - public IntPtr costmap_update; // OccupancyGridUpdateHandle - [MarshalAs(UnmanagedType.I1)] - public bool is_costmap_updated; - public IntPtr footprint; // PolygonStampedHandle + public Header header; + public IntPtr child_frame_id; + public PoseWithCovariance pose; + public TwistWithCovariance twist; } - // ============================================================================ - // String Management - // ============================================================================ + [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); - - // ============================================================================ - // Complex Message Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_occupancy_grid(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_occupancy_grid_update(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_laser_scan(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_point_cloud(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_point_cloud2(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_odometry(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_path2d(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_polygon_stamped(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_order(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_occupancy_grids(IntPtr maps, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_laser_scans(IntPtr scans, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_point_clouds(IntPtr clouds, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_point_cloud2s(IntPtr clouds, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_planner_data(ref PlannerDataOutput data); - - // ============================================================================ - // State Conversion - // ============================================================================ + /// 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); - // ============================================================================ - // Helper Functions - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_offset_goal_2d( - double pose_x, double pose_y, double pose_theta, - string frame_id, double offset_distance, - ref PoseStamped out_goal); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_offset_goal_stamped( - ref PoseStamped in_pose, double offset_distance, - ref PoseStamped out_goal); + public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback); - // ============================================================================ - // Navigation Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern IntPtr navigation_create(); + /// 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; + } - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_destroy(IntPtr handle); + /// 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 IntPtr tf_listener_create(); + public static extern TFListenerHandle tf_listener_create(); [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void tf_listener_destroy(IntPtr handle); + 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( - IntPtr tf_handle, + TFListenerHandle tf_handle, string parent_frame, string child_frame, double x, double y, double z, double qx, double qy, double qz, double qw); // ============================================================================ - // Navigation Interface Methods + // Navigation Handle Management // ============================================================================ [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle); + 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_set_robot_footprint( - IntPtr handle, Point[] points, UIntPtr point_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_footprint( - IntPtr handle, out IntPtr out_points, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_points(IntPtr points); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to( - IntPtr handle, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to_order( - IntPtr handle, IntPtr order, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + 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_dock_to( - IntPtr handle, string marker, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_dock_to_order( - IntPtr handle, IntPtr order, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_straight_to( - IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance); + public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_rotate_to( - IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance); + 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)] - public static extern void navigation_pause(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_resume(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_cancel(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_linear( - IntPtr handle, double linear_x, double linear_y, double linear_z); + public static extern bool navigation_move_straight_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_angular( - IntPtr handle, double angular_x, double angular_y, double angular_z); + public static extern bool navigation_rotate_to(NavigationHandle handle, PoseStamped goal, double yaw_goal_tolerance); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_stamped( - IntPtr handle, ref PoseStamped out_pose); + public static extern bool navigation_pause(NavigationHandle handle); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_2d( - IntPtr handle, ref Pose2D out_pose); + public static extern bool navigation_resume(NavigationHandle handle); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_twist( - IntPtr handle, ref Twist2DStamped out_twist); + public static extern bool navigation_cancel(NavigationHandle handle); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_feedback( - IntPtr handle, ref NavFeedback out_feedback); + public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_feedback(ref NavFeedback feedback); + [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_static_map(IntPtr handle, string map_name, IntPtr map); + 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_laser_scan(IntPtr handle, string laser_scan_name, IntPtr laser_scan); + 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_add_point_cloud(IntPtr handle, string point_cloud_name, IntPtr point_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_point_cloud2(IntPtr handle, string point_cloud2_name, IntPtr point_cloud2); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_odometry(IntPtr handle, string odometry_name, IntPtr odometry); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_static_map(IntPtr handle, string map_name, out IntPtr out_map); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_laser_scan(IntPtr handle, string laser_scan_name, out IntPtr out_scan); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_point_cloud(IntPtr handle, string point_cloud_name, out IntPtr out_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_point_cloud2(IntPtr handle, string point_cloud2_name, out IntPtr out_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_static_maps(IntPtr handle, out IntPtr out_maps, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_laser_scans(IntPtr handle, out IntPtr out_scans, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_point_clouds(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_point_cloud2s(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_static_map(IntPtr handle, string map_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_laser_scan(IntPtr 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(IntPtr 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(IntPtr handle, string point_cloud2_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_static_maps(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_laser_scans(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_point_clouds(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_point_cloud2s(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_data(IntPtr handle); - - - // ============================================================================ - // Planner Data - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_global_data(IntPtr handle, ref PlannerDataOutput out_data); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data); - - - // ============================================================================ - // Navigation Commands with Order - // ============================================================================ - - - - - // ============================================================================ - // Helper Methods for String Conversion - // ============================================================================ - public static string MarshalString(IntPtr ptr) - { - if (ptr == IntPtr.Zero) - return string.Empty; - return Marshal.PtrToStringAnsi(ptr); - } + public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid); + } // ============================================================================ @@ -507,8 +383,8 @@ namespace NavigationExample static void Main(string[] args) { // Create TF listener - IntPtr tfHandle = NavigationAPI.tf_listener_create(); - if (tfHandle == IntPtr.Zero) + NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create(); + if (tfHandle.ptr == IntPtr.Zero) { LogError("Failed to create TF listener"); return; @@ -544,8 +420,8 @@ namespace NavigationExample } // Create navigation instance - IntPtr navHandle = NavigationAPI.navigation_create(); - if (navHandle == IntPtr.Zero) + NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create(); + if (navHandle.ptr == IntPtr.Zero) { LogError("Failed to create navigation instance"); NavigationAPI.tf_listener_destroy(tfHandle); @@ -571,54 +447,132 @@ namespace NavigationExample }; NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); - // Get robot pose - NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D(); - if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose)) - { - Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}"); - } - // Get navigation feedback NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) { - string stateStr = NavigationAPI.MarshalString( - NavigationAPI.navigation_state_to_string(feedback.navigation_state)); + IntPtr stateStrPtr = NavigationAPI.navigation_state_to_string(feedback.navigation_state); + string stateStr = NavigationAPI.MarshalString(stateStrPtr); + NavigationAPI.nav_c_api_free_string(stateStrPtr); string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str); Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}"); NavigationAPI.navigation_free_feedback(ref feedback); } + System.Threading.Thread.Sleep(1000); + IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan"); + NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId)); + NavigationAPI.LaserScan fscanHandle; + fscanHandle.header = fscanHeader; + fscanHandle.angle_min = -1.57f; + fscanHandle.angle_max = 1.57f; + fscanHandle.angle_increment = 0.785f; + fscanHandle.time_increment = 0.0f; + fscanHandle.scan_time = 0.1f; + fscanHandle.range_min = 0.05f; + fscanHandle.range_max = 10.0f; + fscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5); + Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, fscanHandle.ranges, 5); + fscanHandle.ranges_count = new UIntPtr(5); + fscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5); + Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, fscanHandle.intensities, 5); + fscanHandle.intensities_count = new UIntPtr(5); + NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan", fscanHandle); - // Get global planner data (opaque handles) - NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput(); - if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData)) - { - Console.WriteLine($"Global data received (costmap_updated={globalData.is_costmap_updated})"); - NavigationAPI.navigation_free_planner_data(ref globalData); + + IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan"); + NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId)); + NavigationAPI.LaserScan bscanHandle; + bscanHandle.header = bscanHeader; + bscanHandle.angle_min = 1.57f; + bscanHandle.angle_max = -1.57f; + bscanHandle.angle_increment = -0.785f; + bscanHandle.time_increment = 0.0f; + bscanHandle.scan_time = 0.1f; + bscanHandle.range_min = 0.05f; + bscanHandle.range_max = 10.0f; + bscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5); + Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, bscanHandle.ranges, 5); + bscanHandle.ranges_count = new UIntPtr(5); + bscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5); + Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, bscanHandle.intensities, 5); + bscanHandle.intensities_count = new UIntPtr(5); + NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan", bscanHandle); + + System.Threading.Thread.Sleep(1000); + + IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom"); + NavigationAPI.Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId)); + NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry(); + odometryHandle.header = odometryHeader; + IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint"); + odometryHandle.child_frame_id = childFrameId; + odometryHandle.pose.pose.position.x = 0.0; + odometryHandle.pose.pose.position.y = 0.0; + odometryHandle.pose.pose.position.z = 0.0; + odometryHandle.pose.pose.orientation.x = 0.0; + odometryHandle.pose.pose.orientation.y = 0.0; + odometryHandle.pose.pose.orientation.z = 0.0; + odometryHandle.pose.pose.orientation.w = 1.0; + + double[] pose_covariance = new double[36]; + for(int i = 0; i < pose_covariance.Length; i++) { + pose_covariance[i] = 0.0; } + odometryHandle.pose.covariance = Marshal.AllocHGlobal(sizeof(double) * pose_covariance.Length); + Marshal.Copy(pose_covariance, 0, odometryHandle.pose.covariance, pose_covariance.Length); + odometryHandle.pose.covariance_count = new UIntPtr((uint)pose_covariance.Length); - // Get all static maps (names + opaque handles) - IntPtr mapsPtr; - UIntPtr mapsCount; - if (NavigationAPI.navigation_get_all_static_maps(navHandle, out mapsPtr, out mapsCount)) - { - ulong count = mapsCount.ToUInt64(); - Console.WriteLine($"Static maps: {count}"); - if (mapsPtr != IntPtr.Zero && count > 0) - { - int itemSize = Marshal.SizeOf(); - for (ulong i = 0; i < count; i++) - { - IntPtr itemPtr = IntPtr.Add(mapsPtr, checked((int)(i * (ulong)itemSize))); - var item = Marshal.PtrToStructure(itemPtr); - string name = NavigationAPI.MarshalString(item.name); - Console.WriteLine($"- {name}"); - } - NavigationAPI.navigation_free_named_occupancy_grids(mapsPtr, (UIntPtr)count); - } + odometryHandle.twist.twist.linear.x = 0.0; + odometryHandle.twist.twist.linear.y = 0.0; + odometryHandle.twist.twist.linear.z = 0.0; + odometryHandle.twist.twist.angular.x = 0.0; + odometryHandle.twist.twist.angular.y = 0.0; + odometryHandle.twist.twist.angular.z = 0.0; + double[] twist_covariance = new double[36]; + for(int i = 0; i < twist_covariance.Length; i++) { + twist_covariance[i] = 0.0; } + odometryHandle.twist.covariance = Marshal.AllocHGlobal(sizeof(double) * twist_covariance.Length); + Marshal.Copy(twist_covariance, 0, odometryHandle.twist.covariance, twist_covariance.Length); + odometryHandle.twist.covariance_count = new UIntPtr((uint)twist_covariance.Length); + NavigationAPI.navigation_add_odometry(navHandle, "odometry", odometryHandle); + System.Threading.Thread.Sleep(1000); + // Add static map + NavigationAPI.Time mapLoadTime = NavigationAPI.time_create(); + NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData(); + mapMetaData.map_load_time = mapLoadTime; + mapMetaData.resolution = 0.05f; + mapMetaData.width = 3; + mapMetaData.height = 10; + mapMetaData.origin = new NavigationAPI.Pose(); + mapMetaData.origin.position.x = 0.0; + mapMetaData.origin.position.y = 0.0; + mapMetaData.origin.position.z = 0.0; + mapMetaData.origin.orientation.x = 0.0; + mapMetaData.origin.orientation.y = 0.0; + mapMetaData.origin.orientation.z = 0.0; + mapMetaData.origin.orientation.w = 1.0; + NavigationAPI.OccupancyGrid occupancyGrid = new NavigationAPI.OccupancyGrid(); + IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map"); + occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId)); + occupancyGrid.info = mapMetaData; + byte[] data = new byte[30]; + for (int i = 0; i < data.Length; i++) { + data[i] = 100; + } + occupancyGrid.data = Marshal.AllocHGlobal(sizeof(byte) * data.Length); + Marshal.Copy(data, 0, occupancyGrid.data, data.Length); + 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")); + + NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid); + // Cleanup NavigationAPI.navigation_destroy(navHandle); NavigationAPI.tf_listener_destroy(tfHandle); diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index d57761c..3ba8536 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/examples/run_example.sh b/examples/run_example.sh index b651fab..19bf39a 100755 --- a/examples/run_example.sh +++ b/examples/run_example.sh @@ -82,8 +82,6 @@ fi # Luôn copy source code mới nhất (cập nhật file nếu đã có) cd "$EXAMPLE_DIR/NavigationExample" -echo "Updating Program.cs from CSharpExample.cs..." -cp ../CSharpExample.cs Program.cs # Bước 3: Copy library echo "Copying library..." diff --git a/src/APIs/c_api/CMakeLists.txt b/src/APIs/c_api/CMakeLists.txt index cfa9c29..165b552 100644 --- a/src/APIs/c_api/CMakeLists.txt +++ b/src/APIs/c_api/CMakeLists.txt @@ -10,13 +10,12 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -# Find Boost -find_package(Boost REQUIRED) +# Find Boost (filesystem needed for plugin path / boost::dll usage) +find_package(Boost REQUIRED COMPONENTS filesystem system) # Dependencies set(PACKAGES_DIR move_base_core - move_base tf3 robot_time robot_cpp @@ -29,8 +28,28 @@ include_directories( ) # Tìm tất cả file source -file(GLOB SOURCES "src/*.cpp") -file(GLOB HEADERS "include/*.h") +file(GLOB SOURCES + "src/*.cpp" + "src/std_msgs/*.cpp" + "src/sensor_msgs/*.cpp" + "src/geometry_msgs/*.cpp" + "src/nav_msgs/*.cpp" + "src/nav_2d_msgs/*.cpp" + "src/robot_nav_msgs/*.cpp" + "src/robot_geometry_msgs/*.cpp" + "src/map_msgs/*.cpp" +) +file(GLOB HEADERS + "include/*.h" + "include/std_msgs/*.h" + "include/sensor_msgs/*.h" + "include/geometry_msgs/*.h" + "include/nav_msgs/*.h" + "include/nav_2d_msgs/*.h" + "include/robot_nav_msgs/*.h" + "include/robot_geometry_msgs/*.h" + "include/map_msgs/*.h" +) # Tạo thư viện shared (.so) add_library(nav_c_api SHARED ${SOURCES} ${HEADERS}) @@ -41,6 +60,8 @@ target_link_libraries(nav_c_api ${PACKAGES_DIR} PRIVATE Boost::boost + Boost::filesystem + Boost::system dl ) diff --git a/src/APIs/c_api/include/convertor.h b/src/APIs/c_api/include/convertor.h new file mode 100644 index 0000000..1be3c7c --- /dev/null +++ b/src/APIs/c_api/include/convertor.h @@ -0,0 +1,153 @@ +#ifndef C_API_CONVERTOR_H +#define C_API_CONVERTOR_H + +// C +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/PointCloud.h" +#include "sensor_msgs/PointCloud2.h" +#include "nav_msgs/Odometry.h" +#include "std_msgs/Header.h" +#include "nav_msgs/OccupancyGrid.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/Pose2D.h" +#include "nav_2d_msgs/Twist2D.h" +#include "nav_2d_msgs/Twist2DStamped.h" + +// C++ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +/** + * @brief Convert C LaserScan to C++ LaserScan + * @param laser_scan C LaserScan + * @return C++ LaserScan + */ +robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan); + +/** + * @brief Convert C OccupancyGrid to C++ OccupancyGrid + * @param occupancy_grid C OccupancyGrid + * @return C++ OccupancyGrid + */ +robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occupancy_grid); + +/** + * @brief Convert C++ OccupancyGrid to C OccupancyGrid + * @param cpp C++ OccupancyGrid + * @param out C OccupancyGrid + */ +void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out); + +/** + * @brief Convert C++ LaserScan to C LaserScan + * @param cpp C++ LaserScan + * @param out C LaserScan + */ +void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out); + +/** + * @brief Convert C++ PointCloud to C PointCloud + * @param cpp C++ PointCloud + * @param out C PointCloud + */ +void convert2CPointCloud(const robot_sensor_msgs::PointCloud& cpp, PointCloud& out); + +/** + * @brief Convert C++ PointCloud2 to C PointCloud2 + * @param cpp C++ PointCloud2 + * @param out C PointCloud2 + */ +void convert2CPointCloud2(const robot_sensor_msgs::PointCloud2& cpp, PointCloud2& out); + +/** + * @brief Convert C PointCloud to C++ PointCloud + * @param c C PointCloud + * @return C++ PointCloud + */ +robot_sensor_msgs::PointCloud convert2CppPointCloud(const PointCloud& c); + +/** + * @brief Convert C PointCloud2 to C++ PointCloud2 + * @param c C PointCloud2 + * @return C++ PointCloud2 + */ +robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c); + +/** + * @brief Convert C Odometry to C++ Odometry + * @param odometry C Odometry + * @return C++ Odometry + */ +robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry); + +/** + * @brief Convert C PoseStamped to C++ PoseStamped + * @param pose_stamped C PoseStamped + * @return C++ PoseStamped + */ +robot_geometry_msgs::PoseStamped convert2CppPoseStamped(const PoseStamped& pose_stamped); + +/** + * @brief Convert C++ PoseStamped to C PoseStamped + * @param cpp_pose_stamped C++ PoseStamped + * @return C PoseStamped + */ +PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pose_stamped); + + +/** + * @brief Convert C Pose2D to C++ Pose2D + * @param pose_2d C Pose2D + * @return C++ Pose2D + */ +robot_geometry_msgs::Pose2D convert2CppPose2D(const Pose2D& pose_2d); + +/** + * @brief Convert C++ Pose2D to C Pose2D + * @param cpp_pose_2d C++ Pose2D + * @return C Pose2D + */ + Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose_2d); + +/** + * @brief Convert C Twist2D to C++ Twist2D + * @param twist_2d C Twist2D + * @return C++ Twist2D + */ +robot_nav_2d_msgs::Twist2D convert2CppTwist2D(const Twist2D& twist_2d); + + +/** + * @brief Convert C++ Twist2D to C Twist2D + * @param cpp_twist_2d C++ Twist2D + * @return C Twist2D + */ +Twist2D convert2CTwist2D(const robot_nav_2d_msgs::Twist2D& cpp_twist_2d); + +/** + * @brief Convert C Twist2DStamped to C++ Twist2DStamped + * @param twist_2d_stamped C Twist2DStamped + * @return C++ Twist2DStamped + */ +robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped& twist_2d_stamped); + + +/** + * @brief Convert C++ Twist2DStamped to C Twist2DStamped + * @param cpp_twist_2d_stamped C++ Twist2DStamped + * @return C Twist2DStamped + */ +Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped); + +#endif // C_API_CONVERTOR_H \ No newline at end of file diff --git a/src/APIs/c_api/include/geometry_msgs/Accel.h b/src/APIs/c_api/include/geometry_msgs/Accel.h new file mode 100644 index 0000000..03397f8 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Accel.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_ACCEL_H +#define C_API_GEOMETRY_MSGS_ACCEL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Vector3 linear; + Vector3 angular; +} Accel; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_ACCEL_H diff --git a/src/APIs/c_api/include/geometry_msgs/AccelStamped.h b/src/APIs/c_api/include/geometry_msgs/AccelStamped.h new file mode 100644 index 0000000..5ce2455 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/AccelStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_ACCELSTAMPED_H +#define C_API_GEOMETRY_MSGS_ACCELSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Accel.h" + +typedef struct +{ + Header header; + Accel accel; +} AccelStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_ACCELSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h b/src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h new file mode 100644 index 0000000..c66b1ac --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H +#define C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Accel.h" + +typedef struct +{ + Accel accel; + double covariance[36]; +} AccelWithCovariance; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H diff --git a/src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h b/src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h new file mode 100644 index 0000000..5a48066 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H +#define C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/AccelWithCovariance.h" + +typedef struct +{ + Header header; + AccelWithCovariance accel; +} AccelWithCovarianceStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Inertia.h b/src/APIs/c_api/include/geometry_msgs/Inertia.h new file mode 100644 index 0000000..645d9ae --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Inertia.h @@ -0,0 +1,28 @@ +#ifndef C_API_GEOMETRY_MSGS_INERTIA_H +#define C_API_GEOMETRY_MSGS_INERTIA_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + double m; + Vector3 com; + double ixx; + double ixy; + double ixz; + double iyy; + double iyz; + double izz; +} Inertia; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_INERTIA_H diff --git a/src/APIs/c_api/include/geometry_msgs/InertiaStamped.h b/src/APIs/c_api/include/geometry_msgs/InertiaStamped.h new file mode 100644 index 0000000..d077204 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/InertiaStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_INERTIASTAMPED_H +#define C_API_GEOMETRY_MSGS_INERTIASTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Inertia.h" + +typedef struct +{ + Header header; + Inertia inertia; +} InertiaStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_INERTIASTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Point.h b/src/APIs/c_api/include/geometry_msgs/Point.h new file mode 100644 index 0000000..12346f5 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Point.h @@ -0,0 +1,21 @@ +#ifndef C_API_GEOMETRY_MSGS_POINT_H +#define C_API_GEOMETRY_MSGS_POINT_H + +#ifdef __cplusplus +extern "C" +{ +#endif +#include +#include + + typedef struct + { + double x; + double y; + double z; + } Point; + +#ifdef __cplusplus +} +#endif +#endif // C_API_GEOMETRY_MSGS_POINT_H \ No newline at end of file diff --git a/src/APIs/c_api/include/geometry_msgs/Point32.h b/src/APIs/c_api/include/geometry_msgs/Point32.h new file mode 100644 index 0000000..9721822 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Point32.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_POINT32_H +#define C_API_GEOMETRY_MSGS_POINT32_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + float x; + float y; + float z; +} Point32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POINT32_H diff --git a/src/APIs/c_api/include/geometry_msgs/PointStamped.h b/src/APIs/c_api/include/geometry_msgs/PointStamped.h new file mode 100644 index 0000000..81f89bc --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PointStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POINTSTAMPED_H +#define C_API_GEOMETRY_MSGS_POINTSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" + +typedef struct +{ + Header header; + Point point; +} PointStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POINTSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Polygon.h b/src/APIs/c_api/include/geometry_msgs/Polygon.h new file mode 100644 index 0000000..d34d157 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Polygon.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_POLYGON_H +#define C_API_GEOMETRY_MSGS_POLYGON_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Point32.h" + +typedef struct +{ + Point32 *points; + size_t points_count; +} Polygon; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POLYGON_H diff --git a/src/APIs/c_api/include/geometry_msgs/PolygonStamped.h b/src/APIs/c_api/include/geometry_msgs/PolygonStamped.h new file mode 100644 index 0000000..f4957e6 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PolygonStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H +#define C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Polygon.h" + +typedef struct +{ + Header header; + Polygon polygon; +} PolygonStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Pose.h b/src/APIs/c_api/include/geometry_msgs/Pose.h new file mode 100644 index 0000000..595ab8c --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Pose.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POSE_H +#define C_API_GEOMETRY_MSGS_POSE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Quaternion.h" + +typedef struct +{ + Point position; + Quaternion orientation; +} Pose; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSE_H diff --git a/src/APIs/c_api/include/geometry_msgs/Pose2D.h b/src/APIs/c_api/include/geometry_msgs/Pose2D.h new file mode 100644 index 0000000..cc282d3 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Pose2D.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_POSE2D_H +#define C_API_GEOMETRY_MSGS_POSE2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double theta; +} Pose2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSE2D_H diff --git a/src/APIs/c_api/include/geometry_msgs/PoseArray.h b/src/APIs/c_api/include/geometry_msgs/PoseArray.h new file mode 100644 index 0000000..60876a7 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PoseArray.h @@ -0,0 +1,24 @@ +#ifndef C_API_GEOMETRY_MSGS_POSEARRAY_H +#define C_API_GEOMETRY_MSGS_POSEARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Pose.h" + +typedef struct +{ + Header header; + Pose *poses; + size_t poses_count; +} PoseArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSEARRAY_H diff --git a/src/APIs/c_api/include/geometry_msgs/PoseStamped.h b/src/APIs/c_api/include/geometry_msgs/PoseStamped.h new file mode 100644 index 0000000..6207d60 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PoseStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POSESTAMPED_H +#define C_API_GEOMETRY_MSGS_POSESTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Pose.h" + +typedef struct +{ + Header header; + Pose pose; +} PoseStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSESTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h b/src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h new file mode 100644 index 0000000..cc1d7e9 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H +#define C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Pose.h" + +typedef struct +{ + Pose pose; + double *covariance; + size_t covariance_count; +} PoseWithCovariance; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H diff --git a/src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h b/src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h new file mode 100644 index 0000000..e243833 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H +#define C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/PoseWithCovariance.h" + +typedef struct +{ + Header header; + PoseWithCovariance pose; +} PoseWithCovarianceStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Quaternion.h b/src/APIs/c_api/include/geometry_msgs/Quaternion.h new file mode 100644 index 0000000..e843fc1 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Quaternion.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_QUATERNION_H +#define C_API_GEOMETRY_MSGS_QUATERNION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double z; + double w; +} Quaternion; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_QUATERNION_H diff --git a/src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h b/src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h new file mode 100644 index 0000000..89f21d1 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H +#define C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Quaternion.h" + +typedef struct +{ + Header header; + Quaternion quaternion; +} QuaternionStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Transform.h b/src/APIs/c_api/include/geometry_msgs/Transform.h new file mode 100644 index 0000000..c318393 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Transform.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_TRANSFORM_H +#define C_API_GEOMETRY_MSGS_TRANSFORM_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" +#include "geometry_msgs/Quaternion.h" + +typedef struct +{ + Vector3 translation; + Quaternion rotation; +} Transform; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TRANSFORM_H diff --git a/src/APIs/c_api/include/geometry_msgs/TransformStamped.h b/src/APIs/c_api/include/geometry_msgs/TransformStamped.h new file mode 100644 index 0000000..c983d97 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/TransformStamped.h @@ -0,0 +1,24 @@ +#ifndef C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H +#define C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Transform.h" + +typedef struct +{ + Header header; + char *child_frame_id; + Transform transform; +} TransformStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Twist.h b/src/APIs/c_api/include/geometry_msgs/Twist.h new file mode 100644 index 0000000..93cc3df --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Twist.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_TWIST_H +#define C_API_GEOMETRY_MSGS_TWIST_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Vector3 linear; + Vector3 angular; +} Twist; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TWIST_H diff --git a/src/APIs/c_api/include/geometry_msgs/TwistStamped.h b/src/APIs/c_api/include/geometry_msgs/TwistStamped.h new file mode 100644 index 0000000..4161520 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/TwistStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_TWISTSTAMPED_H +#define C_API_GEOMETRY_MSGS_TWISTSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Twist.h" + +typedef struct +{ + Header header; + Twist twist; +} TwistStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TWISTSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h b/src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h new file mode 100644 index 0000000..6711fae --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H +#define C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Twist.h" + +typedef struct +{ + Twist twist; + double *covariance; + size_t covariance_count; +} TwistWithCovariance; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H diff --git a/src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h b/src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h new file mode 100644 index 0000000..c70a1ee --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H +#define C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/TwistWithCovariance.h" + +typedef struct +{ + Header header; + TwistWithCovariance twist; +} TwistWithCovarianceStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Vector3.h b/src/APIs/c_api/include/geometry_msgs/Vector3.h new file mode 100644 index 0000000..65e14e9 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Vector3.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_VECTOR3_H +#define C_API_GEOMETRY_MSGS_VECTOR3_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double z; +} Vector3; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_VECTOR3_H diff --git a/src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h b/src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h new file mode 100644 index 0000000..4f3b729 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H +#define C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Header header; + Vector3 vector; +} Vector3Stamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Wrench.h b/src/APIs/c_api/include/geometry_msgs/Wrench.h new file mode 100644 index 0000000..f8bd8e3 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Wrench.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_WRENCH_H +#define C_API_GEOMETRY_MSGS_WRENCH_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Vector3 force; + Vector3 torque; +} Wrench; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_WRENCH_H diff --git a/src/APIs/c_api/include/geometry_msgs/WrenchStamped.h b/src/APIs/c_api/include/geometry_msgs/WrenchStamped.h new file mode 100644 index 0000000..70bdf1f --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/WrenchStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H +#define C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Wrench.h" + +typedef struct +{ + Header header; + Wrench wrench; +} WrenchStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H diff --git a/src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h b/src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h new file mode 100644 index 0000000..61e0947 --- /dev/null +++ b/src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h @@ -0,0 +1,33 @@ +#ifndef C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H +#define C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +/** + * C representation of robot_map_msgs::OccupancyGridUpdate. + * Note: This header is intentionally named *_c.h to avoid + * shadowing the C++ header `robot_map_msgs/OccupancyGridUpdate.h`. + */ +typedef struct +{ + Header header; + int32_t x; + int32_t y; + uint32_t width; + uint32_t height; + int8_t *data; + size_t data_count; +} OccupancyGridUpdate; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H + diff --git a/src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h b/src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h new file mode 100644 index 0000000..6494317 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H +#define C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "nav_2d_msgs/Polygon2D.h" + +typedef struct +{ + Polygon2D outer; + Polygon2D *inner; + size_t inner_count; +} ComplexPolygon2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h new file mode 100644 index 0000000..570a695 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDINFO_H +#define C_API_NAV_2D_MSGS_NAVGRIDINFO_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + uint32_t width; + uint32_t height; + double resolution; + char *frame_id; + double origin_x; + double origin_y; +} NavGridInfo; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDINFO_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h new file mode 100644 index 0000000..00b5d4d --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H +#define C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Time.h" +#include "nav_2d_msgs/NavGridInfo.h" + +typedef struct +{ + Time stamp; + NavGridInfo info; + uint8_t *data; + size_t data_count; +} NavGridOfChars; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h new file mode 100644 index 0000000..2080ab7 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H +#define C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Time.h" +#include "nav_2d_msgs/UIntBounds.h" + +typedef struct +{ + Time stamp; + UIntBounds bounds; + uint8_t *data; + size_t data_count; +} NavGridOfCharsUpdate; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h new file mode 100644 index 0000000..0d46911 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H +#define C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Time.h" +#include "nav_2d_msgs/NavGridInfo.h" + +typedef struct +{ + Time stamp; + NavGridInfo info; + double *data; + size_t data_count; +} NavGridOfDoubles; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h new file mode 100644 index 0000000..6b0facc --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H +#define C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Time.h" +#include "nav_2d_msgs/UIntBounds.h" + +typedef struct +{ + Time stamp; + UIntBounds bounds; + double *data; + size_t data_count; +} NavGridOfDoublesUpdate; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Path2D.h b/src/APIs/c_api/include/nav_2d_msgs/Path2D.h new file mode 100644 index 0000000..57648ba --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Path2D.h @@ -0,0 +1,24 @@ +#ifndef C_API_NAV_2D_MSGS_PATH2D_H +#define C_API_NAV_2D_MSGS_PATH2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_2d_msgs/Pose2DStamped.h" + +typedef struct +{ + Header header; + Pose2DStamped *poses; + size_t poses_count; +} Path2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_PATH2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Point2D.h b/src/APIs/c_api/include/nav_2d_msgs/Point2D.h new file mode 100644 index 0000000..4359592 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Point2D.h @@ -0,0 +1,21 @@ +#ifndef C_API_NAV_2D_MSGS_POINT2D_H +#define C_API_NAV_2D_MSGS_POINT2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; +} Point2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POINT2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h b/src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h new file mode 100644 index 0000000..8cb0602 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h @@ -0,0 +1,22 @@ +#ifndef C_API_NAV_2D_MSGS_POLYGON2D_H +#define C_API_NAV_2D_MSGS_POLYGON2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "nav_2d_msgs/Point2D.h" + +typedef struct +{ + Point2D *points; + size_t points_count; +} Polygon2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POLYGON2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h b/src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h new file mode 100644 index 0000000..d4f0e93 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h @@ -0,0 +1,27 @@ +#ifndef C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H +#define C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "std_msgs/ColorRGBA.h" +#include "nav_2d_msgs/ComplexPolygon2D.h" + +typedef struct +{ + Header header; + ComplexPolygon2D *polygons; + size_t polygons_count; + ColorRGBA *colors; + size_t colors_count; +} Polygon2DCollection; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h b/src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h new file mode 100644 index 0000000..de7e011 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H +#define C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_2d_msgs/Polygon2D.h" + +typedef struct +{ + Header header; + Polygon2D polygon; +} Polygon2DStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h b/src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h new file mode 100644 index 0000000..c4b8107 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h @@ -0,0 +1,22 @@ +#ifndef C_API_NAV_2D_MSGS_POSE2D32_H +#define C_API_NAV_2D_MSGS_POSE2D32_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + float x; + float y; + float theta; +} Pose2D32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POSE2D32_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h b/src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h new file mode 100644 index 0000000..6ffbef3 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_POSE2DSTAMPED_H +#define C_API_NAV_2D_MSGS_POSE2DSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Pose2D.h" + +typedef struct +{ + Header header; + Pose2D pose; +} Pose2DStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POSE2DSTAMPED_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h b/src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h new file mode 100644 index 0000000..69258c4 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGIN_H +#define C_API_NAV_2D_MSGS_SWITCHPLUGIN_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "nav_2d_msgs/SwitchPluginRequest.h" +#include "nav_2d_msgs/SwitchPluginResponse.h" + +typedef struct +{ + SwitchPluginRequest request; + SwitchPluginResponse response; +} SwitchPlugin; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_SWITCHPLUGIN_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h b/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h new file mode 100644 index 0000000..8ac217f --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h @@ -0,0 +1,20 @@ +#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H +#define C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *new_plugin; +} SwitchPluginRequest; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginResponse.h b/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginResponse.h new file mode 100644 index 0000000..deaace6 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginResponse.h @@ -0,0 +1,21 @@ +#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H +#define C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + uint8_t success; + char *message; +} SwitchPluginResponse; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Twist2D.h b/src/APIs/c_api/include/nav_2d_msgs/Twist2D.h new file mode 100644 index 0000000..9125feb --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Twist2D.h @@ -0,0 +1,22 @@ +#ifndef C_API_NAV_2D_MSGS_TWIST2D_H +#define C_API_NAV_2D_MSGS_TWIST2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double theta; +} Twist2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_TWIST2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Twist2D32.h b/src/APIs/c_api/include/nav_2d_msgs/Twist2D32.h new file mode 100644 index 0000000..e42a448 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Twist2D32.h @@ -0,0 +1,22 @@ +#ifndef C_API_NAV_2D_MSGS_TWIST2D32_H +#define C_API_NAV_2D_MSGS_TWIST2D32_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + float x; + float y; + float theta; +} Twist2D32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_TWIST2D32_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Twist2DStamped.h b/src/APIs/c_api/include/nav_2d_msgs/Twist2DStamped.h new file mode 100644 index 0000000..a74c638 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Twist2DStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_TWIST2DSTAMPED_H +#define C_API_NAV_2D_MSGS_TWIST2DSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_2d_msgs/Twist2D.h" + +typedef struct +{ + Header header; + Twist2D velocity; +} Twist2DStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_TWIST2DSTAMPED_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/UIntBounds.h b/src/APIs/c_api/include/nav_2d_msgs/UIntBounds.h new file mode 100644 index 0000000..ea094fd --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/UIntBounds.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_UINTBOUNDS_H +#define C_API_NAV_2D_MSGS_UINTBOUNDS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + uint32_t min_x; + uint32_t min_y; + uint32_t max_x; + uint32_t max_y; +} UIntBounds; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_UINTBOUNDS_H diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index 2fdc4fa..0b4c51b 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/src/APIs/c_api/include/nav_c_api.h @@ -6,154 +6,58 @@ extern "C" { #endif -#include -#include -#include + #include + #include + #include + /* C struct types only - do not include convertor.h here (it pulls C++ code into extern "C") */ + #include "std_msgs/Header.h" + #include "geometry_msgs/Point.h" + #include "geometry_msgs/Pose2D.h" + #include "geometry_msgs/PoseStamped.h" + #include "nav_msgs/OccupancyGrid.h" + #include "nav_msgs/Odometry.h" + #include "map_msgs/OccupancyGridUpdate.h" + #include "nav_2d_msgs/Twist2D.h" + #include "nav_2d_msgs/Twist2DStamped.h" + #include "nav_2d_msgs/Path2D.h" + #include "sensor_msgs/LaserScan.h" + #include "sensor_msgs/PointCloud.h" + #include "sensor_msgs/PointCloud2.h" + #include "geometry_msgs/PolygonStamped.h" - // Forward declarations - typedef void *NavigationHandle; - typedef void *TFListenerHandle; - typedef void *OccupancyGridHandle; - typedef void *OccupancyGridUpdateHandle; - typedef void *LaserScanHandle; - typedef void *PointCloudHandle; - typedef void *PointCloud2Handle; - typedef void *OdometryHandle; - typedef void *Path2DHandle; - typedef void *PolygonStampedHandle; - typedef void *OrderHandle; + 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; - // ============================================================================ - // Enums - // ============================================================================ - - /** - * @brief Navigation states, including planning and controller status - */ + typedef struct + { + void *ptr; + } NavigationHandle; + + typedef struct + { + void *ptr; + } TFListenerHandle; + typedef enum { - NAV_STATE_PENDING = 0, - NAV_STATE_ACTIVE = 1, - NAV_STATE_PREEMPTED = 2, - NAV_STATE_SUCCEEDED = 3, - NAV_STATE_ABORTED = 4, - NAV_STATE_REJECTED = 5, - NAV_STATE_PREEMPTING = 6, - NAV_STATE_RECALLING = 7, - NAV_STATE_RECALLED = 8, - NAV_STATE_LOST = 9, - NAV_STATE_PLANNING = 10, - NAV_STATE_CONTROLLING = 11, - NAV_STATE_CLEARING = 12, - NAV_STATE_PAUSED = 13 + 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 } NavigationState; - - // ============================================================================ - // Structures - // ============================================================================ - - /** - * @brief Point structure (x, y, z) - */ - typedef struct - { - double x; - double y; - double z; - } Point; - - /** - * @brief Pose2D structure (x, y, theta) - */ - typedef struct - { - double x; - double y; - double theta; - } Pose2D; - - /** - * @brief Twist2D structure (x, y, theta velocities) - */ - typedef struct - { - double x; - double y; - double theta; - } Twist2D; - - /** - * @brief Quaternion structure - */ - typedef struct - { - double x; - double y; - double z; - double w; - } Quaternion; - - /** - * @brief Position structure - */ - typedef struct - { - double x; - double y; - double z; - } Position; - - /** - * @brief Pose structure - */ - typedef struct - { - Position position; - Quaternion orientation; - } Pose; - - /** - * @brief Header structure - */ - typedef struct - { - uint32_t seq; - int64_t sec; - uint32_t nsec; - char *frame_id; - } Header; - - /** - * @brief PoseStamped structure - */ - typedef struct - { - Header header; - Pose pose; - } PoseStamped; - - /** - * @brief Twist2DStamped structure - */ - typedef struct - { - Header header; - Twist2D velocity; - } Twist2DStamped; - - /** - * @brief Vector3 structure - */ - typedef struct - { - double x; - double y; - double z; - } Vector3; - - /** - * @brief Navigation feedback structure - */ + typedef struct { NavigationState navigation_state; @@ -164,201 +68,24 @@ extern "C" } NavFeedback; /** - * @brief Named OccupancyGrid structure - * @note map is an opaque handle to a C++ robot_nav_msgs::OccupancyGrid + * @brief Planner data output structure (C version). + * Mirrors robot::move_base_core::PlannerDataOutput in `move_base_core/navigation.h`. */ typedef struct { - char *name; - OccupancyGridHandle map; - } NamedOccupancyGrid; - - /** - * @brief Named LaserScan structure - * @note scan is an opaque handle to a C++ robot_sensor_msgs::LaserScan - */ - typedef struct - { - char *name; - LaserScanHandle scan; - } NamedLaserScan; - - /** - * @brief Named PointCloud structure - * @note cloud is an opaque handle to a C++ robot_sensor_msgs::PointCloud - */ - typedef struct - { - char *name; - PointCloudHandle cloud; - } NamedPointCloud; - - /** - * @brief Named PointCloud2 structure - * @note cloud is an opaque handle to a C++ robot_sensor_msgs::PointCloud2 - */ - typedef struct - { - char *name; - PointCloud2Handle cloud; - } NamedPointCloud2; - - /** - * @brief Planner data output structure (opaque message handles) - */ - typedef struct - { - Path2DHandle plan; - OccupancyGridHandle costmap; - OccupancyGridUpdateHandle costmap_update; + Path2D plan; + OccupancyGrid costmap; + OccupancyGridUpdate costmap_update; bool is_costmap_updated; - PolygonStampedHandle footprint; + PolygonStamped footprint; } PlannerDataOutput; - // ============================================================================ - // String Management - // ============================================================================ - /** - * @brief Free a string allocated by the library - * @param str String to free + * @brief Free a string allocated by the API (e.g. feed_back_str, navigation_state_to_string result). + * @param str Pointer from strdup; no-op if NULL. */ void nav_c_api_free_string(char *str); - // ============================================================================ - // Complex Message Handle Management - // ============================================================================ - - /** - * @brief Free an occupancy grid handle - * @param handle Occupancy grid handle to free - */ - void navigation_free_occupancy_grid(OccupancyGridHandle handle); - /** - * @brief Free an occupancy grid update handle - * @param handle Occupancy grid update handle to free - */ - void navigation_free_occupancy_grid_update(OccupancyGridUpdateHandle handle); - - /** - * @brief Free a laser scan handle - * @param handle Laser scan handle to free - */ - void navigation_free_laser_scan(LaserScanHandle handle); - - /** - * @brief Free a point cloud handle - * @param handle Point cloud handle to free - */ - void navigation_free_point_cloud(PointCloudHandle handle); - - /** - * @brief Free a point cloud2 handle - * @param handle Point cloud2 handle to free - */ - void navigation_free_point_cloud2(PointCloud2Handle handle); - - /** - * @brief Free an odometry handle - * @param handle Odometry handle to free - */ - void navigation_free_odometry(OdometryHandle handle); - - /** - * @brief Free a path2d handle - * @param handle Path2d handle to free - */ - void navigation_free_path2d(Path2DHandle handle); - - /** - * @brief Free a polygon stamped handle - * @param handle Polygon stamped handle to free - */ - void navigation_free_polygon_stamped(PolygonStampedHandle handle); - - /** - * @brief Free an order handle - * @param handle Order handle to free - */ - void navigation_free_order(OrderHandle handle); - - /** - * @brief Free an array of named occupancy grids - * @param maps Array of named occupancy grids to free - * @param count Number of named occupancy grids in the array - */ - void navigation_free_named_occupancy_grids(NamedOccupancyGrid *maps, size_t count); - - /** - * @brief Free an array of named laser scans - * @param scans Array of named laser scans to free - * @param count Number of named laser scans in the array - */ - void navigation_free_named_laser_scans(NamedLaserScan *scans, size_t count); - - /** - * @brief Free an array of named point clouds - * @param clouds Array of named point clouds to free - * @param count Number of named point clouds in the array - */ - void navigation_free_named_point_clouds(NamedPointCloud *clouds, size_t count); - - /** - * @brief Free an array of named point cloud2s - * @param clouds Array of named point cloud2s to free - * @param count Number of named point cloud2s in the array - */ - void navigation_free_named_point_cloud2s(NamedPointCloud2 *clouds, size_t count); - - /** - * @brief Free a planner data output - * @param data Planner data output to free - */ - void navigation_free_planner_data(PlannerDataOutput *data); - - // ============================================================================ - // State Conversion - // ============================================================================ - - /** - * @brief Convert a State enum to its string representation - * @param state Enum value of NavigationState - * @return String representation (caller must free with nav_c_api_free_string) - */ - char *navigation_state_to_string(NavigationState state); - - // ============================================================================ - // Helper Functions - // ============================================================================ - - /** - * @brief Creates a target pose by offsetting a given 2D pose along its heading direction - * @param pose_x X coordinate of the original pose - * @param pose_y Y coordinate of the original pose - * @param pose_theta Heading angle in radians - * @param frame_id The coordinate frame ID (null-terminated string) - * @param offset_distance Distance to offset along heading (positive = forward, negative = backward) - * @param out_goal Output parameter for the offset pose - * @return true on success, false on failure - */ - bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta, - const char *frame_id, double offset_distance, - PoseStamped *out_goal); - - /** - * @brief Creates an offset target pose from a given PoseStamped - * @param in_pose Input pose - * @param offset_distance Distance to offset along heading direction - * @param out_goal Output parameter for the offset pose - * @return true on success, false on failure - */ - bool navigation_offset_goal_stamped(const PoseStamped *in_pose, double offset_distance, - PoseStamped *out_goal); - - // ============================================================================ - // Navigation Handle Management - // ============================================================================ - /** * @brief Create a new navigation instance * @return Navigation handle, or NULL on failure @@ -371,10 +98,6 @@ extern "C" */ void navigation_destroy(NavigationHandle handle); - // ============================================================================ - // TF Listener Management - // ============================================================================ - /** * @brief Create a TF listener instance * @return TF listener handle, or NULL on failure @@ -439,13 +162,8 @@ extern "C" * @param out_count Output number of points in the array * @return true on success, false on failure */ - bool navigation_get_robot_footprint(NavigationHandle handle, Point **out_points, size_t *out_count); + bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count); - /** - * @brief Free a points array allocated by navigation_get_robot_footprint - * @param points Pointer to point array - */ - void navigation_free_points(Point *points); /** * @brief Send a goal for the robot to navigate to @@ -455,21 +173,21 @@ extern "C" * @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, + bool navigation_move_to(NavigationHandle handle, 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 - * @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 + // * @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 docking goal to a predefined marker @@ -481,21 +199,21 @@ extern "C" * @return true if docking command succeeded */ bool navigation_dock_to(NavigationHandle handle, const char *marker, - const PoseStamped *goal, + 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 - * @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 + // * @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 Move straight toward the target position @@ -504,7 +222,7 @@ extern "C" * @param xy_goal_tolerance Acceptable positional error (meters) * @return true if command issued successfully */ - bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped *goal, + bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped goal, double xy_goal_tolerance); /** @@ -514,7 +232,7 @@ extern "C" * @param yaw_goal_tolerance Acceptable angular error (radians) * @return true if rotation command was sent successfully */ - bool navigation_rotate_to(NavigationHandle handle, const PoseStamped *goal, + bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal, double yaw_goal_tolerance); /** @@ -563,7 +281,7 @@ extern "C" * @param out_pose Output parameter with the robot's current pose * @return true if pose was successfully retrieved */ - bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped *out_pose); + bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose); /** * @brief Get the robot's pose as a 2D pose @@ -571,7 +289,7 @@ extern "C" * @param out_pose Output parameter with the robot's current 2D pose * @return true if pose was successfully retrieved */ - bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D *out_pose); + bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose); /** * @brief Get the robot's current twist @@ -580,7 +298,7 @@ extern "C" * @return true if twist was successfully retrieved * @note out_twist->header.frame_id must be freed using nav_c_api_free_string */ - bool navigation_get_twist(NavigationHandle handle, Twist2DStamped *out_twist); + bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist); /** * @brief Get navigation feedback @@ -589,13 +307,7 @@ extern "C" * @return true if feedback was successfully retrieved * @note The feed_back_str field must be freed using nav_c_api_free_string */ - bool navigation_get_feedback(NavigationHandle handle, NavFeedback *out_feedback); - - /** - * @brief Free navigation feedback structure - * @param feedback Feedback structure to free - */ - void navigation_free_feedback(NavFeedback *feedback); + bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback); /** * @brief Add a static map to the navigation system @@ -604,7 +316,7 @@ extern "C" * @param map Occupancy grid handle * @return true if the map was added successfully */ - bool navigation_add_static_map(NavigationHandle handle, const char *map_name, OccupancyGridHandle map); + bool navigation_add_static_map(NavigationHandle handle, const char *map_name, const OccupancyGrid occupancy_grid); /** * @brief Add a laser scan to the navigation system @@ -613,7 +325,7 @@ extern "C" * @param laser_scan Laser scan handle * @return true if the laser scan was added successfully */ - bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScanHandle laser_scan); + bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, const LaserScan laser_scan); /** * @brief Add a point cloud to the navigation system @@ -622,7 +334,7 @@ extern "C" * @param point_cloud Point cloud handle * @return true if the point cloud was added successfully */ - bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloudHandle point_cloud); + bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud); /** * @brief Add a point cloud2 to the navigation system @@ -631,7 +343,7 @@ extern "C" * @param point_cloud2 Point cloud2 handle * @return true if the point cloud2 was added successfully */ - bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2Handle point_cloud2); + bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2); /** * @brief Add an odometry to the navigation system @@ -640,7 +352,7 @@ extern "C" * @param odometry Odometry handle * @return true if the odometry was added successfully */ - bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, OdometryHandle odometry); + bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry); /** * @brief Get a static map from the navigation system @@ -649,7 +361,7 @@ extern "C" * @param out_map Output parameter for the map handle * @return true if the map was retrieved successfully */ - bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGridHandle *out_map); + bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map); /** * @brief Get a laser scan from the navigation system @@ -658,7 +370,7 @@ extern "C" * @param out_scan Output parameter for the laser scan handle * @return true if the laser scan was retrieved successfully */ - bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScanHandle *out_scan); + bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan); /** * @brief Get a point cloud from the navigation system @@ -667,7 +379,7 @@ extern "C" * @param out_cloud Output parameter for the point cloud handle * @return true if the point cloud was retrieved successfully */ - bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloudHandle *out_cloud); + bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud); /** * @brief Get a point cloud2 from the navigation system @@ -676,7 +388,7 @@ extern "C" * @param out_cloud Output parameter for the point cloud2 handle * @return true if the point cloud2 was retrieved successfully */ - bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2Handle *out_cloud); + bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud); /** * @brief Get all static maps from the navigation system @@ -685,7 +397,7 @@ extern "C" * @param out_count Output parameter for the number of maps * @return true if the maps were retrieved successfully */ - bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid **out_maps, size_t *out_count); + bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count); /** * @brief Get all laser scans from the navigation system @@ -694,7 +406,7 @@ extern "C" * @param out_count Output parameter for the number of scans * @return true if the scans were retrieved successfully */ - bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan **out_scans, size_t *out_count); + bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count); /** * @brief Get all point clouds from the navigation system @@ -703,7 +415,7 @@ extern "C" * @param out_count Output parameter for the number of clouds * @return true if the clouds were retrieved successfully */ - bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud **out_clouds, size_t *out_count); + bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count); /** * @brief Get all point cloud2s from the navigation system @@ -712,7 +424,7 @@ extern "C" * @param out_count Output parameter for the number of clouds * @return true if the clouds were retrieved successfully */ - bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 **out_clouds, size_t *out_count); + bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count); /** * @brief Remove a static map from the navigation system diff --git a/src/APIs/c_api/include/nav_msgs/ActionTypes.h b/src/APIs/c_api/include/nav_msgs/ActionTypes.h new file mode 100644 index 0000000..325800b --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/ActionTypes.h @@ -0,0 +1,29 @@ + #ifndef C_API_NAV_MSGS_ACTIONTYPES_H + #define C_API_NAV_MSGS_ACTIONTYPES_H + + #ifdef __cplusplus + extern "C" { + #endif + + #include + #include + #include "std_msgs/Time.h" + + typedef struct + { + Time stamp; + char *id; + } GoalID; + + typedef struct + { + GoalID goal_id; + uint8_t status; + char *text; + } GoalStatus; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_ACTIONTYPES_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMap.h b/src/APIs/c_api/include/nav_msgs/GetMap.h new file mode 100644 index 0000000..dc9fc25 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMap.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_GETMAP_H + #define C_API_NAV_MSGS_GETMAP_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/GetMapRequest.h" +#include "nav_msgs/GetMapResponse.h" + +typedef struct +{ + GetMapRequest request; + GetMapResponse response; +} GetMap; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAP_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapAction.h b/src/APIs/c_api/include/nav_msgs/GetMapAction.h new file mode 100644 index 0000000..8a7321e --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapAction.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_GETMAPACTION_H + #define C_API_NAV_MSGS_GETMAPACTION_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/GetMapActionGoal.h" +#include "nav_msgs/GetMapActionResult.h" +#include "nav_msgs/GetMapActionFeedback.h" + +typedef struct +{ + GetMapActionGoal action_goal; + GetMapActionResult action_result; + GetMapActionFeedback action_feedback; +} GetMapAction; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPACTION_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapActionFeedback.h b/src/APIs/c_api/include/nav_msgs/GetMapActionFeedback.h new file mode 100644 index 0000000..cfb8091 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapActionFeedback.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H + #define C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_msgs/ActionTypes.h" +#include "nav_msgs/GetMapFeedback.h" + +typedef struct +{ + Header header; + GoalStatus status; + GetMapFeedback feedback; +} GetMapActionFeedback; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapActionGoal.h b/src/APIs/c_api/include/nav_msgs/GetMapActionGoal.h new file mode 100644 index 0000000..27e6926 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapActionGoal.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_GETMAPACTIONGOAL_H + #define C_API_NAV_MSGS_GETMAPACTIONGOAL_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_msgs/ActionTypes.h" +#include "nav_msgs/GetMapGoal.h" + +typedef struct +{ + Header header; + GoalID goal_id; + GetMapGoal goal; +} GetMapActionGoal; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPACTIONGOAL_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapActionResult.h b/src/APIs/c_api/include/nav_msgs/GetMapActionResult.h new file mode 100644 index 0000000..6068a0d --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapActionResult.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_GETMAPACTIONRESULT_H + #define C_API_NAV_MSGS_GETMAPACTIONRESULT_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_msgs/ActionTypes.h" +#include "nav_msgs/GetMapResult.h" + +typedef struct +{ + Header header; + GoalStatus status; + GetMapResult result; +} GetMapActionResult; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPACTIONRESULT_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapFeedback.h b/src/APIs/c_api/include/nav_msgs/GetMapFeedback.h new file mode 100644 index 0000000..818d346 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapFeedback.h @@ -0,0 +1,20 @@ + #ifndef C_API_NAV_MSGS_GETMAPFEEDBACK_H + #define C_API_NAV_MSGS_GETMAPFEEDBACK_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include + +typedef struct +{ + uint8_t _dummy; +} GetMapFeedback; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPFEEDBACK_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapGoal.h b/src/APIs/c_api/include/nav_msgs/GetMapGoal.h new file mode 100644 index 0000000..4af2dc8 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapGoal.h @@ -0,0 +1,20 @@ + #ifndef C_API_NAV_MSGS_GETMAPGOAL_H + #define C_API_NAV_MSGS_GETMAPGOAL_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include + +typedef struct +{ + uint8_t _dummy; +} GetMapGoal; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPGOAL_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapRequest.h b/src/APIs/c_api/include/nav_msgs/GetMapRequest.h new file mode 100644 index 0000000..cc53cf1 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapRequest.h @@ -0,0 +1,20 @@ + #ifndef C_API_NAV_MSGS_GETMAPREQUEST_H + #define C_API_NAV_MSGS_GETMAPREQUEST_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include + +typedef struct +{ + uint8_t _dummy; +} GetMapRequest; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPREQUEST_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapResponse.h b/src/APIs/c_api/include/nav_msgs/GetMapResponse.h new file mode 100644 index 0000000..e00c11f --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapResponse.h @@ -0,0 +1,21 @@ + #ifndef C_API_NAV_MSGS_GETMAPRESPONSE_H + #define C_API_NAV_MSGS_GETMAPRESPONSE_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/OccupancyGrid.h" + +typedef struct +{ + OccupancyGrid map; +} GetMapResponse; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPRESPONSE_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapResult.h b/src/APIs/c_api/include/nav_msgs/GetMapResult.h new file mode 100644 index 0000000..011ff51 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapResult.h @@ -0,0 +1,21 @@ + #ifndef C_API_NAV_MSGS_GETMAPRESULT_H + #define C_API_NAV_MSGS_GETMAPRESULT_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/OccupancyGrid.h" + +typedef struct +{ + OccupancyGrid map; +} GetMapResult; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPRESULT_H diff --git a/src/APIs/c_api/include/nav_msgs/GetPlan.h b/src/APIs/c_api/include/nav_msgs/GetPlan.h new file mode 100644 index 0000000..a1830c4 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetPlan.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_GETPLAN_H + #define C_API_NAV_MSGS_GETPLAN_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/GetPlanRequest.h" +#include "nav_msgs/GetPlanResponse.h" + +typedef struct +{ + GetPlanRequest request; + GetPlanResponse response; +} GetPlan; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETPLAN_H diff --git a/src/APIs/c_api/include/nav_msgs/GetPlanRequest.h b/src/APIs/c_api/include/nav_msgs/GetPlanRequest.h new file mode 100644 index 0000000..0ba5658 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetPlanRequest.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_GETPLANREQUEST_H + #define C_API_NAV_MSGS_GETPLANREQUEST_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "geometry_msgs/PoseStamped.h" + +typedef struct +{ + PoseStamped start; + PoseStamped goal; + float tolerance; +} GetPlanRequest; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETPLANREQUEST_H diff --git a/src/APIs/c_api/include/nav_msgs/GetPlanResponse.h b/src/APIs/c_api/include/nav_msgs/GetPlanResponse.h new file mode 100644 index 0000000..2215632 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetPlanResponse.h @@ -0,0 +1,21 @@ + #ifndef C_API_NAV_MSGS_GETPLANRESPONSE_H + #define C_API_NAV_MSGS_GETPLANRESPONSE_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/Path.h" + +typedef struct +{ + Path plan; +} GetPlanResponse; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETPLANRESPONSE_H diff --git a/src/APIs/c_api/include/nav_msgs/GridCells.h b/src/APIs/c_api/include/nav_msgs/GridCells.h new file mode 100644 index 0000000..ed7f331 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GridCells.h @@ -0,0 +1,26 @@ + #ifndef C_API_NAV_MSGS_GRIDCELLS_H + #define C_API_NAV_MSGS_GRIDCELLS_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" + +typedef struct +{ + Header header; + float cell_width; + float cell_height; + Point *cells; + size_t cells_count; +} GridCells; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GRIDCELLS_H diff --git a/src/APIs/c_api/include/nav_msgs/LoadMap.h b/src/APIs/c_api/include/nav_msgs/LoadMap.h new file mode 100644 index 0000000..8cb1b90 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/LoadMap.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_LOADMAP_H + #define C_API_NAV_MSGS_LOADMAP_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/LoadMapRequest.h" +#include "nav_msgs/LoadMapResponse.h" + +typedef struct +{ + LoadMapRequest request; + LoadMapResponse response; +} LoadMap; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_LOADMAP_H diff --git a/src/APIs/c_api/include/nav_msgs/LoadMapRequest.h b/src/APIs/c_api/include/nav_msgs/LoadMapRequest.h new file mode 100644 index 0000000..f3735c8 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/LoadMapRequest.h @@ -0,0 +1,20 @@ + #ifndef C_API_NAV_MSGS_LOADMAPREQUEST_H + #define C_API_NAV_MSGS_LOADMAPREQUEST_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include + +typedef struct +{ + char *map_url; +} LoadMapRequest; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_LOADMAPREQUEST_H diff --git a/src/APIs/c_api/include/nav_msgs/LoadMapResponse.h b/src/APIs/c_api/include/nav_msgs/LoadMapResponse.h new file mode 100644 index 0000000..699a780 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/LoadMapResponse.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_LOADMAPRESPONSE_H + #define C_API_NAV_MSGS_LOADMAPRESPONSE_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include +#include "nav_msgs/OccupancyGrid.h" + +typedef struct +{ + OccupancyGrid map; + bool result; +} LoadMapResponse; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_LOADMAPRESPONSE_H diff --git a/src/APIs/c_api/include/nav_msgs/MapMetaData.h b/src/APIs/c_api/include/nav_msgs/MapMetaData.h new file mode 100644 index 0000000..90ba28a --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/MapMetaData.h @@ -0,0 +1,26 @@ + #ifndef C_API_NAV_MSGS_MAPMETADATA_H + #define C_API_NAV_MSGS_MAPMETADATA_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Time.h" +#include "geometry_msgs/Pose.h" + +typedef struct +{ + Time map_load_time; + float resolution; + uint32_t width; + uint32_t height; + Pose origin; +} MapMetaData; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_MAPMETADATA_H diff --git a/src/APIs/c_api/include/nav_msgs/OccupancyGrid.h b/src/APIs/c_api/include/nav_msgs/OccupancyGrid.h new file mode 100644 index 0000000..eae5454 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/OccupancyGrid.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_OCCUPANCYGRID_H + #define C_API_NAV_MSGS_OCCUPANCYGRID_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_msgs/MapMetaData.h" + +typedef struct +{ + Header header; + MapMetaData info; + int8_t *data; + size_t data_count; +} OccupancyGrid; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_OCCUPANCYGRID_H diff --git a/src/APIs/c_api/include/nav_msgs/Odometry.h b/src/APIs/c_api/include/nav_msgs/Odometry.h new file mode 100644 index 0000000..972a1d9 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/Odometry.h @@ -0,0 +1,35 @@ + #ifndef C_API_NAV_MSGS_ODOMETRY_H + #define C_API_NAV_MSGS_ODOMETRY_H + + #ifdef __cplusplus + extern "C" { + #endif + + #include + #include + #include "std_msgs/Header.h" + #include "geometry_msgs/PoseWithCovariance.h" + #include "geometry_msgs/TwistWithCovariance.h" + + typedef struct + { + Header header; + char *child_frame_id; + PoseWithCovariance pose; + TwistWithCovariance twist; + } Odometry; + + + extern "C" Odometry odometry_create(void); + + extern "C" Odometry odometry_set_data( + Header header, + char *child_frame_id, + PoseWithCovariance pose, + TwistWithCovariance twist); + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_ODOMETRY_H diff --git a/src/APIs/c_api/include/nav_msgs/Path.h b/src/APIs/c_api/include/nav_msgs/Path.h new file mode 100644 index 0000000..53b6a45 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/Path.h @@ -0,0 +1,24 @@ + #ifndef C_API_NAV_MSGS_PATH_H + #define C_API_NAV_MSGS_PATH_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/PoseStamped.h" + +typedef struct +{ + Header header; + PoseStamped *poses; + size_t poses_count; +} Path; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_PATH_H diff --git a/src/APIs/c_api/include/nav_msgs/SetMap.h b/src/APIs/c_api/include/nav_msgs/SetMap.h new file mode 100644 index 0000000..aa76a7e --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/SetMap.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_SETMAP_H + #define C_API_NAV_MSGS_SETMAP_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/SetMapRequest.h" +#include "nav_msgs/SetMapResponse.h" + +typedef struct +{ + SetMapRequest request; + SetMapResponse response; +} SetMap; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_SETMAP_H diff --git a/src/APIs/c_api/include/nav_msgs/SetMapRequest.h b/src/APIs/c_api/include/nav_msgs/SetMapRequest.h new file mode 100644 index 0000000..284372c --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/SetMapRequest.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_SETMAPREQUEST_H + #define C_API_NAV_MSGS_SETMAPREQUEST_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/OccupancyGrid.h" +#include "geometry_msgs/PoseWithCovariance.h" + +typedef struct +{ + OccupancyGrid map; + PoseWithCovariance initial_pose; +} SetMapRequest; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_SETMAPREQUEST_H diff --git a/src/APIs/c_api/include/nav_msgs/SetMapResponse.h b/src/APIs/c_api/include/nav_msgs/SetMapResponse.h new file mode 100644 index 0000000..b5ac1ed --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/SetMapResponse.h @@ -0,0 +1,21 @@ + #ifndef C_API_NAV_MSGS_SETMAPRESPONSE_H + #define C_API_NAV_MSGS_SETMAPRESPONSE_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include + +typedef struct +{ + bool success; +} SetMapResponse; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_SETMAPRESPONSE_H diff --git a/src/APIs/c_api/include/sensor_msgs/BatteryState.h b/src/APIs/c_api/include/sensor_msgs/BatteryState.h new file mode 100644 index 0000000..258b05a --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/BatteryState.h @@ -0,0 +1,38 @@ +#ifndef C_API_SENSOR_MSGS_BATTERYSTATE_H +#define C_API_SENSOR_MSGS_BATTERYSTATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + float voltage; + float current; + float charge; + float capacity; + float design_capacity; + float percentage; + uint8_t power_supply_status; + uint8_t power_supply_health; + uint8_t power_supply_technology; + bool present; + float *cell_voltage; + size_t cell_voltage_count; + float *cell_temperature; + size_t cell_temperature_count; + char *location; + char *serial_number; +} BatteryState; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_BATTERYSTATE_H diff --git a/src/APIs/c_api/include/sensor_msgs/CameraInfo.h b/src/APIs/c_api/include/sensor_msgs/CameraInfo.h new file mode 100644 index 0000000..621e55d --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/CameraInfo.h @@ -0,0 +1,33 @@ +#ifndef C_API_SENSOR_MSGS_CAMERAINFO_H +#define C_API_SENSOR_MSGS_CAMERAINFO_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "sensor_msgs/RegionOfInterest.h" + +typedef struct +{ + Header header; + uint32_t height; + uint32_t width; + char *distortion_model; + double *D; + size_t D_count; + double K[9]; + double R[9]; + double P[12]; + uint32_t binning_x; + uint32_t binning_y; + RegionOfInterest roi; +} CameraInfo; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_CAMERAINFO_H diff --git a/src/APIs/c_api/include/sensor_msgs/ChannelFloat32.h b/src/APIs/c_api/include/sensor_msgs/ChannelFloat32.h new file mode 100644 index 0000000..8a8a782 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/ChannelFloat32.h @@ -0,0 +1,22 @@ +#ifndef C_API_SENSOR_MSGS_CHANNELFLOAT32_H +#define C_API_SENSOR_MSGS_CHANNELFLOAT32_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *name; + float *values; + size_t values_count; +} ChannelFloat32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_CHANNELFLOAT32_H diff --git a/src/APIs/c_api/include/sensor_msgs/CompressedImage.h b/src/APIs/c_api/include/sensor_msgs/CompressedImage.h new file mode 100644 index 0000000..897cdff --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/CompressedImage.h @@ -0,0 +1,24 @@ +#ifndef C_API_SENSOR_MSGS_COMPRESSEDIMAGE_H +#define C_API_SENSOR_MSGS_COMPRESSEDIMAGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + char *format; + uint8_t *data; + size_t data_count; +} CompressedImage; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_COMPRESSEDIMAGE_H diff --git a/src/APIs/c_api/include/sensor_msgs/FluidPressure.h b/src/APIs/c_api/include/sensor_msgs/FluidPressure.h new file mode 100644 index 0000000..bbf8d20 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/FluidPressure.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_FLUIDPRESSURE_H +#define C_API_SENSOR_MSGS_FLUIDPRESSURE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + double fluid_pressure; + double variance; +} FluidPressure; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_FLUIDPRESSURE_H diff --git a/src/APIs/c_api/include/sensor_msgs/Illuminance.h b/src/APIs/c_api/include/sensor_msgs/Illuminance.h new file mode 100644 index 0000000..f77f525 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Illuminance.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_ILLUMINANCE_H +#define C_API_SENSOR_MSGS_ILLUMINANCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + double illuminance; + double variance; +} Illuminance; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_ILLUMINANCE_H diff --git a/src/APIs/c_api/include/sensor_msgs/Image.h b/src/APIs/c_api/include/sensor_msgs/Image.h new file mode 100644 index 0000000..e3f1bc5 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Image.h @@ -0,0 +1,28 @@ +#ifndef C_API_SENSOR_MSGS_IMAGE_H +#define C_API_SENSOR_MSGS_IMAGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + uint32_t height; + uint32_t width; + char *encoding; + uint8_t is_bigendian; + uint32_t step; + uint8_t *data; + size_t data_count; +} Image; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_IMAGE_H diff --git a/src/APIs/c_api/include/sensor_msgs/Imu.h b/src/APIs/c_api/include/sensor_msgs/Imu.h new file mode 100644 index 0000000..78f6a41 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Imu.h @@ -0,0 +1,29 @@ +#ifndef C_API_SENSOR_MSGS_IMU_H +#define C_API_SENSOR_MSGS_IMU_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Header header; + Quaternion orientation; + double orientation_covariance[9]; + Vector3 angular_velocity; + double angular_velocity_covariance[9]; + Vector3 linear_acceleration; + double linear_acceleration_covariance[9]; +} Imu; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_IMU_H diff --git a/src/APIs/c_api/include/sensor_msgs/JointState.h b/src/APIs/c_api/include/sensor_msgs/JointState.h new file mode 100644 index 0000000..0007c17 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/JointState.h @@ -0,0 +1,29 @@ +#ifndef C_API_SENSOR_MSGS_JOINTSTATE_H +#define C_API_SENSOR_MSGS_JOINTSTATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + char **name; + size_t name_count; + double *position; + size_t position_count; + double *velocity; + size_t velocity_count; + double *effort; + size_t effort_count; +} JointState; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_JOINTSTATE_H diff --git a/src/APIs/c_api/include/sensor_msgs/Joy.h b/src/APIs/c_api/include/sensor_msgs/Joy.h new file mode 100644 index 0000000..d2cf043 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Joy.h @@ -0,0 +1,25 @@ +#ifndef C_API_SENSOR_MSGS_JOY_H +#define C_API_SENSOR_MSGS_JOY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + float *axes; + size_t axes_count; + int32_t *buttons; + size_t buttons_count; +} Joy; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_JOY_H diff --git a/src/APIs/c_api/include/sensor_msgs/JoyFeedback.h b/src/APIs/c_api/include/sensor_msgs/JoyFeedback.h new file mode 100644 index 0000000..717081c --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/JoyFeedback.h @@ -0,0 +1,22 @@ +#ifndef C_API_SENSOR_MSGS_JOYFEEDBACK_H +#define C_API_SENSOR_MSGS_JOYFEEDBACK_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + uint8_t type; + uint8_t id; + float intensity; +} JoyFeedback; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_JOYFEEDBACK_H diff --git a/src/APIs/c_api/include/sensor_msgs/JoyFeedbackArray.h b/src/APIs/c_api/include/sensor_msgs/JoyFeedbackArray.h new file mode 100644 index 0000000..6435943 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/JoyFeedbackArray.h @@ -0,0 +1,22 @@ +#ifndef C_API_SENSOR_MSGS_JOYFEEDBACKARRAY_H +#define C_API_SENSOR_MSGS_JOYFEEDBACKARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "sensor_msgs/JoyFeedback.h" + +typedef struct +{ + JoyFeedback *array; + size_t array_count; +} JoyFeedbackArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_JOYFEEDBACKARRAY_H diff --git a/src/APIs/c_api/include/sensor_msgs/LaserEcho.h b/src/APIs/c_api/include/sensor_msgs/LaserEcho.h new file mode 100644 index 0000000..35ce456 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/LaserEcho.h @@ -0,0 +1,21 @@ +#ifndef C_API_SENSOR_MSGS_LASERECHO_H +#define C_API_SENSOR_MSGS_LASERECHO_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + float *echoes; + size_t echoes_count; +} LaserEcho; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_LASERECHO_H diff --git a/src/APIs/c_api/include/sensor_msgs/LaserScan.h b/src/APIs/c_api/include/sensor_msgs/LaserScan.h new file mode 100644 index 0000000..71613b1 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/LaserScan.h @@ -0,0 +1,32 @@ +#ifndef C_API_SENSOR_MSGS_LASER_SCAN_H +#define C_API_SENSOR_MSGS_LASER_SCAN_H + +#ifdef __cplusplus +extern "C" +{ +#endif +#include +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + float angle_min; + float angle_max; + float angle_increment; + float time_increment; + float scan_time; + float range_min; + float range_max; + float *ranges; + size_t ranges_count; + float *intensities; + size_t intensities_count; +} LaserScan; + +#ifdef __cplusplus +} +#endif +#endif // C_API_SENSOR_MSGS_LASER_SCAN_H \ No newline at end of file diff --git a/src/APIs/c_api/include/sensor_msgs/MultiEchoLaserScan.h b/src/APIs/c_api/include/sensor_msgs/MultiEchoLaserScan.h new file mode 100644 index 0000000..774e93e --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/MultiEchoLaserScan.h @@ -0,0 +1,33 @@ +#ifndef C_API_SENSOR_MSGS_MULTIECHOLASERSCAN_H +#define C_API_SENSOR_MSGS_MULTIECHOLASERSCAN_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "sensor_msgs/LaserEcho.h" + +typedef struct +{ + Header header; + float angle_min; + float angle_max; + float angle_increment; + float time_increment; + float scan_time; + float range_min; + float range_max; + LaserEcho *ranges; + size_t ranges_count; + LaserEcho *intensities; + size_t intensities_count; +} MultiEchoLaserScan; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_MULTIECHOLASERSCAN_H diff --git a/src/APIs/c_api/include/sensor_msgs/PointCloud.h b/src/APIs/c_api/include/sensor_msgs/PointCloud.h new file mode 100644 index 0000000..1c2a8d0 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/PointCloud.h @@ -0,0 +1,27 @@ +#ifndef C_API_SENSOR_MSGS_POINTCLOUD_H +#define C_API_SENSOR_MSGS_POINTCLOUD_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Point32.h" +#include "sensor_msgs/ChannelFloat32.h" + +typedef struct +{ + Header header; + Point32 *points; + size_t points_count; + ChannelFloat32 *channels; + size_t channels_count; +} PointCloud; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_POINTCLOUD_H diff --git a/src/APIs/c_api/include/sensor_msgs/PointCloud2.h b/src/APIs/c_api/include/sensor_msgs/PointCloud2.h new file mode 100644 index 0000000..f275eae --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/PointCloud2.h @@ -0,0 +1,33 @@ +#ifndef C_API_SENSOR_MSGS_POINTCLOUD2_H +#define C_API_SENSOR_MSGS_POINTCLOUD2_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include "std_msgs/Header.h" +#include "sensor_msgs/PointField.h" + +typedef struct +{ + Header header; + uint32_t height; + uint32_t width; + PointField *fields; + size_t fields_count; + bool is_bigendian; + uint32_t point_step; + uint32_t row_step; + uint8_t *data; + size_t data_count; + bool is_dense; +} PointCloud2; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_POINTCLOUD2_H diff --git a/src/APIs/c_api/include/sensor_msgs/PointField.h b/src/APIs/c_api/include/sensor_msgs/PointField.h new file mode 100644 index 0000000..569c8ac --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/PointField.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_POINTFIELD_H +#define C_API_SENSOR_MSGS_POINTFIELD_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *name; + uint32_t offset; + uint8_t datatype; + uint32_t count; +} PointField; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_POINTFIELD_H diff --git a/src/APIs/c_api/include/sensor_msgs/Range.h b/src/APIs/c_api/include/sensor_msgs/Range.h new file mode 100644 index 0000000..bfbaa6d --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Range.h @@ -0,0 +1,26 @@ +#ifndef C_API_SENSOR_MSGS_RANGE_H +#define C_API_SENSOR_MSGS_RANGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + uint8_t radiation_type; + float field_of_view; + float min_range; + float max_range; + float range; +} Range; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_RANGE_H diff --git a/src/APIs/c_api/include/sensor_msgs/RegionOfInterest.h b/src/APIs/c_api/include/sensor_msgs/RegionOfInterest.h new file mode 100644 index 0000000..62cdd67 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/RegionOfInterest.h @@ -0,0 +1,25 @@ +#ifndef C_API_SENSOR_MSGS_REGIONOFINTEREST_H +#define C_API_SENSOR_MSGS_REGIONOFINTEREST_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +typedef struct +{ + uint32_t x_offset; + uint32_t y_offset; + uint32_t height; + uint32_t width; + bool do_rectify; +} RegionOfInterest; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_REGIONOFINTEREST_H diff --git a/src/APIs/c_api/include/sensor_msgs/RelativeHumidity.h b/src/APIs/c_api/include/sensor_msgs/RelativeHumidity.h new file mode 100644 index 0000000..7892098 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/RelativeHumidity.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_RELATIVEHUMIDITY_H +#define C_API_SENSOR_MSGS_RELATIVEHUMIDITY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + double relative_humidity; + double variance; +} RelativeHumidity; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_RELATIVEHUMIDITY_H diff --git a/src/APIs/c_api/include/sensor_msgs/Temperature.h b/src/APIs/c_api/include/sensor_msgs/Temperature.h new file mode 100644 index 0000000..d1120d2 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Temperature.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_TEMPERATURE_H +#define C_API_SENSOR_MSGS_TEMPERATURE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + double temperature; + double variance; +} Temperature; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_TEMPERATURE_H diff --git a/src/APIs/c_api/include/sensor_msgs/TimeReference.h b/src/APIs/c_api/include/sensor_msgs/TimeReference.h new file mode 100644 index 0000000..0ca62a0 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/TimeReference.h @@ -0,0 +1,24 @@ +#ifndef C_API_SENSOR_MSGS_TIMEREFERENCE_H +#define C_API_SENSOR_MSGS_TIMEREFERENCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "std_msgs/Time.h" + +typedef struct +{ + Header header; + Time time_ref; + char *source; +} TimeReference; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_TIMEREFERENCE_H diff --git a/src/APIs/c_api/include/std_msgs/Bool.h b/src/APIs/c_api/include/std_msgs/Bool.h new file mode 100644 index 0000000..9a0d4dd --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Bool.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_BOOL_H +#define C_API_STD_MSGS_BOOL_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint8_t data; +} Bool; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_BOOL_H diff --git a/src/APIs/c_api/include/std_msgs/Byte.h b/src/APIs/c_api/include/std_msgs/Byte.h new file mode 100644 index 0000000..b7554b1 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Byte.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_BYTE_H +#define C_API_STD_MSGS_BYTE_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int8_t data; +} Byte; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_BYTE_H diff --git a/src/APIs/c_api/include/std_msgs/ByteMultiArray.h b/src/APIs/c_api/include/std_msgs/ByteMultiArray.h new file mode 100644 index 0000000..7ed0c51 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/ByteMultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_BYTEMULTIARRAY_H +#define C_API_STD_MSGS_BYTEMULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int8_t *data; + size_t data_count; +} ByteMultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_BYTEMULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Char.h b/src/APIs/c_api/include/std_msgs/Char.h new file mode 100644 index 0000000..8b4ba99 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Char.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_CHAR_H +#define C_API_STD_MSGS_CHAR_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint8_t data; +} Char; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_CHAR_H diff --git a/src/APIs/c_api/include/std_msgs/ColorRGBA.h b/src/APIs/c_api/include/std_msgs/ColorRGBA.h new file mode 100644 index 0000000..561ae9a --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/ColorRGBA.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_COLORRGBA_H +#define C_API_STD_MSGS_COLORRGBA_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + float r; + float g; + float b; + float a; +} ColorRGBA; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_COLORRGBA_H diff --git a/src/APIs/c_api/include/std_msgs/Duration.h b/src/APIs/c_api/include/std_msgs/Duration.h new file mode 100644 index 0000000..0e7da17 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Duration.h @@ -0,0 +1,21 @@ +#ifndef C_API_STD_MSGS_DURATION_H +#define C_API_STD_MSGS_DURATION_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int32_t sec; + int32_t nsec; +} Duration; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_DURATION_H diff --git a/src/APIs/c_api/include/std_msgs/Empty.h b/src/APIs/c_api/include/std_msgs/Empty.h new file mode 100644 index 0000000..9e3779f --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Empty.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_EMPTY_H +#define C_API_STD_MSGS_EMPTY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint8_t _dummy; +} Empty; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_EMPTY_H diff --git a/src/APIs/c_api/include/std_msgs/Float32.h b/src/APIs/c_api/include/std_msgs/Float32.h new file mode 100644 index 0000000..e2f72c2 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Float32.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_FLOAT32_H +#define C_API_STD_MSGS_FLOAT32_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + float data; +} Float32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_FLOAT32_H diff --git a/src/APIs/c_api/include/std_msgs/Float32MultiArray.h b/src/APIs/c_api/include/std_msgs/Float32MultiArray.h new file mode 100644 index 0000000..de400fa --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Float32MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_FLOAT32MULTIARRAY_H +#define C_API_STD_MSGS_FLOAT32MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + float *data; + size_t data_count; +} Float32MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_FLOAT32MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Float64.h b/src/APIs/c_api/include/std_msgs/Float64.h new file mode 100644 index 0000000..b7ef266 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Float64.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_FLOAT64_H +#define C_API_STD_MSGS_FLOAT64_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + double data; +} Float64; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_FLOAT64_H diff --git a/src/APIs/c_api/include/std_msgs/Float64MultiArray.h b/src/APIs/c_api/include/std_msgs/Float64MultiArray.h new file mode 100644 index 0000000..41272f2 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Float64MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_FLOAT64MULTIARRAY_H +#define C_API_STD_MSGS_FLOAT64MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + double *data; + size_t data_count; +} Float64MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_FLOAT64MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Header.h b/src/APIs/c_api/include/std_msgs/Header.h new file mode 100644 index 0000000..8f181ff --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Header.h @@ -0,0 +1,45 @@ +#ifndef C_API_STD_MSGS_HEADER_H +#define C_API_STD_MSGS_HEADER_H + +#ifdef __cplusplus +extern "C" +{ +#endif +#include +#include +#include + + typedef struct + { + uint32_t seq; + uint32_t sec; + uint32_t nsec; + char *frame_id; + } Header; + + /** + * @brief Create a new header + * @param frame_id Frame id + * @return Header + */ + Header header_create(const char *frame_id); + + /** + * @brief Set data for a header + * @param header Header + * @param seq Sequence + * @param sec Second + * @param nsec Nanosecond + * @param frame_id Frame id + * @return Header + */ + Header header_set_data( + uint32_t seq, + uint32_t sec, + uint32_t nsec, + char *frame_id); +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_HEADER_H \ No newline at end of file diff --git a/src/APIs/c_api/include/std_msgs/Int16.h b/src/APIs/c_api/include/std_msgs/Int16.h new file mode 100644 index 0000000..6b26619 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int16.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_INT16_H +#define C_API_STD_MSGS_INT16_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int16_t data; +} Int16; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT16_H diff --git a/src/APIs/c_api/include/std_msgs/Int16MultiArray.h b/src/APIs/c_api/include/std_msgs/Int16MultiArray.h new file mode 100644 index 0000000..53dbf90 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int16MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_INT16MULTIARRAY_H +#define C_API_STD_MSGS_INT16MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int16_t *data; + size_t data_count; +} Int16MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT16MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Int32.h b/src/APIs/c_api/include/std_msgs/Int32.h new file mode 100644 index 0000000..cac37e6 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int32.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_INT32_H +#define C_API_STD_MSGS_INT32_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int32_t data; +} Int32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT32_H diff --git a/src/APIs/c_api/include/std_msgs/Int32MultiArray.h b/src/APIs/c_api/include/std_msgs/Int32MultiArray.h new file mode 100644 index 0000000..5178f2d --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int32MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_INT32MULTIARRAY_H +#define C_API_STD_MSGS_INT32MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int32_t *data; + size_t data_count; +} Int32MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT32MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Int64.h b/src/APIs/c_api/include/std_msgs/Int64.h new file mode 100644 index 0000000..01142c1 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int64.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_INT64_H +#define C_API_STD_MSGS_INT64_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int64_t data; +} Int64; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT64_H diff --git a/src/APIs/c_api/include/std_msgs/Int64MultiArray.h b/src/APIs/c_api/include/std_msgs/Int64MultiArray.h new file mode 100644 index 0000000..f5154d0 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int64MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_INT64MULTIARRAY_H +#define C_API_STD_MSGS_INT64MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int64_t *data; + size_t data_count; +} Int64MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT64MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Int8.h b/src/APIs/c_api/include/std_msgs/Int8.h new file mode 100644 index 0000000..10747b7 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int8.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_INT8_H +#define C_API_STD_MSGS_INT8_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int8_t data; +} Int8; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT8_H diff --git a/src/APIs/c_api/include/std_msgs/Int8MultiArray.h b/src/APIs/c_api/include/std_msgs/Int8MultiArray.h new file mode 100644 index 0000000..13f3495 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int8MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_INT8MULTIARRAY_H +#define C_API_STD_MSGS_INT8MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int8_t *data; + size_t data_count; +} Int8MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT8MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/MultiArrayDimension.h b/src/APIs/c_api/include/std_msgs/MultiArrayDimension.h new file mode 100644 index 0000000..b7b1b9c --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/MultiArrayDimension.h @@ -0,0 +1,22 @@ +#ifndef C_API_STD_MSGS_MULTIARRAYDIMENSION_H +#define C_API_STD_MSGS_MULTIARRAYDIMENSION_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + char *label; + uint32_t size; + uint32_t stride; +} MultiArrayDimension; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_MULTIARRAYDIMENSION_H diff --git a/src/APIs/c_api/include/std_msgs/MultiArrayLayout.h b/src/APIs/c_api/include/std_msgs/MultiArrayLayout.h new file mode 100644 index 0000000..1928b7d --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/MultiArrayLayout.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_MULTIARRAYLAYOUT_H +#define C_API_STD_MSGS_MULTIARRAYLAYOUT_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayDimension.h" + + +typedef struct +{ + MultiArrayDimension *dim; + size_t dim_count; + uint32_t data_offset; +} MultiArrayLayout; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_MULTIARRAYLAYOUT_H diff --git a/src/APIs/c_api/include/std_msgs/String.h b/src/APIs/c_api/include/std_msgs/String.h new file mode 100644 index 0000000..ce6193d --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/String.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_STRING_H +#define C_API_STD_MSGS_STRING_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + char *data; +} String; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_STRING_H diff --git a/src/APIs/c_api/include/std_msgs/Time.h b/src/APIs/c_api/include/std_msgs/Time.h new file mode 100644 index 0000000..cb70ce1 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Time.h @@ -0,0 +1,27 @@ +#ifndef C_API_STD_MSGS_TIME_H +#define C_API_STD_MSGS_TIME_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint32_t sec; + uint32_t nsec; +} Time; + +/** + * @brief Create a new time + * @return Time + */ +Time time_create(); + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_TIME_H diff --git a/src/APIs/c_api/include/std_msgs/UInt16.h b/src/APIs/c_api/include/std_msgs/UInt16.h new file mode 100644 index 0000000..205ef45 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt16.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_UINT16_H +#define C_API_STD_MSGS_UINT16_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint16_t data; +} UInt16; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT16_H diff --git a/src/APIs/c_api/include/std_msgs/UInt16MultiArray.h b/src/APIs/c_api/include/std_msgs/UInt16MultiArray.h new file mode 100644 index 0000000..2d6885d --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt16MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_UINT16MULTIARRAY_H +#define C_API_STD_MSGS_UINT16MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + uint16_t *data; + size_t data_count; +} UInt16MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT16MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/UInt32.h b/src/APIs/c_api/include/std_msgs/UInt32.h new file mode 100644 index 0000000..5cd3152 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt32.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_UINT32_H +#define C_API_STD_MSGS_UINT32_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint32_t data; +} UInt32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT32_H diff --git a/src/APIs/c_api/include/std_msgs/UInt32MultiArray.h b/src/APIs/c_api/include/std_msgs/UInt32MultiArray.h new file mode 100644 index 0000000..1ad24f1 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt32MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_UINT32MULTIARRAY_H +#define C_API_STD_MSGS_UINT32MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + uint32_t *data; + size_t data_count; +} UInt32MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT32MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/UInt64.h b/src/APIs/c_api/include/std_msgs/UInt64.h new file mode 100644 index 0000000..5a94d2a --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt64.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_UINT64_H +#define C_API_STD_MSGS_UINT64_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint64_t data; +} UInt64; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT64_H diff --git a/src/APIs/c_api/include/std_msgs/UInt64MultiArray.h b/src/APIs/c_api/include/std_msgs/UInt64MultiArray.h new file mode 100644 index 0000000..32507e7 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt64MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_UINT64MULTIARRAY_H +#define C_API_STD_MSGS_UINT64MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + uint64_t *data; + size_t data_count; +} UInt64MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT64MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/UInt8.h b/src/APIs/c_api/include/std_msgs/UInt8.h new file mode 100644 index 0000000..d097ad0 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt8.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_UINT8_H +#define C_API_STD_MSGS_UINT8_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint8_t data; +} UInt8; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT8_H diff --git a/src/APIs/c_api/include/std_msgs/UInt8MultiArray.h b/src/APIs/c_api/include/std_msgs/UInt8MultiArray.h new file mode 100644 index 0000000..316bcaa --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt8MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_UINT8MULTIARRAY_H +#define C_API_STD_MSGS_UINT8MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + uint8_t *data; + size_t data_count; +} UInt8MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT8MULTIARRAY_H diff --git a/src/APIs/c_api/src/convertor.cpp b/src/APIs/c_api/src/convertor.cpp new file mode 100644 index 0000000..1960f3a --- /dev/null +++ b/src/APIs/c_api/src/convertor.cpp @@ -0,0 +1,368 @@ +#include "convertor.h" +#include +#include +#include + + +robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan) +{ + robot_sensor_msgs::LaserScan robot_laser_scan; + robot_laser_scan.header.seq = laser_scan.header.seq; + robot_laser_scan.header.stamp.sec = laser_scan.header.sec; + robot_laser_scan.header.stamp.nsec = laser_scan.header.nsec; + robot::log_info("LaserScan header.stamp.sec=%d header.stamp.nsec=%d", laser_scan.header.sec, laser_scan.header.nsec); + robot::log_info("LaserScan header.frame_id=%s", laser_scan.header.frame_id); + robot_laser_scan.header.frame_id = laser_scan.header.frame_id; + robot_laser_scan.angle_min = laser_scan.angle_min; + robot_laser_scan.angle_max = laser_scan.angle_max; + robot_laser_scan.angle_increment = laser_scan.angle_increment; + robot_laser_scan.time_increment = laser_scan.time_increment; + robot_laser_scan.scan_time = laser_scan.scan_time; + robot_laser_scan.range_min = laser_scan.range_min; + robot_laser_scan.range_max = laser_scan.range_max; + size_t ranges_size = laser_scan.ranges ? laser_scan.ranges_count : 0; + size_t intensities_size = laser_scan.intensities ? laser_scan.intensities_count : 0; + robot_laser_scan.ranges.resize(ranges_size); + robot_laser_scan.intensities.resize(intensities_size); + for (size_t i = 0; i < ranges_size; i++) { + robot_laser_scan.ranges[i] = laser_scan.ranges[i]; + } + for (size_t i = 0; i < intensities_size; i++) { + robot_laser_scan.intensities[i] = laser_scan.intensities[i]; + } + return robot_laser_scan; +} + + +robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occupancy_grid) { + robot::log_info("OccupancyGrid sizeof=%zu data_off=%zu data_count_off=%zu", + sizeof(OccupancyGrid), + offsetof(OccupancyGrid, data), + offsetof(OccupancyGrid, data_count)); + robot_nav_msgs::OccupancyGrid robot_occupancy_grid; + robot_occupancy_grid.header.seq = occupancy_grid.header.seq; + robot_occupancy_grid.header.stamp.sec = occupancy_grid.header.sec; + robot_occupancy_grid.header.stamp.nsec = occupancy_grid.header.nsec; + robot_occupancy_grid.header.frame_id = occupancy_grid.header.frame_id; + robot_occupancy_grid.info.map_load_time.sec = occupancy_grid.info.map_load_time.sec; + robot_occupancy_grid.info.map_load_time.nsec = occupancy_grid.info.map_load_time.nsec; + robot_occupancy_grid.info.resolution = occupancy_grid.info.resolution; + robot_occupancy_grid.info.width = occupancy_grid.info.width; + robot_occupancy_grid.info.height = occupancy_grid.info.height; + robot_occupancy_grid.info.origin.position.x = occupancy_grid.info.origin.position.x; + robot_occupancy_grid.info.origin.position.y = occupancy_grid.info.origin.position.y; + robot_occupancy_grid.info.origin.position.z = occupancy_grid.info.origin.position.z; + robot_occupancy_grid.info.origin.orientation.x = occupancy_grid.info.origin.orientation.x; + robot_occupancy_grid.info.origin.orientation.y = occupancy_grid.info.origin.orientation.y; + robot_occupancy_grid.info.origin.orientation.z = occupancy_grid.info.origin.orientation.z; + robot_occupancy_grid.info.origin.orientation.w = occupancy_grid.info.origin.orientation.w; + + size_t data_size = occupancy_grid.data ? occupancy_grid.data_count : 0; + + // robot_occupancy_grid.data.resize(data_size); + // robot::log_info("convertOccupancyGrid: 2"); + std::stringstream ss; + for (size_t i = 0; i < data_size; i++) { + ss << occupancy_grid.data[i] << " "; + } + robot::log_info("occupancy_grid.data: %s", ss.str().c_str()); + return robot_occupancy_grid; +} + +robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry) +{ + robot_nav_msgs::Odometry robot_odometry; + robot_odometry.header.seq = odometry.header.seq; + robot_odometry.header.stamp.sec = odometry.header.sec; + robot_odometry.header.stamp.nsec = odometry.header.nsec; + robot_odometry.header.frame_id = odometry.header.frame_id ? odometry.header.frame_id : ""; + + robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : ""; + robot_odometry.pose.pose.position.x = odometry.pose.pose.position.x; + robot_odometry.pose.pose.position.y = odometry.pose.pose.position.y; + robot_odometry.pose.pose.position.z = odometry.pose.pose.position.z; + robot_odometry.pose.pose.orientation.x = odometry.pose.pose.orientation.x; + robot_odometry.pose.pose.orientation.y = odometry.pose.pose.orientation.y; + robot_odometry.pose.pose.orientation.z = odometry.pose.pose.orientation.z; + robot_odometry.pose.pose.orientation.w = odometry.pose.pose.orientation.w; + + if (odometry.pose.covariance && odometry.pose.covariance_count > 0) + { + const size_t n = std::min(static_cast(36), odometry.pose.covariance_count); + for (size_t i = 0; i < n; ++i) + robot_odometry.pose.covariance[i] = odometry.pose.covariance[i]; + } + + robot_odometry.twist.twist.linear.x = odometry.twist.twist.linear.x; + robot_odometry.twist.twist.linear.y = odometry.twist.twist.linear.y; + robot_odometry.twist.twist.linear.z = odometry.twist.twist.linear.z; + robot_odometry.twist.twist.angular.x = odometry.twist.twist.angular.x; + robot_odometry.twist.twist.angular.y = odometry.twist.twist.angular.y; + robot_odometry.twist.twist.angular.z = odometry.twist.twist.angular.z; + + if (odometry.twist.covariance && odometry.twist.covariance_count > 0) + { + const size_t n = std::min(static_cast(36), odometry.twist.covariance_count); + for (size_t i = 0; i < n; ++i) + robot_odometry.twist.covariance[i] = odometry.twist.covariance[i]; + } + + return robot_odometry; +} + +// Convert C PoseStamped to C++ PoseStamped +robot_geometry_msgs::PoseStamped convert2CppPoseStamped(const PoseStamped& c_pose) +{ + robot_geometry_msgs::PoseStamped cpp_pose; + cpp_pose.header.seq = c_pose.header.seq; + cpp_pose.header.stamp.sec = c_pose.header.sec; + cpp_pose.header.stamp.nsec = c_pose.header.nsec; + cpp_pose.header.frame_id = c_pose.header.frame_id; + + cpp_pose.pose.position.x = c_pose.pose.position.x; + cpp_pose.pose.position.y = c_pose.pose.position.y; + cpp_pose.pose.position.z = c_pose.pose.position.z; + cpp_pose.pose.orientation.x = c_pose.pose.orientation.x; + cpp_pose.pose.orientation.y = c_pose.pose.orientation.y; + cpp_pose.pose.orientation.z = c_pose.pose.orientation.z; + cpp_pose.pose.orientation.w = c_pose.pose.orientation.w; + return cpp_pose; +} + +// Convert C++ PoseStamped to C PoseStamped +PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pose) +{ + PoseStamped c_pose; + c_pose.header.seq = cpp_pose.header.seq; + c_pose.header.sec = cpp_pose.header.stamp.sec; + c_pose.header.nsec = cpp_pose.header.stamp.nsec; + c_pose.header.frame_id = (char*)cpp_pose.header.frame_id.c_str(); + c_pose.pose.position.x = cpp_pose.pose.position.x; + c_pose.pose.position.y = cpp_pose.pose.position.y; + c_pose.pose.position.z = cpp_pose.pose.position.z; + c_pose.pose.orientation.x = cpp_pose.pose.orientation.x; + c_pose.pose.orientation.y = cpp_pose.pose.orientation.y; + c_pose.pose.orientation.z = cpp_pose.pose.orientation.z; + c_pose.pose.orientation.w = cpp_pose.pose.orientation.w; + return c_pose; +} + +// Convert C++ Pose2D to C Pose2D +Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose) +{ + Pose2D c_pose; + c_pose.x = cpp_pose.x; + c_pose.y = cpp_pose.y; + c_pose.theta = cpp_pose.theta; + return c_pose; +} +Header convert2CHeader(const robot_std_msgs::Header& cpp_header) +{ + Header c_header; + c_header.seq = cpp_header.seq; + c_header.sec = cpp_header.stamp.sec; + c_header.nsec = cpp_header.stamp.nsec; + c_header.frame_id = (char*)cpp_header.frame_id.c_str(); + return c_header; +} + +Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist) +{ + Twist2DStamped c_twist; + c_twist.header = convert2CHeader(cpp_twist.header); + c_twist.velocity.x = cpp_twist.velocity.x; + c_twist.velocity.y = cpp_twist.velocity.y; + c_twist.velocity.theta = cpp_twist.velocity.theta; + return c_twist; +} + +// ========== C++ to C converters (for get_* API) ========== + +void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out) +{ + out.header.seq = cpp.header.seq; + out.header.sec = cpp.header.stamp.sec; + out.header.nsec = cpp.header.stamp.nsec; + out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str()); + out.info.map_load_time.sec = cpp.info.map_load_time.sec; + out.info.map_load_time.nsec = cpp.info.map_load_time.nsec; + out.info.resolution = cpp.info.resolution; + out.info.width = cpp.info.width; + out.info.height = cpp.info.height; + out.info.origin.position.x = cpp.info.origin.position.x; + out.info.origin.position.y = cpp.info.origin.position.y; + out.info.origin.position.z = cpp.info.origin.position.z; + out.info.origin.orientation.x = cpp.info.origin.orientation.x; + out.info.origin.orientation.y = cpp.info.origin.orientation.y; + out.info.origin.orientation.z = cpp.info.origin.orientation.z; + out.info.origin.orientation.w = cpp.info.origin.orientation.w; + 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)); + } +} + +void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out) +{ + out.header.seq = cpp.header.seq; + out.header.sec = cpp.header.stamp.sec; + out.header.nsec = cpp.header.stamp.nsec; + out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str()); + out.angle_min = cpp.angle_min; + out.angle_max = cpp.angle_max; + out.angle_increment = cpp.angle_increment; + out.time_increment = cpp.time_increment; + out.scan_time = cpp.scan_time; + out.range_min = cpp.range_min; + out.range_max = cpp.range_max; + out.ranges_count = cpp.ranges.size(); + out.ranges = nullptr; + if (out.ranges_count > 0) + { + out.ranges = static_cast(malloc(out.ranges_count * sizeof(float))); + if (out.ranges) + memcpy(out.ranges, cpp.ranges.data(), out.ranges_count * sizeof(float)); + } + out.intensities_count = cpp.intensities.size(); + out.intensities = nullptr; + if (out.intensities_count > 0) + { + out.intensities = static_cast(malloc(out.intensities_count * sizeof(float))); + if (out.intensities) + memcpy(out.intensities, cpp.intensities.data(), out.intensities_count * sizeof(float)); + } +} + +void convert2CPointCloud(const robot_sensor_msgs::PointCloud& cpp, PointCloud& out) +{ + out.header.seq = cpp.header.seq; + out.header.sec = cpp.header.stamp.sec; + out.header.nsec = cpp.header.stamp.nsec; + out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str()); + 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; + } + } + out.channels_count = cpp.channels.size(); + out.channels = nullptr; + if (out.channels_count > 0) + { + out.channels = static_cast(malloc(out.channels_count * sizeof(ChannelFloat32))); + if (out.channels) + for (size_t i = 0; i < out.channels_count; ++i) + { + out.channels[i].name = cpp.channels[i].name.empty() ? nullptr : strdup(cpp.channels[i].name.c_str()); + out.channels[i].values_count = cpp.channels[i].values.size(); + out.channels[i].values = nullptr; + if (out.channels[i].values_count > 0) + { + out.channels[i].values = static_cast(malloc(out.channels[i].values_count * sizeof(float))); + if (out.channels[i].values) + memcpy(out.channels[i].values, cpp.channels[i].values.data(), out.channels[i].values_count * sizeof(float)); + } + } + } +} + +void convert2CPointCloud2(const robot_sensor_msgs::PointCloud2& cpp, PointCloud2& out) +{ + out.header.seq = cpp.header.seq; + out.header.sec = cpp.header.stamp.sec; + out.header.nsec = cpp.header.stamp.nsec; + out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str()); + out.height = cpp.height; + out.width = cpp.width; + out.is_bigendian = cpp.is_bigendian; + out.point_step = cpp.point_step; + out.row_step = cpp.row_step; + out.is_dense = cpp.is_dense; + out.fields_count = cpp.fields.size(); + out.fields = nullptr; + if (out.fields_count > 0) + { + out.fields = static_cast(malloc(out.fields_count * sizeof(PointField))); + if (out.fields) + for (size_t i = 0; i < out.fields_count; ++i) + { + out.fields[i].name = cpp.fields[i].name.empty() ? nullptr : strdup(cpp.fields[i].name.c_str()); + out.fields[i].offset = cpp.fields[i].offset; + out.fields[i].datatype = cpp.fields[i].datatype; + out.fields[i].count = cpp.fields[i].count; + } + } + out.data_count = cpp.data.size(); + out.data = nullptr; + if (out.data_count > 0) + { + out.data = static_cast(malloc(out.data_count)); + if (out.data) + memcpy(out.data, cpp.data.data(), out.data_count); + } +} + +// ========== C to C++ for PointCloud / PointCloud2 (for add_* API) ========== + +robot_sensor_msgs::PointCloud convert2CppPointCloud(const PointCloud& c) +{ + robot_sensor_msgs::PointCloud cpp; + cpp.header.seq = c.header.seq; + cpp.header.stamp.sec = c.header.sec; + cpp.header.stamp.nsec = c.header.nsec; + cpp.header.frame_id = c.header.frame_id ? c.header.frame_id : ""; + cpp.points.resize(c.points_count); + for (size_t i = 0; i < c.points_count && c.points; ++i) + { + cpp.points[i].x = c.points[i].x; + cpp.points[i].y = c.points[i].y; + cpp.points[i].z = c.points[i].z; + } + cpp.channels.resize(c.channels_count); + for (size_t i = 0; i < c.channels_count && c.channels; ++i) + { + cpp.channels[i].name = c.channels[i].name ? c.channels[i].name : ""; + cpp.channels[i].values.resize(c.channels[i].values_count); + if (c.channels[i].values && c.channels[i].values_count > 0) + memcpy(cpp.channels[i].values.data(), c.channels[i].values, c.channels[i].values_count * sizeof(float)); + } + return cpp; +} + +robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c) +{ + robot_sensor_msgs::PointCloud2 cpp; + cpp.header.seq = c.header.seq; + cpp.header.stamp.sec = c.header.sec; + cpp.header.stamp.nsec = c.header.nsec; + cpp.header.frame_id = c.header.frame_id ? c.header.frame_id : ""; + cpp.height = c.height; + cpp.width = c.width; + cpp.is_bigendian = c.is_bigendian; + cpp.point_step = c.point_step; + cpp.row_step = c.row_step; + cpp.is_dense = c.is_dense; + cpp.fields.resize(c.fields_count); + for (size_t i = 0; i < c.fields_count && c.fields; ++i) + { + cpp.fields[i].name = c.fields[i].name ? c.fields[i].name : ""; + cpp.fields[i].offset = c.fields[i].offset; + cpp.fields[i].datatype = c.fields[i].datatype; + cpp.fields[i].count = c.fields[i].count; + } + cpp.data.resize(c.data_count); + if (c.data && c.data_count > 0) + memcpy(cpp.data.data(), c.data, c.data_count); + return cpp; +} diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 0d836b5..1b1967c 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -1,1145 +1,1171 @@ +#include #include "nav_c_api.h" +#include "convertor.h" +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include #include #include #include -#include - -// ============================================================================ -// Internal Helper Functions -// ============================================================================ - -namespace { - // Convert C PoseStamped to C++ PoseStamped - robot_geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) { - robot_geometry_msgs::PoseStamped cpp_pose; - cpp_pose.header.seq = c_pose->header.seq; - cpp_pose.header.stamp.sec = c_pose->header.sec; - cpp_pose.header.stamp.nsec = c_pose->header.nsec; - if (c_pose->header.frame_id) { - cpp_pose.header.frame_id = c_pose->header.frame_id; - } - cpp_pose.pose.position.x = c_pose->pose.position.x; - cpp_pose.pose.position.y = c_pose->pose.position.y; - cpp_pose.pose.position.z = c_pose->pose.position.z; - cpp_pose.pose.orientation.x = c_pose->pose.orientation.x; - cpp_pose.pose.orientation.y = c_pose->pose.orientation.y; - cpp_pose.pose.orientation.z = c_pose->pose.orientation.z; - cpp_pose.pose.orientation.w = c_pose->pose.orientation.w; - return cpp_pose; - } - - // Convert C++ PoseStamped to C PoseStamped - void cpp_to_c_pose_stamped(const robot_geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) { - c_pose->header.seq = cpp_pose.header.seq; - c_pose->header.sec = cpp_pose.header.stamp.sec; - c_pose->header.nsec = cpp_pose.header.stamp.nsec; - if (!cpp_pose.header.frame_id.empty()) { - c_pose->header.frame_id = strdup(cpp_pose.header.frame_id.c_str()); - } else { - c_pose->header.frame_id = nullptr; - } - c_pose->pose.position.x = cpp_pose.pose.position.x; - c_pose->pose.position.y = cpp_pose.pose.position.y; - c_pose->pose.position.z = cpp_pose.pose.position.z; - c_pose->pose.orientation.x = cpp_pose.pose.orientation.x; - c_pose->pose.orientation.y = cpp_pose.pose.orientation.y; - c_pose->pose.orientation.z = cpp_pose.pose.orientation.z; - c_pose->pose.orientation.w = cpp_pose.pose.orientation.w; - } - - // Convert C++ Pose2D to C Pose2D - void cpp_to_c_pose_2d(const robot_geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) { - c_pose->x = cpp_pose.x; - c_pose->y = cpp_pose.y; - c_pose->theta = cpp_pose.theta; - } - - void cpp_to_c_header(const robot_std_msgs::Header& cpp_header, Header* c_header) { - c_header->seq = cpp_header.seq; - c_header->sec = cpp_header.stamp.sec; - c_header->nsec = cpp_header.stamp.nsec; - if (!cpp_header.frame_id.empty()) { - c_header->frame_id = strdup(cpp_header.frame_id.c_str()); - } else { - c_header->frame_id = nullptr; - } - } - - void cpp_to_c_twist2d_stamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist, - Twist2DStamped* c_twist) { - cpp_to_c_header(cpp_twist.header, &c_twist->header); - c_twist->velocity.x = cpp_twist.velocity.x; - c_twist->velocity.y = cpp_twist.velocity.y; - c_twist->velocity.theta = cpp_twist.velocity.theta; - } - - // Convert C++ NavFeedback to C NavFeedback - void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) { - c_feedback->navigation_state = static_cast(static_cast(cpp_feedback.navigation_state)); - if (!cpp_feedback.feed_back_str.empty()) { - c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str()); - } else { - c_feedback->feed_back_str = nullptr; - } - c_feedback->current_pose.x = cpp_feedback.current_pose.x; - c_feedback->current_pose.y = cpp_feedback.current_pose.y; - c_feedback->current_pose.theta = cpp_feedback.current_pose.theta; - c_feedback->goal_checked = cpp_feedback.goal_checked; - c_feedback->is_ready = cpp_feedback.is_ready; - } - - template - T* clone_message(const T& src) { - return new (std::nothrow) T(src); - } - - void clear_planner_data(PlannerDataOutput* data) { - if (!data) { - return; - } - delete static_cast(data->plan); - delete static_cast(data->costmap); - delete static_cast(data->costmap_update); - delete static_cast(data->footprint); - data->plan = nullptr; - data->costmap = nullptr; - data->costmap_update = nullptr; - data->footprint = nullptr; - data->is_costmap_updated = false; - } +static NavFeedback convert2CNavFeedback(const robot::move_base_core::NavFeedback &cpp) +{ + NavFeedback out; + out.navigation_state = static_cast(static_cast(cpp.navigation_state)); + out.feed_back_str = cpp.feed_back_str.empty() ? nullptr : strdup(cpp.feed_back_str.c_str()); + out.current_pose.x = cpp.current_pose.x; + out.current_pose.y = cpp.current_pose.y; + out.current_pose.theta = cpp.current_pose.theta; + out.goal_checked = cpp.goal_checked; + out.is_ready = cpp.is_ready; + return out; } -// ============================================================================ -// String Management -// ============================================================================ - -extern "C" void nav_c_api_free_string(char* str) { - if (str) { - free(str); - } +extern "C" char *navigation_state_to_string(NavigationState state) +{ + robot::move_base_core::State cpp_state = static_cast(static_cast(state)); + std::string str = robot::move_base_core::toString(cpp_state); + return strdup(str.c_str()); } -// ============================================================================ -// State Conversion -// ============================================================================ - -extern "C" char* navigation_state_to_string(NavigationState state) { - robot::move_base_core::State cpp_state = static_cast(static_cast(state)); - std::string str = robot::move_base_core::toString(cpp_state); - return strdup(str.c_str()); +extern "C" void nav_c_api_free_string(char *str) +{ + if (str) + free(str); } // ============================================================================ // Helper Functions // ============================================================================ -extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta, - const char* frame_id, double offset_distance, - PoseStamped* out_goal) { - if (!out_goal || !frame_id) { - return false; - } +extern "C" bool navigation_offset_goal_2d(const Pose2D &in_pose, + const char *frame_id, double offset_distance, + PoseStamped &out_goal) +{ + if (!frame_id) + { + return false; + } - robot_geometry_msgs::Pose2D pose; - pose.x = pose_x; - pose.y = pose_y; - pose.theta = pose_theta; - - robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance); - cpp_to_c_pose_stamped(result, out_goal); - return true; + robot_geometry_msgs::Pose2D pose = convert2CppPose2D(in_pose); + robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance); + out_goal = convert2CPoseStamped(result); + return true; } -extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance, - PoseStamped* out_goal) { - if (!in_pose || !out_goal) { - return false; - } - - robot_geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose); - robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance); - cpp_to_c_pose_stamped(result, out_goal); - return true; -} - -// ============================================================================ -// Navigation Handle Management -// ============================================================================ - -extern "C" NavigationHandle navigation_create(void) { - try { - // Create a concrete MoveBase instance - // Using default constructor - initialization will be done via navigation_initialize() - move_base::MoveBase* move_base = new move_base::MoveBase(); - return static_cast(move_base); - } catch (const std::exception& e) { - // Log error if possible (in production, use proper logging) - return nullptr; - } catch (...) { - return nullptr; - } -} - -extern "C" void navigation_destroy(NavigationHandle handle) { - if (handle) { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - delete nav; - } +extern "C" bool navigation_offset_goal_stamped(const PoseStamped &in_pose, double offset_distance, + PoseStamped &out_goal) +{ + robot_geometry_msgs::PoseStamped cpp_pose = convert2CppPoseStamped(in_pose); + robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance); + out_goal = convert2CPoseStamped(result); + return true; } // ============================================================================ // TF Listener Management // ============================================================================ -extern "C" TFListenerHandle tf_listener_create(void) { - try { - auto tf = std::make_shared(); - return new std::shared_ptr(tf); - } catch (...) { - return nullptr; - } +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" void tf_listener_destroy(TFListenerHandle handle) +{ + if (handle.ptr) + { + auto *tf_ptr = static_cast *>(handle.ptr); + delete tf_ptr; + } } extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle, - const char* parent_frame, - const char* child_frame, + 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; + if (!tf_handle.ptr || !parent_frame || !child_frame) + { + return false; + } + + try + { + auto *tf_ptr = static_cast *>(tf_handle.ptr); + if (!tf_ptr || !(*tf_ptr)) + { + 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; - 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; + } +} - 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 + { + // Create a concrete MoveBase instance + // Using default constructor - initialization will be done via navigation_initialize() + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath("MoveBase"); + 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)}; + } + catch (const std::exception &e) + { + // Log error if possible (in production, use proper logging) + return NavigationHandle{nullptr}; + } + catch (...) + { + return NavigationHandle{nullptr}; + } +} + +extern "C" void navigation_destroy(NavigationHandle handle) +{ + if (handle.ptr) + { + auto *nav_ptr = static_cast *>(handle.ptr); + delete nav_ptr; + } } // ============================================================================ // Navigation Interface Methods // ============================================================================ -extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) +extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) { - printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); - 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 = static_cast(handle); - auto* tf_ptr = static_cast*>(tf_handle); - if (!tf_ptr || !(*tf_ptr)) { - printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); - return false; - } - nav->initialize(*tf_ptr); - - return true; - } catch (const std::exception &e) { - printf("[%s:%d]\n Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what()); - return false; - } catch (...) { - printf("[%s:%d]\n Error: Failed to initialize navigation\n", __FILE__, __LINE__); - return false; + printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); + if (!handle.ptr || !tf_handle.ptr) + { + 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) + return false; + auto *nav = nav_ptr->get(); + auto *tf_ptr = static_cast *>(tf_handle.ptr); + if (!tf_ptr || !(*tf_ptr)) + { + printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); + return false; } + nav->initialize(*tf_ptr); + + return true; + } + catch (const std::exception &e) + { + printf("[%s:%d]\n Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what()); + return false; + } + 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 || !points || point_count == 0) { - 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) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - std::vector footprint; - footprint.reserve(point_count); - for (size_t i = 0; i < point_count; ++i) { - robot_geometry_msgs::Point pt; - pt.x = points[i].x; - pt.y = points[i].y; - pt.z = points[i].z; - footprint.push_back(pt); - } - nav->setRobotFootprint(footprint); - return true; - } catch (...) { - return false; + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*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) + { + robot_geometry_msgs::Point pt; + pt.x = points[i].x; + pt.y = points[i].y; + pt.z = points[i].z; + footprint.push_back(pt); } + nav->setRobotFootprint(footprint); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count) { - if (!handle || !out_points || !out_count) { - return false; +extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count) +{ + if (!handle.ptr) + { + return false; + } + + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + std::vector footprint = nav->getRobotFootprint(); + if (footprint.empty()) + { + return false; } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - std::vector footprint = nav->getRobotFootprint(); - if (footprint.empty()) { - *out_points = nullptr; - *out_count = 0; - return true; - } - - Point* points = static_cast(malloc(sizeof(Point) * footprint.size())); - if (!points) { - return false; - } - - for (size_t i = 0; i < footprint.size(); ++i) { - points[i].x = footprint[i].x; - points[i].y = footprint[i].y; - points[i].z = footprint[i].z; - } - - *out_points = points; - *out_count = footprint.size(); - return true; - } catch (...) { - return false; + out_count = footprint.size(); + for (size_t i = 0; i < out_count; ++i) + { + out_points[i].x = footprint[i].x; + out_points[i].y = footprint[i].y; + out_points[i].z = footprint[i].z; } + return true; + } + catch (...) + { + return false; + } } -extern "C" void navigation_free_points(Point* points) { - if (points) { - free(points); - } +extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + if (!handle.ptr) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal, - double xy_goal_tolerance, double yaw_goal_tolerance) { - if (!handle || !goal) { - return false; - } +// 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; +// } +// } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->moveTo(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) +{ + if (!handle.ptr || !marker) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav->dockTo(std::string(marker), 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) { - if (!handle || !marker || !goal) { - return false; - } +// 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; +// } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); - } catch (...) { - return false; - } +extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped goal, + double xy_goal_tolerance) +{ + if (!handle.ptr) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav->moveStraightTo(cpp_goal, xy_goal_tolerance); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal, - double xy_goal_tolerance) { - if (!handle || !goal) { - return false; - } +extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal, double yaw_goal_tolerance) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->moveStraightTo(cpp_goal, xy_goal_tolerance); - } catch (...) { - return false; - } + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav->rotateTo(cpp_goal, yaw_goal_tolerance); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal, - double yaw_goal_tolerance) { - if (!handle || !goal) { - return false; +extern "C" void navigation_pause(NavigationHandle handle) +{ + if (handle.ptr) + { + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + nav->pause(); } - - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->rotateTo(cpp_goal, yaw_goal_tolerance); - } catch (...) { - return false; + catch (...) + { + // Ignore exceptions } + } } -extern "C" void navigation_pause(NavigationHandle handle) { - if (handle) { - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - nav->pause(); - } catch (...) { - // Ignore exceptions - } +extern "C" void navigation_resume(NavigationHandle handle) +{ + if (handle.ptr) + { + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + nav->resume(); } + catch (...) + { + // Ignore exceptions + } + } } -extern "C" void navigation_resume(NavigationHandle handle) { - if (handle) { - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - nav->resume(); - } catch (...) { - // Ignore exceptions - } +extern "C" void navigation_cancel(NavigationHandle handle) +{ + if (handle.ptr) + { + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + nav->cancel(); } -} - -extern "C" void navigation_cancel(NavigationHandle handle) { - if (handle) { - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - nav->cancel(); - } catch (...) { - // Ignore exceptions - } + catch (...) + { + // Ignore exceptions } + } } extern "C" bool navigation_set_twist_linear(NavigationHandle handle, - double linear_x, double linear_y, double linear_z) { - if (!handle) { - return false; - } + double linear_x, double linear_y, double linear_z) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::Vector3 linear; - linear.x = linear_x; - linear.y = linear_y; - linear.z = linear_z; - return nav->setTwistLinear(linear); - } catch (...) { - return false; - } + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::Vector3 linear; + linear.x = linear_x; + linear.y = linear_y; + linear.z = linear_z; + return nav->setTwistLinear(linear); + } + catch (...) + { + return false; + } } extern "C" bool navigation_set_twist_angular(NavigationHandle handle, - double angular_x, double angular_y, double angular_z) { - if (!handle) { - return false; - } + double angular_x, double angular_y, double angular_z) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::Vector3 angular; - angular.x = angular_x; - angular.y = angular_y; - angular.z = angular_z; - return nav->setTwistAngular(angular); - } catch (...) { - return false; - } + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::Vector3 angular; + angular.x = angular_x; + angular.y = angular_y; + angular.z = angular_z; + return nav->setTwistAngular(angular); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose) { - if (!handle || !out_pose) { - return false; +extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose) +{ + if (!handle.ptr) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_pose; + if (nav->getRobotPose(cpp_pose)) + { + out_pose = convert2CPoseStamped(cpp_pose); + return true; } + return false; + } + catch (...) + { + return false; + } +} - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_pose; - if (nav->getRobotPose(cpp_pose)) { - cpp_to_c_pose_stamped(cpp_pose, out_pose); - return true; - } - return false; - } catch (...) { - return false; +extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose) +{ + if (!handle.ptr) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::Pose2D cpp_pose; + if (nav->getRobotPose(cpp_pose)) + { + out_pose = convert2CPose2D(cpp_pose); + return true; } + return false; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose) { - if (!handle || !out_pose) { - return false; - } +extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::Pose2D cpp_pose; - if (nav->getRobotPose(cpp_pose)) { - cpp_to_c_pose_2d(cpp_pose, out_pose); - return true; - } - return false; - } catch (...) { - return false; - } + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist(); + out_twist = convert2CTwist2DStamped(cpp_twist); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist) { - if (!handle || !out_twist) { - return false; - } +extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist(); - cpp_to_c_twist2d_stamped(cpp_twist, out_twist); - return true; - } catch (...) { - return false; - } -} - -extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) { - if (!handle || !out_feedback) { - return false; - } - - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - if (nav->getFeedback() != nullptr) { - cpp_to_c_nav_feedback(*nav->getFeedback(), out_feedback); - return true; - } else { - return false; - } - } catch (...) { - return false; - } -} - -extern "C" void navigation_free_feedback(NavFeedback* feedback) { - if (feedback) { - nav_c_api_free_string(feedback->feed_back_str); - feedback->feed_back_str = nullptr; - } -} - -// ============================================================================ -// Complex Message Handle Management -// ============================================================================ - -extern "C" void navigation_free_occupancy_grid(OccupancyGridHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_occupancy_grid_update(OccupancyGridUpdateHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_laser_scan(LaserScanHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_point_cloud(PointCloudHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_point_cloud2(PointCloud2Handle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_odometry(OdometryHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_path2d(Path2DHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_polygon_stamped(PolygonStampedHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_order(OrderHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_named_occupancy_grids(NamedOccupancyGrid* maps, size_t count) { - if (!maps) { - return; - } - for (size_t i = 0; i < count; ++i) { - free(maps[i].name); - delete static_cast(maps[i].map); - } - free(maps); -} - -extern "C" void navigation_free_named_laser_scans(NamedLaserScan* scans, size_t count) { - if (!scans) { - return; - } - for (size_t i = 0; i < count; ++i) { - free(scans[i].name); - delete static_cast(scans[i].scan); - } - free(scans); -} - -extern "C" void navigation_free_named_point_clouds(NamedPointCloud* clouds, size_t count) { - if (!clouds) { - return; - } - for (size_t i = 0; i < count; ++i) { - free(clouds[i].name); - delete static_cast(clouds[i].cloud); - } - free(clouds); -} - -extern "C" void navigation_free_named_point_cloud2s(NamedPointCloud2* clouds, size_t count) { - if (!clouds) { - return; - } - for (size_t i = 0; i < count; ++i) { - free(clouds[i].name); - delete static_cast(clouds[i].cloud); - } - free(clouds); -} - -extern "C" void navigation_free_planner_data(PlannerDataOutput* data) { - clear_planner_data(data); + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot::move_base_core::NavFeedback *fb = nav->getFeedback(); + if (fb == nullptr) + return false; + out_feedback = convert2CNavFeedback(*fb); + return true; + } + catch (...) + { + return false; + } } // ============================================================================ // Navigation Data Management // ============================================================================ -extern "C" bool navigation_add_static_map(NavigationHandle handle, const char* map_name, OccupancyGridHandle map) { - if (!handle || !map_name || !map) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* map_ptr = static_cast(map); - nav->addStaticMap(std::string(map_name), *map_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_add_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid occupancy_grid) +{ + if (!handle.ptr || !map_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*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); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char* laser_scan_name, LaserScanHandle laser_scan) { - if (!handle || !laser_scan_name || !laser_scan) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* scan_ptr = static_cast(laser_scan); - nav->addLaserScan(std::string(laser_scan_name), *scan_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan laser_scan) +{ + if (!handle.ptr || !laser_scan_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*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); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char* point_cloud_name, PointCloudHandle point_cloud) { - if (!handle || !point_cloud_name || !point_cloud) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* cloud_ptr = static_cast(point_cloud); - nav->addPointCloud(std::string(point_cloud_name), *cloud_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud) +{ + if (!handle.ptr || !point_cloud_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*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); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char* point_cloud2_name, PointCloud2Handle point_cloud2) { - if (!handle || !point_cloud2_name || !point_cloud2) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* cloud_ptr = static_cast(point_cloud2); - nav->addPointCloud2(std::string(point_cloud2_name), *cloud_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2) +{ + if (!handle.ptr || !point_cloud2_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*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); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_add_odometry(NavigationHandle handle, const char* odometry_name, OdometryHandle odometry) { - if (!handle || !odometry_name || !odometry) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* odom_ptr = static_cast(odometry); - nav->addOdometry(std::string(odometry_name), *odom_ptr); - return true; - } catch (...) { - return false; +extern "C" bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry) +{ + if (!handle.ptr || !odometry_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*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); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_static_map(NavigationHandle handle, const char* map_name, OccupancyGridHandle* out_map) { - if (!handle || !map_name || !out_map) { - return false; - } - try { - auto* nav = static_cast(handle); - robot_nav_msgs::OccupancyGrid map = nav->getStaticMap(std::string(map_name)); - auto* map_ptr = clone_message(map); - if (!map_ptr) { - return false; - } - *out_map = static_cast(map_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map) +{ + if (!handle.ptr || !map_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_nav_msgs::OccupancyGrid robot_map = nav->getStaticMap(std::string(map_name)); + convert2COccupancyGrid(robot_map, out_map); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char* laser_scan_name, LaserScanHandle* out_scan) { - if (!handle || !laser_scan_name || !out_scan) { - return false; - } - try { - auto* nav = static_cast(handle); - robot_sensor_msgs::LaserScan scan = nav->getLaserScan(std::string(laser_scan_name)); - auto* scan_ptr = clone_message(scan); - if (!scan_ptr) { - return false; - } - *out_scan = static_cast(scan_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan) +{ + if (!handle.ptr || !laser_scan_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_sensor_msgs::LaserScan robot_scan = nav->getLaserScan(std::string(laser_scan_name)); + convert2CLaserScan(robot_scan, out_scan); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char* point_cloud_name, PointCloudHandle* out_cloud) { - if (!handle || !point_cloud_name || !out_cloud) { - return false; - } - try { - auto* nav = static_cast(handle); - robot_sensor_msgs::PointCloud cloud = nav->getPointCloud(std::string(point_cloud_name)); - auto* cloud_ptr = clone_message(cloud); - if (!cloud_ptr) { - return false; - } - *out_cloud = static_cast(cloud_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud) +{ + if (!handle.ptr || !point_cloud_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud robot_cloud = nav->getPointCloud(std::string(point_cloud_name)); + convert2CPointCloud(robot_cloud, out_cloud); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char* point_cloud2_name, PointCloud2Handle* out_cloud) { - if (!handle || !point_cloud2_name || !out_cloud) { - return false; - } - try { - auto* nav = static_cast(handle); - robot_sensor_msgs::PointCloud2 cloud = nav->getPointCloud2(std::string(point_cloud2_name)); - auto* cloud_ptr = clone_message(cloud); - if (!cloud_ptr) { - return false; - } - *out_cloud = static_cast(cloud_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud) +{ + if (!handle.ptr || !point_cloud2_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud2 robot_cloud = nav->getPointCloud2(std::string(point_cloud2_name)); + convert2CPointCloud2(robot_cloud, out_cloud); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid** out_maps, size_t* out_count) { - if (!handle || !out_maps || !out_count) { - return false; - } - try { - auto* nav = static_cast(handle); - auto maps = nav->getAllStaticMaps(); - if (maps.empty()) { - *out_maps = nullptr; - *out_count = 0; - return true; - } - - NamedOccupancyGrid* result = static_cast(malloc(sizeof(NamedOccupancyGrid) * maps.size())); - if (!result) { - return false; - } - - size_t index = 0; - for (const auto& entry : maps) { - result[index].name = strdup(entry.first.c_str()); - result[index].map = static_cast(clone_message(entry.second)); - if (!result[index].name || !result[index].map) { - navigation_free_named_occupancy_grids(result, index + 1); - return false; - } - ++index; - } - - *out_maps = result; - *out_count = maps.size(); - return true; - } catch (...) { - return false; +extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count) +{ + if (!handle.ptr || !out_maps) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto maps = nav->getAllStaticMaps(); + out_count = maps.size(); + size_t i = 0; + for (const auto &p : maps) + { + out_maps[i].name = strdup(p.first.c_str()); + convert2COccupancyGrid(p.second, out_maps[i].grid); + ++i; } + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan** out_scans, size_t* out_count) { - if (!handle || !out_scans || !out_count) { - return false; +extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count) +{ + if (!handle.ptr || !out_scans) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto scans = nav->getAllLaserScans(); + if (scans.empty()) + { + out_count = 0; + return true; } - try { - auto* nav = static_cast(handle); - auto scans = nav->getAllLaserScans(); - if (scans.empty()) { - *out_scans = nullptr; - *out_count = 0; - return true; - } - - NamedLaserScan* result = static_cast(malloc(sizeof(NamedLaserScan) * scans.size())); - if (!result) { - return false; - } - - size_t index = 0; - for (const auto& entry : scans) { - result[index].name = strdup(entry.first.c_str()); - result[index].scan = static_cast(clone_message(entry.second)); - if (!result[index].name || !result[index].scan) { - navigation_free_named_laser_scans(result, index + 1); - return false; - } - ++index; - } - - *out_scans = result; - *out_count = scans.size(); - return true; - } catch (...) { - return false; + out_count = scans.size(); + size_t i = 0; + for (const auto &p : scans) + { + out_scans[i].name = strdup(p.first.c_str()); + convert2CLaserScan(p.second, out_scans[i].scan); + ++i; } + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud** out_clouds, size_t* out_count) { - if (!handle || !out_clouds || !out_count) { - return false; +extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count) +{ + if (!handle.ptr || !out_clouds) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto clouds = nav->getAllPointClouds(); + if (clouds.empty()) + { + out_count = 0; + return true; } - try { - auto* nav = static_cast(handle); - auto clouds = nav->getAllPointClouds(); - if (clouds.empty()) { - *out_clouds = nullptr; - *out_count = 0; - return true; - } - - NamedPointCloud* result = static_cast(malloc(sizeof(NamedPointCloud) * clouds.size())); - if (!result) { - return false; - } - - size_t index = 0; - for (const auto& entry : clouds) { - result[index].name = strdup(entry.first.c_str()); - result[index].cloud = static_cast(clone_message(entry.second)); - if (!result[index].name || !result[index].cloud) { - navigation_free_named_point_clouds(result, index + 1); - return false; - } - ++index; - } - - *out_clouds = result; - *out_count = clouds.size(); - return true; - } catch (...) { - return false; + out_count = clouds.size(); + size_t i = 0; + for (const auto &p : clouds) + { + out_clouds[i].name = strdup(p.first.c_str()); + convert2CPointCloud(p.second, out_clouds[i].cloud); + ++i; } + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2** out_clouds, size_t* out_count) { - if (!handle || !out_clouds || !out_count) { - return false; +extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count) +{ + if (!handle.ptr || !out_clouds) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto clouds = nav->getAllPointCloud2s(); + if (clouds.empty()) + { + out_count = 0; + return true; } - try { - auto* nav = static_cast(handle); - auto clouds = nav->getAllPointCloud2s(); - if (clouds.empty()) { - *out_clouds = nullptr; - *out_count = 0; - return true; - } - - NamedPointCloud2* result = static_cast(malloc(sizeof(NamedPointCloud2) * clouds.size())); - if (!result) { - return false; - } - - size_t index = 0; - for (const auto& entry : clouds) { - result[index].name = strdup(entry.first.c_str()); - result[index].cloud = static_cast(clone_message(entry.second)); - if (!result[index].name || !result[index].cloud) { - navigation_free_named_point_cloud2s(result, index + 1); - return false; - } - ++index; - } - - *out_clouds = result; - *out_count = clouds.size(); - return true; - } catch (...) { - return false; + out_count = clouds.size(); + size_t i = 0; + for (const auto &p : clouds) + { + out_clouds[i].name = strdup(p.first.c_str()); + convert2CPointCloud2(p.second, out_clouds[i].cloud); + ++i; } + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char* map_name) { - if (!handle || !map_name) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeStaticMap(std::string(map_name)); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char *map_name) +{ + if (!handle.ptr || !map_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeStaticMap(std::string(map_name)); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char* laser_scan_name) { - if (!handle || !laser_scan_name) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeLaserScan(std::string(laser_scan_name)); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char *laser_scan_name) +{ + if (!handle.ptr || !laser_scan_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeLaserScan(std::string(laser_scan_name)); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const char* point_cloud_name) { - if (!handle || !point_cloud_name) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removePointCloud(std::string(point_cloud_name)); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const char *point_cloud_name) +{ + if (!handle.ptr || !point_cloud_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removePointCloud(std::string(point_cloud_name)); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const char* point_cloud2_name) { - if (!handle || !point_cloud2_name) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removePointCloud2(std::string(point_cloud2_name)); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const char *point_cloud2_name) +{ + if (!handle.ptr || !point_cloud2_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removePointCloud2(std::string(point_cloud2_name)); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllStaticMaps(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllStaticMaps(); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllLaserScans(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllLaserScans(); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllPointClouds(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllPointClouds(); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllPointCloud2s(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllPointCloud2s(); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_data(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllData(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_data(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllData(); + } + catch (...) + { + return false; + } } // ============================================================================ -// Planner Data +// Planner Data (disabled - PlannerDataOutput and related types not in public API) // ============================================================================ +#if 0 +extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput *out_data) +{ + if (!handle.ptr || !out_data) + { + return false; + } + clear_planner_data(out_data); + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*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; -extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput* out_data) { - if (!handle || !out_data) { - return false; + 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; } + return true; + } + catch (...) + { clear_planner_data(out_data); - try { - auto* nav = static_cast(handle); - robot::move_base_core::PlannerDataOutput data = nav->getGlobalData(); - out_data->plan = static_cast(clone_message(data.plan)); - out_data->costmap = static_cast(clone_message(data.costmap)); - out_data->costmap_update = static_cast(clone_message(data.costmap_update)); - out_data->footprint = static_cast(clone_message(data.footprint)); - out_data->is_costmap_updated = data.is_costmap_updated; - - if (!out_data->plan || !out_data->costmap || !out_data->costmap_update || !out_data->footprint) { - clear_planner_data(out_data); - return false; - } - return true; - } catch (...) { - clear_planner_data(out_data); - return false; - } + return false; + } } -extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput* out_data) { - if (!handle || !out_data) { - return false; +extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput *out_data) +{ + if (!handle.ptr || !out_data) + { + return false; + } + clear_planner_data(out_data); + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*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; } + return true; + } + catch (...) + { clear_planner_data(out_data); - try { - auto* nav = static_cast(handle); - robot::move_base_core::PlannerDataOutput data = nav->getLocalData(); - out_data->plan = static_cast(clone_message(data.plan)); - out_data->costmap = static_cast(clone_message(data.costmap)); - out_data->costmap_update = static_cast(clone_message(data.costmap_update)); - out_data->footprint = static_cast(clone_message(data.footprint)); - out_data->is_costmap_updated = data.is_costmap_updated; - - if (!out_data->plan || !out_data->costmap || !out_data->costmap_update || !out_data->footprint) { - clear_planner_data(out_data); - return false; - } - return true; - } catch (...) { - clear_planner_data(out_data); - return false; - } + return false; + } } - -// ============================================================================ -// Navigation Commands with Order -// ============================================================================ - -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 || !order || !goal) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* order_ptr = static_cast(order); - 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_order(NavigationHandle handle, const OrderHandle order, - const PoseStamped* goal, - double xy_goal_tolerance, double yaw_goal_tolerance) { - if (!handle || !order || !goal) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* order_ptr = static_cast(order); - 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; - } -} - +#endif diff --git a/src/APIs/c_api/src/std_msgs/Header.cpp b/src/APIs/c_api/src/std_msgs/Header.cpp new file mode 100644 index 0000000..f9f9f34 --- /dev/null +++ b/src/APIs/c_api/src/std_msgs/Header.cpp @@ -0,0 +1,21 @@ +#include "std_msgs/Header.h" +#include +#include +#include + +extern "C" Header header_create(const char *frame_id) { + Header header{}; + header.frame_id = strdup(frame_id); + header.sec = robot::Time::now().sec; + header.nsec = robot::Time::now().nsec; + return header; +} + +extern "C" Header header_set_data(uint32_t seq, uint32_t sec, uint32_t nsec, char *frame_id) { + Header header{}; + header.seq = seq; + header.sec = sec; + header.nsec = nsec; + header.frame_id = strdup(frame_id); + return header; +} diff --git a/src/APIs/c_api/src/std_msgs/Time.cpp b/src/APIs/c_api/src/std_msgs/Time.cpp new file mode 100644 index 0000000..3895350 --- /dev/null +++ b/src/APIs/c_api/src/std_msgs/Time.cpp @@ -0,0 +1,12 @@ +#include "std_msgs/Time.h" +#include +#include +#include + +extern "C" Time time_create() +{ + Time time{}; + time.sec = robot::Time::now().sec; + time.nsec = robot::Time::now().nsec; + return time; +} \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 26b188d..7e520e5 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -416,6 +416,24 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms { it->second = map; } + robot::log_info("map header: %s", map.header.frame_id.c_str()); + robot::log_info("map header: %ld.%ld", map.header.stamp.sec, map.header.stamp.nsec); + robot::log_info("map header: %d", map.header.seq); + robot::log_info("map info resolution: %f", map.info.resolution); + robot::log_info("map info width: %d", map.info.width); + robot::log_info("map info height: %d", map.info.height); + robot::log_info("map info origin position x: %f", map.info.origin.position.x); + robot::log_info("map info origin position y: %f", map.info.origin.position.y); + robot::log_info("map info origin position z: %f", map.info.origin.position.z); + robot::log_info("map info origin orientation x: %f", map.info.origin.orientation.x); + robot::log_info("map info origin orientation y: %f", map.info.origin.orientation.y); + robot::log_info("map info origin orientation z: %f", map.info.origin.orientation.z); + robot::log_info("map info origin orientation w: %f", map.info.origin.orientation.w); + robot::log_info("map data size: %zu", map.data.size()); + for(size_t i = 0; i < map.data.size(); i++) { + robot::log_info("map data[%zu]: %d", i, map.data[i]); + } + robot::log_info("--------------------------------"); updateGlobalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); updateLocalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); } @@ -432,7 +450,6 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) { - auto it = laser_scans_.find(laser_scan_name); if (it == laser_scans_.end()) { @@ -442,6 +459,29 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot { it->second = laser_scan; } + robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec); + robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str()); + robot::log_info("angle_min: %f", laser_scan.angle_min); + robot::log_info("angle_max: %f", laser_scan.angle_max); + robot::log_info("angle_increment: %f", laser_scan.angle_increment); + robot::log_info("time_increment: %f", laser_scan.time_increment); + robot::log_info("scan_time: %f", laser_scan.scan_time); + robot::log_info("range_min: %f", laser_scan.range_min); + robot::log_info("range_max: %f", laser_scan.range_max); + robot::log_info("ranges_size: %zu", laser_scan.ranges.size()); + robot::log_info("intensities_size: %zu", laser_scan.intensities.size()); + std::stringstream ranges_str; + for (size_t i = 0; i < laser_scan.ranges.size(); i++) { + ranges_str << laser_scan.ranges[i] << " "; + } + robot::log_info("ranges: %s", ranges_str.str().c_str()); + std::stringstream intensities_str; + for (size_t i = 0; i < laser_scan.intensities.size(); i++) { + intensities_str << laser_scan.intensities[i] << " "; + } + robot::log_info("intensities: %s", intensities_str.str().c_str()); + robot::log_info("--------------------------------"); + updateLocalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateGlobalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); } @@ -659,6 +699,33 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) { + 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()); + robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x); + robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y); + robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z); + robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x); + robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y); + robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z); + robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w); + std::stringstream pose_covariance_str; + for(int i = 0; i < 36; i++) { + pose_covariance_str << odometry.pose.covariance[i] << " "; + } + robot::log_info("odometry pose covariance: %s", pose_covariance_str.str().c_str()); + robot::log_info("odometry twist linear x: %f", odometry.twist.twist.linear.x); + robot::log_info("odometry twist linear y: %f", odometry.twist.twist.linear.y); + robot::log_info("odometry twist linear z: %f", odometry.twist.twist.linear.z); + robot::log_info("odometry twist angular x: %f", odometry.twist.twist.angular.x); + robot::log_info("odometry twist angular y: %f", odometry.twist.twist.angular.y); + robot::log_info("odometry twist angular z: %f", odometry.twist.twist.angular.z); + std::stringstream twist_covariance_str; + for(int i = 0; i < 36; i++) { + twist_covariance_str << odometry.twist.covariance[i] << " "; + } + robot::log_info("odometry twist covariance: %s", twist_covariance_str.str().c_str()); + odometry_ = odometry; }