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 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 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; } // ============================================================================ // String Management // ============================================================================ [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] public static extern void nav_c_api_free_string(IntPtr str); // ============================================================================ // 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); // ============================================================================ // 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_move_to( IntPtr handle, 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_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_feedback( IntPtr handle, ref NavFeedback out_feedback); [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] public static extern void navigation_free_feedback(ref NavFeedback feedback); // ============================================================================ // 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; } // 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); } // Cleanup NavigationAPI.navigation_destroy(navHandle); NavigationAPI.tf_listener_destroy(tfHandle); } } }