created

examples/CSharpExample.cs

@@ -0,0 +1,322 @@
+using System;
+using System.Runtime.InteropServices;
+using System.Runtime.CompilerServices;
+
+namespace NavigationExample
+{
+    /// <summary>
+    /// C# P/Invoke wrapper for Navigation C API
+    /// </summary>
+    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);
+        }
+    }
+}
+

examples/NavigationExample/NavigationExample.csproj

@@ -0,0 +1,13 @@
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net6.0</TargetFramework>
+    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+  </PropertyGroup>
+  <ItemGroup>
+    <None Include="libnav_c_api.so">
+      <CopyToOutputDirectory>Always</CopyToOutputDirectory>
+    </None>
+  </ItemGroup>
+</Project>

examples/NavigationExample/Program.cs

@@ -0,0 +1,322 @@
+using System;
+using System.Runtime.InteropServices;
+using System.Runtime.CompilerServices;
+
+namespace NavigationExample
+{
+    /// <summary>
+    /// C# P/Invoke wrapper for Navigation C API
+    /// </summary>
+    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);
+        }
+    }
+}
+

examples/QUICK_START.md

@@ -0,0 +1,148 @@
+# Quick Start - Chạy C# Example
+
+## Bước 1: Cài đặt .NET SDK (nếu chưa có)
+
+### Ubuntu/Debian:
+```bash
+# Cài đặt .NET 6.0 hoặc mới hơn
+wget https://dot.net/v1/dotnet-install.sh
+chmod +x dotnet-install.sh
+./dotnet-install.sh --channel 6.0
+
+# Thêm vào PATH
+export PATH=$PATH:$HOME/.dotnet
+export DOTNET_ROOT=$HOME/.dotnet
+
+# Hoặc cài qua apt (nếu có)
+sudo apt-get update
+sudo apt-get install -y dotnet-sdk-6.0
+```
+
+### Kiểm tra:
+```bash
+dotnet --version
+```
+
+## Bước 2: Build C API Library
+
+```bash
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core
+mkdir -p build
+cd build
+cmake ..
+make nav_c_api
+```
+
+Library sẽ ở: `build/src/APIs/c_api/libnav_c_api.so`
+
+## Bước 3: Chạy C# Example
+
+### Cách 1: Dùng script tự động (Khuyến nghị)
+
+```bash
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
+chmod +x run_example.sh
+./run_example.sh
+```
+
+Script sẽ tự động:
+- ✅ Build library nếu chưa có
+- ✅ Tạo C# project
+- ✅ Copy library và source code
+- ✅ Set LD_LIBRARY_PATH cho tất cả dependencies
+- ✅ Build và chạy example
+
+### Cách 2: Chạy thủ công
+
+```bash
+# 1. Tạo project
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
+dotnet new console -n NavigationExample
+cd NavigationExample
+
+# 2. Copy source code
+cp ../CSharpExample.cs Program.cs
+
+# 3. Tạo .csproj
+cat > NavigationExample.csproj << 'EOF'
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net6.0</TargetFramework>
+    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+  </PropertyGroup>
+  <ItemGroup>
+    <None Include="libnav_c_api.so">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </None>
+  </ItemGroup>
+</Project>
+EOF
+
+# 4. Copy library
+cp ../../build/src/APIs/c_api/libnav_c_api.so .
+
+# 5. Set library path (quan trọng!)
+export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH
+
+# 6. Build và chạy
+dotnet build
+dotnet run
+```
+
+## Bước 4: Kiểm tra kết quả
+
+Nếu chạy thành công, bạn sẽ thấy output như:
+```
+Robot pose: x=0.0, y=0.0, theta=0.0
+State: PENDING, Feedback: ...
+```
+
+## Troubleshooting
+
+### Lỗi: "dotnet: command not found"
+→ Cài đặt .NET SDK (xem Bước 1)
+
+### Lỗi: "Unable to load DLL 'libnav_c_api.so'"
+```bash
+# Kiểm tra library có tồn tại không
+ls -la ../../build/src/APIs/c_api/libnav_c_api.so
+
+# Set LD_LIBRARY_PATH (bao gồm tất cả dependencies)
+export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH
+
+# Hoặc copy vào system lib
+sudo cp libnav_c_api.so /usr/local/lib/
+sudo ldconfig
+```
+
+### Lỗi: "Missing dependencies"
+```bash
+# Kiểm tra dependencies
+ldd libnav_c_api.so
+
+# Đảm bảo tất cả dependencies đều có sẵn
+```
+
+### Lỗi: "Failed to create navigation instance"
+→ Đã được sửa! `navigation_create()` bây giờ tạo instance của `move_base::MoveBase`.
+   Nếu vẫn gặp lỗi, kiểm tra:
+   - Library đã được build đầy đủ: `make nav_c_api` trong build directory
+   - Dependencies đã được build: `make move_base costmap_2d`
+   - LD_LIBRARY_PATH đã được set đúng
+

examples/README.md

@@ -0,0 +1,142 @@
+# Hướng dẫn chạy C# Example
+
+## Yêu cầu
+
+1. **.NET SDK** (version 6.0 trở lên hoặc .NET 10)
+2. **C API Library** đã được build (`libnav_c_api.so`)
+3. **Linux environment** (hoặc WSL nếu dùng Windows)
+
+## Bước 1: Build C API Library
+
+```bash
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core
+mkdir -p build
+cd build
+cmake ..
+make nav_c_api
+```
+
+Library sẽ được tạo tại: `build/src/APIs/c_api/libnav_c_api.so`
+
+## Bước 2: Chạy script tự động (Khuyến nghị)
+
+```bash
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
+chmod +x run_example.sh
+./run_example.sh
+```
+
+Script sẽ tự động:
+- Build library nếu chưa có
+- Tạo C# project
+- Copy library và source code
+- Set LD_LIBRARY_PATH cho tất cả dependencies
+- Build và chạy example
+
+## Bước 3: Hoặc chạy thủ công
+
+### Tạo C# Project
+
+```bash
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
+dotnet new console -n NavigationExample
+cd NavigationExample
+```
+
+### Copy file và library
+
+```bash
+# Copy C# source code
+cp ../CSharpExample.cs Program.cs
+
+# Copy library vào thư mục project
+cp ../../build/src/APIs/c_api/libnav_c_api.so .
+```
+
+## Bước 4: Cập nhật .csproj (nếu cần)
+
+Tạo file `NavigationExample.csproj`:
+
+```xml
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net6.0</TargetFramework>
+    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+  </PropertyGroup>
+  <ItemGroup>
+    <None Include="libnav_c_api.so">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </None>
+  </ItemGroup>
+</Project>
+```
+
+## Bước 4: Build và chạy
+
+```bash
+# Set LD_LIBRARY_PATH để tìm được tất cả dependencies
+export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH
+
+# Build
+dotnet build
+
+# Chạy
+dotnet run
+```
+
+## Lưu ý
+
+1. **Library path**: Đảm bảo `libnav_c_api.so` có thể được tìm thấy:
+   - Copy vào cùng thư mục với executable
+   - Hoặc set `LD_LIBRARY_PATH` environment variable
+   - Hoặc copy vào `/usr/local/lib` và chạy `ldconfig`
+
+2. **Dependencies**: Library cần các dependencies khác:
+   ```bash
+   # Kiểm tra dependencies
+   ldd libnav_c_api.so
+   
+   # Đảm bảo tất cả dependencies đều có sẵn
+   ```
+
+3. **Permissions**: Đảm bảo file có quyền execute:
+   ```bash
+   chmod +x libnav_c_api.so
+   ```
+
+## Troubleshooting
+
+### Lỗi: "Unable to load DLL 'libnav_c_api.so'"
+
+**Giải pháp:**
+```bash
+# Set LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/APIs/c_api:$LD_LIBRARY_PATH
+
+# Hoặc copy library vào system path
+sudo cp libnav_c_api.so /usr/local/lib/
+sudo ldconfig
+```
+
+### Lỗi: "DllNotFoundException"
+
+**Giải pháp:**
+- Kiểm tra tên file library có đúng không
+- Kiểm tra architecture (x64 vs x86)
+- Kiểm tra dependencies: `ldd libnav_c_api.so`
+
+### Lỗi: "Symbol not found"
+
+**Giải pháp:**
+- Đảm bảo library đã được build đầy đủ
+- Kiểm tra tên hàm trong C# có khớp với C API không
+

examples/START_HERE.md

@@ -0,0 +1,83 @@
+# 🚀 Hướng dẫn chạy thử C API Navigation
+
+## Cách nhanh nhất (1 lệnh)
+
+```bash
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
+chmod +x run_example.sh
+./run_example.sh
+```
+
+## Chi tiết các bước
+
+### Bước 1: Build C API Library
+
+```bash
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build
+cmake ..
+make nav_c_api
+```
+
+Library sẽ được tạo tại: `build/src/APIs/c_api/libnav_c_api.so`
+
+### Bước 2: Chạy C# Example
+
+Script `run_example.sh` sẽ tự động:
+- ✅ Kiểm tra và build library nếu cần
+- ✅ Tạo C# project
+- ✅ Copy library và source code
+- ✅ Set LD_LIBRARY_PATH cho tất cả dependencies
+- ✅ Build và chạy example
+
+```bash
+cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
+./run_example.sh
+```
+
+## Kết quả mong đợi
+
+Nếu chạy thành công, bạn sẽ thấy:
+
+```
+==========================================
+Building and Running C# Navigation Example
+==========================================
+Library already exists: .../build/src/APIs/c_api/libnav_c_api.so
+C# project already exists
+Copying library...
+Building C# project...
+
+Running example...
+==========================================
+Robot pose: x=0, y=0, theta=0
+State: PENDING, Feedback: 
+==========================================
+Done!
+```
+
+## Yêu cầu
+
+1. **.NET SDK 6.0+** (script sẽ kiểm tra và hướng dẫn nếu thiếu)
+2. **Library đã được build** (script sẽ tự động build nếu thiếu)
+3. **Linux environment**
+
+## Troubleshooting
+
+### Lỗi: "dotnet: command not found"
+→ Script sẽ hiển thị hướng dẫn cài đặt .NET SDK
+
+### Lỗi: "Failed to create navigation instance"
+→ Đảm bảo `move_base` đã được build:
+```bash
+cd build && make move_base
+```
+
+### Lỗi: "Cannot load library"
+→ Kiểm tra LD_LIBRARY_PATH đã được set đúng (script tự động set)
+
+## Xem thêm
+
+- `README.md` - Hướng dẫn chi tiết
+- `QUICK_START.md` - Quick start guide
+- `run_example.sh` - Script tự động
+

examples/run_example.sh

@@ -0,0 +1,117 @@
+#!/bin/bash
+
+# Script để build và chạy C# example
+
+set -e
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+PROJECT_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)"
+BUILD_DIR="$PROJECT_ROOT/build"
+LIB_DIR="$BUILD_DIR/src/APIs/c_api"
+EXAMPLE_DIR="$SCRIPT_DIR"
+
+echo "=========================================="
+echo "Building and Running C# Navigation Example"
+echo "=========================================="
+echo ""
+echo "📁 Paths:"
+echo "   SCRIPT_DIR:    $SCRIPT_DIR"
+echo "   PROJECT_ROOT:   $PROJECT_ROOT"
+echo "   BUILD_DIR:     $BUILD_DIR"
+echo "   LIB_DIR:       $LIB_DIR"
+echo "   EXAMPLE_DIR:   $EXAMPLE_DIR"
+echo ""
+
+
+echo "Building C API library..."
+cd "$BUILD_DIR"
+cmake ..
+make 
+echo "Library built successfully!"
+
+
+# Kiểm tra dotnet có được cài đặt không
+if ! command -v dotnet &> /dev/null; then
+    echo "=========================================="
+    echo "WARNING: .NET SDK not found!"
+    echo "=========================================="
+    echo "To run the C# example, please install .NET SDK:"
+    echo ""
+    echo "  Ubuntu/Debian:"
+    echo "    wget https://dot.net/v1/dotnet-install.sh -O dotnet-install.sh"
+    echo "    chmod +x dotnet-install.sh"
+    echo "    ./dotnet-install.sh --channel 6.0"
+    echo "    export PATH=\$PATH:\$HOME/.dotnet"
+    echo ""
+    echo "  Or visit: https://dotnet.microsoft.com/download"
+    echo ""
+    echo "Skipping C# example. Library is available at:"
+    echo "  $LIB_DIR/libnav_c_api.so"
+    echo "=========================================="
+    exit 0
+fi
+
+# Bước 2: Tạo C# project nếu chưa có
+if [ ! -d "$EXAMPLE_DIR/NavigationExample" ]; then
+    echo "Creating C# project..."
+    cd "$EXAMPLE_DIR"
+    dotnet new console -n NavigationExample
+    cd NavigationExample
+    
+    # Tạo .csproj file
+    cat > NavigationExample.csproj << 'EOF'
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net6.0</TargetFramework>
+    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+  </PropertyGroup>
+  <ItemGroup>
+    <None Include="libnav_c_api.so">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </None>
+  </ItemGroup>
+</Project>
+EOF
+    
+    echo "C# project created!"
+else
+    echo "C# project already exists"
+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..."
+cp "$LIB_DIR/libnav_c_api.so" .
+
+# Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
+export LD_LIBRARY_PATH="$LIB_DIR:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH"
+
+# Bước 5: Build và chạy
+echo "Building C# project..."
+dotnet build
+
+echo ""
+echo "Running example..."
+echo "=========================================="
+echo "LD_LIBRARY_PATH includes:"
+echo "$LD_LIBRARY_PATH" | tr ':' '\n' | head -5
+echo "..."
+echo "=========================================="
+dotnet run
+
+echo "=========================================="
+echo "Done!"
+