update find path file plugin .so

.gitignore

@@ -421,3 +421,5 @@ FodyWeavers.xsd
 build
 install
 devel
+
+obstacle

.gitmodules

@@ -28,3 +28,6 @@
 [submodule "src/Libraries/xmlrpcpp"]
 	path = src/Libraries/xmlrpcpp
 	url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git
+[submodule "src/Libraries/laser_filter"]
+	path = src/Libraries/laser_filter
+	url = https://git.pnkr.asia/DuongTD/laser_filter.git

CMAKE_BUILD_README.md

@@ -51,6 +51,7 @@ sudo apt-get install -y \
     libyaml-cpp-dev \
     libpcl-dev \
     libgoogle-glog-dev
+sudo apt install liborocos-kdl-dev
 
 # Optional: Cài đặt Google Test (nếu muốn build tests)
 sudo apt-get install -y libgtest-dev

CMakeLists.txt

@@ -74,6 +74,10 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
+if (NOT TARGET laser_filter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter) 
+endif()
+
 if (NOT TARGET robot_nav_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
 endif()
@@ -134,6 +138,22 @@ if (NOT TARGET pnkx_local_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
 endif()
 
+if (NOT TARGET robot_angles)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/angles)
+endif()
+
+if (NOT TARGET grid_map_core)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/grid_map_core)
+endif()
+
+if (NOT TARGET robot_base_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/base_local_planner)
+endif()
+
+if (NOT TARGET hybrid_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/hybrid_local_planner)
+endif()
+
 if (NOT TARGET robot_actionlib_msgs)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
 endif()

config/config/obstacle_layer_params.yaml

@@ -1,4 +1,5 @@
 obstacle_layer:
+  enabled: true
   track_unknown_space: true
   transform_tolerance: 0.2
   topic: "map"

config/costmap_common_params.yaml

@@ -1,11 +1,4 @@
-position_planner_name: PNKXLocalPlanner
+robot_base_frame: base_link
-docking_planner_name: PNKXDockingLocalPlanner
-go_straight_planner_name: PNKXGoStraightLocalPlanner
-rotate_planner_name: PNKXRotateLocalPlanner
-base_local_planner: LocalPlannerAdapter
-base_global_planner: CustomPlanner
-
-robot_base_frame: base_footprint
 transform_tolerance: 1.0
 obstacle_range: 3.0
 #mark_threshold: 1
@@ -26,21 +19,37 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
+  observation_sources:  l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing
-  f_scan_marking:
+  l_scan_marking:
-    topic: /f_scan
+    topic: /l_scan
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
-  f_scan_clearing:
+  l_scan_clearing:
-    topic: /f_scan
+    topic: /l_scan
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_marking:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: true
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  r_scan_clearing:
+    topic: /r_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_marking:
@@ -48,7 +57,7 @@ obstacles:
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_clearing:
@@ -56,118 +65,9 @@ obstacles:
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   
 
-global_costmap:
-  robot_base_frame: base_footprint
-  transform_tolerance: 1.0
-  obstacle_range: 3.0
-  #mark_threshold: 1
-  publish_voxel_map: true
-  footprint_padding: 0.0
 
-  navigation_map:
-    map_topic: /map
-    map_pkg: managerments
-    map_file: maze
-
-  virtual_walls_map:
-    map_topic: /virtual_walls/map
-    namespace: /virtual_walls
-    map_pkg: managerments
-    map_file: maze
-    use_maximum: true
-    lethal_cost_threshold: 100
-
-  obstacles:
-    observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-    f_scan_marking:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    f_scan_clearing:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_marking:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_clearing:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-local_costmap:
-  robot_base_frame: base_footprint
-  transform_tolerance: 1.0
-  obstacle_range: 3.0
-  #mark_threshold: 1
-  publish_voxel_map: true
-  footprint_padding: 0.0
-
-  navigation_map:
-    map_topic: /map
-    map_pkg: managerments
-    map_file: maze
-
-  virtual_walls_map:
-    map_topic: /virtual_walls/map
-    namespace: /virtual_walls
-    map_pkg: managerments
-    map_file: maze
-    use_maximum: true
-    lethal_cost_threshold: 100
-
-  obstacles:
-    observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
-    f_scan_marking:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    f_scan_clearing:
-      topic: /f_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_marking:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: false
-      marking: true
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25
-    b_scan_clearing:
-      topic: /b_scan
-      data_type: LaserScan
-      clearing: true
-      marking: false
-      inf_is_valid: false
-      min_obstacle_height: 0.0
-      max_obstacle_height: 0.25

config/costmap_global_params.yaml

@@ -1,10 +1,10 @@
 global_costmap:
   library_path: libplugins
-  robot_base_frame: base_footprint
+  robot_base_frame: base_link
   global_frame: map
   update_frequency: 1.0
   publish_frequency: 1.0
-  raytrace_range: 2.0
+  raytrace_range: 3.5
   resolution: 0.05
   z_resolution: 0.2
   rolling_window: false

config/costmap_local_params.yaml

@@ -1,11 +1,11 @@
 local_costmap: 
   library_path: libplugins
   global_frame: odom
-  robot_base_frame: base_footprint
+  robot_base_frame: base_link
   update_frequency: 6.0
   publish_frequency: 6.0
   rolling_window: true
-  raytrace_range: 2.0
+  raytrace_range: 3.5
   resolution: 0.05
   z_resolution: 0.15
   z_voxels: 8

config/dock_global_params.yaml

@@ -0,0 +1,11 @@
+DockPlanner:
+  library_path: libdock_planner
+  MyGlobalPlanner:
+    cost_threshold: 200        # Ngưỡng chi phí vật cản (0-255)
+    safety_distance: 2         # Khoảng cách an toàn (cells)
+    use_dijkstra: false        # Sử dụng Dijkstra thay vì A*
+    
+  # File: config/costmap_params.yaml
+  global_costmap:
+    inflation_radius: 0.3      # Bán kính phình vật cản
+    cost_scaling_factor: 10.0  # Hệ số tỷ lệ chi phí

config/hybrid_local_planner_params.yaml

@@ -0,0 +1,59 @@
+LocalPlannerAdapter:
+  library_path: liblocal_planner_adapter
+  yaw_goal_tolerance: 0.017  
+  xy_goal_tolerance: 0.03 
+  min_approach_linear_velocity: 0.06
+
+HybridLocalPlanner:
+# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
+# HybridLocalPlanner:
+    library_path: libhybrid_local_planner
+    odom_topic: odom
+    # Trajectory
+    max_global_plan_lookahead_dist: 4.0
+    global_plan_viapoint_sep: 0.5
+    global_plan_prune_distance: 0.0
+    global_plan_goal_sep: 0.05
+
+    # Robot
+
+    robot_max_v_ac: 0.4
+    robot_max_w_ac: 0.4
+    robot_max_v_pt: 1.0
+    robot_max_w_pt: 0.4
+    robot_max_v_backwards_pt: -0.2
+    acc_lim_x: 1.0
+    acc_lim_theta: 2.0
+    turn_around_priority: True
+    stop_dist: 0.5
+    dec_dist: 1.0
+
+
+    # GoalTolerance
+
+    xy_goal_tolerance: 0.1
+    yaw_goal_tolerance: 0.07
+
+    # Optimization
+
+    # PP Parameters
+    w_vel: 0.8
+    w_omega: 1.0
+    # DWA Parameters
+    enable_backward_motion: false
+    w_targetheading_ac: 1.7
+    w_velocity_ac: 0.2
+    w_clearance_ac: 0.2
+    w_pathDistance_ac: 0.05
+    w_smoothness_ac: 0.3
+    w_targetheading_pt: 0.2
+    w_velocity_pt: 0.8
+    w_clearance_pt: 0.1
+    w_pathDistance_pt: 2.1
+    w_smoothness_pt: 0.3
+    time_horizon: 3.0
+    velocity_resolution: 0.015
+
+    segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment
+    calibration_factor: 1.5 # Hệ số hiệu chuẩn
+    use_obstacle_avoidance: true # Bật tắt tránh vật cản

config/maker_sources.yaml

@@ -37,7 +37,7 @@ charger:
     - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
 
   charger:
-    maker_goal_frame: charger_goal
+    maker_goal_frame: charger
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được
     timeout: 60

config/move_base_common_params.yaml

@@ -1,3 +1,25 @@
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+base_local_planner: LocalPlannerAdapter
+base_global_planner: CustomPlanner
+
+PNKXLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: CustomPlanner
+
+PNKXDockingLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXGoStraightLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXRotateLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
 
 ### replanning
 controller_frequency: 30.0   # run controller at 15.0 Hz

config/mprim/ekf.yaml

@@ -62,7 +62,7 @@ publish_acceleration: false
 #         estimation node from robot_localization! However, that instance should *not* fuse the global data.
 map_frame: map                                   # Defaults to "map" if unspecified
 odom_frame: $(arg tf_prefix)odom                 # Defaults to "odom" if unspecified
-base_link_frame: $(arg tf_prefix)base_footprint  # Defaults to "base_link" if unspecified
+base_link_frame: $(arg tf_prefix)base_link  # Defaults to "base_link" if unspecified
 world_frame: $(arg tf_prefix)odom                # Defaults to the value of odom_frame if unspecified
 
 # The filter accepts an arbitrary number of inputs from each input message type (robot_nav_msgs/Odometry,

config/mprim/localization.yaml

@@ -25,7 +25,7 @@ Amcl:
   update_min_d: 0.05
   update_min_a: 0.05
   odom_frame_id: odom
-  base_frame_id: base_footprint
+  base_frame_id: base_link
   global_frame_id: map
   resample_interval: 1
   transform_tolerance: 0.2

config/mprim/ros_diff_drive_controller.yaml

@@ -25,7 +25,7 @@ wheel_radius_multiplier    : 1.0 # default: 1.0
 cmd_vel_timeout: 1.0
 
 # frame_ids (same as real MiR platform)
-base_frame_id: base_footprint # default: base_link base_footprint
+base_frame_id: base_link # default: base_link base_link
 odom_frame_id: odom           # default: odom 
 
 # Velocity and acceleration limits

config/pnkx_local_planner_params.yaml

@@ -1,4 +1,4 @@
-yaw_goal_tolerance: 0.017  
+yaw_goal_tolerance: 0.03  
 xy_goal_tolerance: 0.02 
 min_approach_linear_velocity: 0.05 
 
@@ -50,13 +50,13 @@ LimitedAccelGenerator:
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
   min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
-  max_vel_theta: 0.7   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
 
-  acc_lim_x: 1.0
+  acc_lim_x: 1.5
   acc_lim_y: 0.0      # diff drive robot
   acc_lim_theta: 1.5
-  decel_lim_x: -1.0
+  decel_lim_x: -1.5
   decel_lim_y: -0.0
   decel_lim_theta: -1.5
 
@@ -79,6 +79,14 @@ MKTAlgorithmDiffPredictiveTrajectory:
   index_samples: 60
   follow_step_path: true
 
+  # Kalman filter tuning (filters v and w commands)
+  kf_q_v: 0.25
+  kf_q_w: 0.8
+  kf_r_v: 0.05
+  kf_r_w: 0.08
+  kf_p0: 0.5
+  kf_filter_angular: false
+
   # Lookahead
   use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
   # only when false:
@@ -88,7 +96,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
   max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
   lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
   min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.5                    # Maximum squared journey to consider for goal (default: 0.2) 
   max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
   # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
@@ -105,7 +113,9 @@ MKTAlgorithmDiffPredictiveTrajectory:
   final_heading_xy_tolerance: 0.1
   final_heading_angle_tolerance: 0.05
   final_heading_min_velocity: 0.05
-  final_heading_kp_angular: 2.0
+  final_heading_kp_angular: 1.2
+  final_heading_ki_angular: 0.002
+  final_heading_kd_angular: 0.12
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
@@ -140,7 +150,9 @@ MKTAlgorithmDiffGoStraight:
   final_heading_xy_tolerance: 0.1
   final_heading_angle_tolerance: 0.05
   final_heading_min_velocity: 0.05
-  final_heading_kp_angular: 2.0
+  final_heading_kp_angular: 1.5
+  final_heading_ki_angular: 0.2
+  final_heading_kd_angular: 0.05
 
 MKTAlgorithmDiffRotateToGoal:
   library_path: libmkt_algorithm_diff
@@ -176,6 +188,9 @@ MKTAlgorithmDiffRotateToGoal:
   final_heading_angle_tolerance: 0.05
   final_heading_min_velocity: 0.05
   final_heading_kp_angular: 2.0
+  near_goal_heading_kp: 1.5
+  near_goal_heading_ki: 0.0
+  near_goal_heading_kd: 0.0
   
 GoalChecker:
   library_path: libmkt_plugins_goal_checker

examples/NavigationExample/NavigationAPI.cs

@@ -0,0 +1,299 @@
+using System;
+using System.Runtime.InteropServices;
+using NavigationExample;
+
+namespace NavigationExample
+{
+    /// <summary>
+    /// C# P/Invoke wrapper for Navigation C API
+    /// </summary>
+    public class NavigationAPI
+    {
+        private const string DllName = "/usr/local/lib/libnav_c_api.so"; // Linux
+        // For Windows: "nav_c_api.dll"
+        // For macOS: "libnav_c_api.dylib"
+
+        // ============================================================================
+        // Enums
+        // ============================================================================
+        public enum NavigationState
+        {
+            Pending = 0,
+            Active = 1,
+            Preempted = 2,
+            Succeeded = 3,
+            Aborted = 4,
+            Rejected = 5,
+            Preempting = 6,
+            Recalling = 7,
+            Recalled = 8,
+            Lost = 9,
+            Planning = 10,
+            Controlling = 11,
+            Clearing = 12,
+            Paused = 13
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct NavFeedback
+        {
+            public NavigationState navigation_state;
+            public IntPtr feed_back_str;  // char*; free with nav_c_api_free_string
+            public Pose2D current_pose;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool goal_checked;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool is_ready;
+        }
+
+
+        /// <summary>Planner data output (plan, costmap, footprint).</summary>
+        [StructLayout(LayoutKind.Sequential)]
+        public struct PlannerDataOutput
+        {
+            public Path2D plan;
+            public OccupancyGrid costmap;
+            public OccupancyGridUpdate costmap_update;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool is_costmap_updated;
+            public PolygonStamped footprint;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct NavigationHandle
+        {
+            public IntPtr ptr;
+        }
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern Header header_create(string frame_id);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern Header header_set_data(
+            uint seq,
+            uint sec,
+            uint nsec,
+            string frame_id);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern Time time_create();
+
+        /// <summary>Free a string allocated by the API (strdup).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void nav_c_api_free_string(IntPtr str);
+
+        /// <summary>Convert NavigationState to string; caller must free with nav_c_api_free_string.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern IntPtr navigation_state_to_string(NavigationState state);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
+
+
+        /// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
+        public static string MarshalString(IntPtr p)
+        {
+            if (p == IntPtr.Zero) return string.Empty;
+            return Marshal.PtrToStringAnsi(p) ?? string.Empty;
+        }
+
+        /// <summary>Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done.</summary>
+        public static void navigation_free_feedback(ref NavFeedback feedback)
+        {
+            if (feedback.feed_back_str != IntPtr.Zero)
+            {
+                nav_c_api_free_string(feedback.feed_back_str);
+                feedback.feed_back_str = IntPtr.Zero;
+            }
+        }
+
+        // ============================================================================
+        // Navigation Handle Management
+        // ============================================================================
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern NavigationHandle navigation_create();
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void navigation_destroy(NavigationHandle handle);
+
+        /// <summary>Initialize navigation using an existing tf3 buffer (from libtf3). Caller owns the buffer and must call tf3_buffer_destroy after navigation_destroy.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_initialize(NavigationHandle handle, IntPtr tf3_buffer);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);
+
+        /// <summary>Send a goal for the robot to navigate to (global frame).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal);
+
+        /// <summary>Navigate using an Order message (graph nodes/edges). Order must be built or obtained from native side; call order_free when done if it was allocated by native.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to_nodes_edges(NavigationHandle handle, IntPtr nodes, UIntPtr node_count, IntPtr edges, UIntPtr edge_count, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, string marker, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_nodes_edges(NavigationHandle handle, string marker, Node[] nodes, Edge[] edges, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);
+
+        /// <summary>Rotate in place to align with target orientation (radians).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_rotate_to(NavigationHandle handle, double goal_yaw);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_pause(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_resume(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_cancel(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist);
+
+        // ============================================================================
+        // Navigation Data Management
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan);
+    
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_point_cloud(NavigationHandle handle, string point_cloud_name, PointCloud point_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_point_cloud2(NavigationHandle handle, string point_cloud2_name, PointCloud2 point_cloud2);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_laser_scan(NavigationHandle handle, string laser_scan_name, ref LaserScan out_scan);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_point_cloud(NavigationHandle handle, string point_cloud_name, ref PointCloud out_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_point_cloud2(NavigationHandle handle, string point_cloud2_name, ref PointCloud2 out_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_static_map(NavigationHandle handle, string map_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_laser_scan(NavigationHandle handle, string laser_scan_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_point_cloud(NavigationHandle handle, string point_cloud_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_point_cloud2(NavigationHandle handle, string point_cloud2_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_static_maps(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_laser_scans(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_point_clouds(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_point_cloud2s(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_data(NavigationHandle handle);
+
+        /// <summary>Get all static maps. out_maps must be pre-allocated; use navigation_get_all_static_maps_count or similar to get count first if needed.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_static_maps(NavigationHandle handle, [Out] NamedOccupancyGrid[] out_maps, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_laser_scans(NavigationHandle handle, [Out] NamedLaserScan[] out_scans, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_point_clouds(NavigationHandle handle, [Out] NamedPointCloud[] out_clouds, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_point_cloud2s(NavigationHandle handle, [Out] NamedPointCloud2[] out_clouds, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_global_data(NavigationHandle handle, ref PlannerDataOutput out_data);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_local_data(NavigationHandle handle, ref PlannerDataOutput out_data);
+    }
+}

examples/NavigationExample/NavigationExample.csproj

@@ -2,9 +2,11 @@
   <PropertyGroup>
     <OutputType>Exe</OutputType>
     <TargetFramework>net6.0</TargetFramework>
-    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
+  <ItemGroup>
+    <PackageReference Include="SixLabors.ImageSharp" Version="3.1.7" />
+  </ItemGroup>
   <ItemGroup>
     <None Include="libnav_c_api.so">
       <CopyToOutputDirectory>Always</CopyToOutputDirectory>

examples/NavigationExample/Program.cs

@@ -1,367 +1,25 @@
 using System;
+using System.Globalization;
+using System.IO;
 using System.Runtime.InteropServices;
 using System.Runtime.CompilerServices;
+using System.Text.RegularExpressions;
+using SixLabors.ImageSharp;
+using SixLabors.ImageSharp.PixelFormats;
+using NavigationExample;
+
 
 namespace NavigationExample
 {
-    /// <summary>
+    /// <summary>Thông tin map đọc từ file YAML (maze.yaml).</summary>
-    /// C# P/Invoke wrapper for Navigation C API
+    public struct MazeMapConfig
-    /// </summary>
-    public class NavigationAPI
     {
-        private const string DllName = "libnav_c_api.so"; // Linux
+        public string ImagePath;       // đường dẫn đầy đủ tới file ảnh
-        // For Windows: "nav_c_api.dll"
+        public float Resolution;
-        // For macOS: "libnav_c_api.dylib"
+        public double OriginX, OriginY, OriginZ;
-
+        public int Negate;            // 0 hoặc 1
-        // ============================================================================
+        public double OccupiedThresh;
-        // Enums
+        public double FreeThresh;
-        // ============================================================================
-        public enum NavigationState
-        {
-            Pending = 0,
-            Active = 1,
-            Preempted = 2,
-            Succeeded = 3,
-            Aborted = 4,
-            Rejected = 5,
-            Preempting = 6,
-            Recalling = 7,
-            Recalled = 8,
-            Lost = 9,
-            Planning = 10,
-            Controlling = 11,
-            Clearing = 12,
-            Paused = 13
-        }
-
-        // ============================================================================
-        // Structures
-        // ============================================================================
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Point
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose2D
-        {
-            public double x;
-            public double y;
-            public double theta;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NavFeedback
-        {
-            public NavigationState navigation_state;
-            public IntPtr feed_back_str;  // char*; free with nav_c_api_free_string
-            public Pose2D current_pose;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool goal_checked;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool is_ready;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Twist2D
-        {
-            public double x;
-            public double y;
-            public double theta;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Quaternion
-        {
-            public double x;
-            public double y;
-            public double z;
-            public double w;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Position
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose
-        {
-            public Position position;
-            public Quaternion orientation;
-        }
-
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Vector3
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Twist
-        {
-            public Vector3 linear;
-            public Vector3 angular;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Header
-        {
-            public uint seq;
-            public uint sec;
-            public uint nsec;
-            public IntPtr frame_id; // char*
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct PoseStamped
-        {
-            public Header header;
-            public Pose pose;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Twist2DStamped
-        {
-            public Header header;
-            public Twist2D velocity;
-        }
-
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NavigationHandle
-        {
-            public IntPtr ptr;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct TFListenerHandle
-        {
-            public IntPtr ptr;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct LaserScan
-        {
-            public Header header;
-            public float angle_min;
-            public float angle_max;
-            public float angle_increment;
-            public float time_increment;
-            public float scan_time;
-            public float range_min;
-            public float range_max;
-            public IntPtr ranges;
-            public UIntPtr ranges_count;
-            public IntPtr intensities;
-            public UIntPtr intensities_count;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct PoseWithCovariance
-        {
-            public Pose pose;
-            public IntPtr covariance;
-            public UIntPtr covariance_count;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct TwistWithCovariance    {
-            public Twist twist;
-            public IntPtr covariance;
-            public UIntPtr covariance_count;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Odometry
-        {
-            public Header header;
-            public IntPtr child_frame_id;
-            public PoseWithCovariance pose;
-            public TwistWithCovariance twist;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct OccupancyGrid
-        {
-            public Header header;
-            public MapMetaData info;
-            public IntPtr data;
-            public UIntPtr data_count;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct MapMetaData
-        {
-            public Time map_load_time;
-            public float resolution;
-            public uint width;
-            public uint height;
-            public Pose origin;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Time
-        {
-            public uint sec;
-            public uint nsec;
-        }
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern Header header_create(string frame_id);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern Header header_set_data(
-            uint seq,
-            uint sec,
-            uint nsec,
-            string frame_id);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern Time time_create();
-
-        /// <summary>Free a string allocated by the API (strdup).</summary>
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void nav_c_api_free_string(IntPtr str);
-
-        /// <summary>Convert NavigationState to string; caller must free with nav_c_api_free_string.</summary>
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern IntPtr navigation_state_to_string(NavigationState state);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
-
-        /// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
-        public static string MarshalString(IntPtr p)
-        {
-            if (p == IntPtr.Zero) return string.Empty;
-            return Marshal.PtrToStringAnsi(p) ?? string.Empty;
-        }
-
-        /// <summary>Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done.</summary>
-        public static void navigation_free_feedback(ref NavFeedback feedback)
-        {
-            if (feedback.feed_back_str != IntPtr.Zero)
-            {
-                nav_c_api_free_string(feedback.feed_back_str);
-                feedback.feed_back_str = IntPtr.Zero;
-            }
-        }
-
-        // ============================================================================
-        // TF Listener Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern TFListenerHandle tf_listener_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void tf_listener_destroy(TFListenerHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool tf_listener_set_static_transform(
-            TFListenerHandle tf_handle,
-            string parent_frame,
-            string child_frame,
-            double x, double y, double z,
-            double qx, double qy, double qz, double qw);
-
-        // ============================================================================
-        // Navigation Handle Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern NavigationHandle navigation_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_destroy(NavigationHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_straight_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_rotate_to(NavigationHandle handle, PoseStamped goal, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_pause(NavigationHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_resume(NavigationHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_cancel(NavigationHandle handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist);
-        // ============================================================================
-        // Navigation Data Management
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan);
-    
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);
-    
     }
 
     // ============================================================================
@@ -369,6 +27,105 @@ namespace NavigationExample
     // ============================================================================
     class Program
     {
+        /// <summary>
+        /// Đọc file maze.yaml (image, resolution, origin, negate, occupied_thresh, free_thresh).
+        /// ImagePath được resolve tuyệt đối theo thư mục chứa file yaml.
+        /// </summary>
+        static bool TryLoadMazeYaml(string yamlPath, out MazeMapConfig config)
+        {
+            config = default;
+            if (string.IsNullOrEmpty(yamlPath) || !File.Exists(yamlPath))
+                return false;
+            string dir = Path.GetDirectoryName(Path.GetFullPath(yamlPath)) ?? "";
+            try
+            {
+                string[] lines = File.ReadAllLines(yamlPath);
+                foreach (string line in lines)
+                {
+                    string trimmed = line.Trim();
+                    if (trimmed.Length == 0 || trimmed.StartsWith("#")) continue;
+                    int colon = trimmed.IndexOf(':');
+                    if (colon <= 0) continue;
+                    string key = trimmed.Substring(0, colon).Trim();
+                    string value = trimmed.Substring(colon + 1).Trim();
+                    if (key == "image")
+                        config.ImagePath = Path.Combine(dir, value);
+                    else if (key == "resolution" && float.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out float res))
+                        config.Resolution = res;
+                    else if (key == "origin")
+                    {
+                        var m = Regex.Match(value, @"\[\s*([-\d.]+\s*),\s*([-\d.]+\s*),\s*([-\d.]+)\s*\]");
+                        if (m.Success)
+                        {
+                            config.OriginX = double.Parse(m.Groups[1].Value, CultureInfo.InvariantCulture);
+                            config.OriginY = double.Parse(m.Groups[2].Value, CultureInfo.InvariantCulture);
+                            config.OriginZ = double.Parse(m.Groups[3].Value, CultureInfo.InvariantCulture);
+                        }
+                    }
+                    else if (key == "negate" && int.TryParse(value, out int neg))
+                        config.Negate = neg;
+                    else if (key == "occupied_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ot))
+                        config.OccupiedThresh = ot;
+                    else if (key == "free_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ft))
+                        config.FreeThresh = ft;
+                }
+                return !string.IsNullOrEmpty(config.ImagePath);
+            }
+            catch
+            {
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// Đọc file ảnh PNG và chuyển pixel sang byte[] occupancy (0=free, 100=occupied, 255=unknown).
+        /// Dùng negate, occupied_thresh, free_thresh từ maze.yaml nếu cung cấp.
+        /// </summary>
+        static byte[] LoadMazePixelsAsOccupancy(string imagePath, out int width, out int height,
+            int negate = 0, double occupiedThresh = 0.65, double freeThresh = 0.196)
+        {
+            width = 0;
+            height = 0;
+            if (string.IsNullOrEmpty(imagePath) || !File.Exists(imagePath))
+            {
+                LogError($"File not found: {imagePath}");
+                return null;
+            }
+            try
+            {
+                using var image = Image.Load<Rgba32>(imagePath);
+                int w = image.Width;
+                int h = image.Height;
+                width = w;
+                height = h;
+                byte[] data = new byte[w * h];
+                image.ProcessPixelRows(accessor =>
+                {
+                    for (int y = 0; y < accessor.Height; y++)
+                    {
+                        Span<Rgba32> row = accessor.GetRowSpan(y);
+                        for (int x = 0; x < row.Length; x++)
+                        {
+                            ref readonly Rgba32 p = ref row[x];
+                            double gray = (p.R + p.G + p.B) / 255.0 / 3.0;
+                            if (negate != 0) gray = 1.0 - gray;
+                            byte occ;
+                            if (gray <= freeThresh) occ = 0;
+                            else if (gray >= occupiedThresh) occ = 100;
+                            else occ = 255; // unknown
+                            data[y * w + x] = occ;
+                        }
+                    }
+                });
+                return data;
+            }
+            catch (Exception ex)
+            {
+                LogError($"Load image failed: {ex.Message}");
+                return null;
+            }
+        }
+
         // Helper method để hiển thị file và line number tự động
         static void LogError(string message,
             [CallerFilePath] string filePath = "",
@@ -382,87 +139,81 @@ namespace NavigationExample
 
         static void Main(string[] args)
         {
-            // Create TF listener
+            // Create tf3 buffer (replaces TF listener; used for all static transforms and navigation init)
-            NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create();
+            IntPtr tf3Buffer = TF3API.tf3_buffer_create(10);
-            if (tfHandle.ptr == IntPtr.Zero)
+            if (tf3Buffer == IntPtr.Zero)
             {
-                LogError("Failed to create TF listener");
+                LogError("Failed to create tf3 buffer (libtf3.so may be missing)");
+                return;
+            }
+            Console.WriteLine($"[NavigationExample] TF3 buffer created, handle = 0x{tf3Buffer.ToInt64():X16}");
+            string version = Marshal.PtrToStringAnsi(TF3API.tf3_get_version()) ?? "?";
+            Console.WriteLine($"[TF3] {version}");
+
+            // Inject static transforms: map -> odom -> base_footprint -> base_link
+            var tMapOdom = TF3API.CreateStaticTransform("map", "odom", 0, 0, 0, 0, 0, 0, 1);
+            var tOdomFoot = TF3API.CreateStaticTransform("odom", "base_footprint", 0, 0, 0, 0, 0, 0, 1);
+            var tFootLink = TF3API.CreateStaticTransform("base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1);
+            if (!TF3API.tf3_set_transform(tf3Buffer, ref tMapOdom, "NavigationExample", true) ||
+                !TF3API.tf3_set_transform(tf3Buffer, ref tOdomFoot, "NavigationExample", true) ||
+                !TF3API.tf3_set_transform(tf3Buffer, ref tFootLink, "NavigationExample", true))
+            {
+                LogError("Failed to set static TF");
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
-            // Inject a static TF so costmap can immediately canTransform(map <-> base_link).
+            // Create navigation instance and initialize with tf3 buffer
-            // 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
             NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
             if (navHandle.ptr == IntPtr.Zero)
             {
                 LogError("Failed to create navigation instance");
-                NavigationAPI.tf_listener_destroy(tfHandle);
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
-            // Initialize navigation
+            if (!NavigationAPI.navigation_initialize(navHandle, tf3Buffer))
-            if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
             {
-                LogError("Failed to initialize navigation");
+                LogError("Failed to initialize navigation with tf3 buffer");
                 NavigationAPI.navigation_destroy(navHandle);
-                NavigationAPI.tf_listener_destroy(tfHandle);
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
+            while (true)
+            {
+                NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
+                if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
+                {
+                    if (feedback.is_ready)
+                    {
+                        Console.WriteLine("Navigation is ready");
+                        break;
+                    }
+                    else
+                    {
+                        Console.WriteLine("Navigation is not ready");
+                    }
+                }
+                System.Threading.Thread.Sleep(100);
+            }
+            Console.WriteLine("[NavigationExample] Navigation initialized successfully");
+
+
             // Set robot footprint
-            NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
+            Point[] footprint = new Point[]
             {
-                new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 },
+                new Point { x = 0.3, y = -0.2, z = 0.0 },
-                new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 },
+                new Point { x = 0.3, y = 0.2, z = 0.0 },
-                new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 },
+                new Point { x = -0.3, y = 0.2, z = 0.0 },
-                new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 }
+                new Point { x = -0.3, y = -0.2, z = 0.0 }
             };
             NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
 
-            // Get navigation feedback
-            NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
-            if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
-            {
-                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));
+            Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
-            NavigationAPI.LaserScan fscanHandle;
+            LaserScan fscanHandle;
             fscanHandle.header = fscanHeader;
             fscanHandle.angle_min = -1.57f;
             fscanHandle.angle_max = 1.57f;
@@ -479,10 +230,9 @@ namespace NavigationExample
             fscanHandle.intensities_count = new UIntPtr(5);
             NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan",  fscanHandle);
 
-
             IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan");
-            NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
+            Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
-            NavigationAPI.LaserScan bscanHandle;
+            LaserScan bscanHandle;
             bscanHandle.header = bscanHeader;
             bscanHandle.angle_min = 1.57f;
             bscanHandle.angle_max = -1.57f;
@@ -499,11 +249,9 @@ namespace NavigationExample
             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));
+            Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
-            NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry();
+            Odometry odometryHandle = new Odometry();
             odometryHandle.header = odometryHeader;
             IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint");
             odometryHandle.child_frame_id = childFrameId;
@@ -538,45 +286,212 @@ namespace NavigationExample
             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: đọc maze.yaml rồi load ảnh và cập nhật mapMetaData
-            // Add static map
+            string mapYamlPath =  "maze.yaml";
-            NavigationAPI.Time mapLoadTime = NavigationAPI.time_create();
+            int mapWidth, mapHeight;
-            NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData();
+            byte[] data;
+            MazeMapConfig mapConfig;
+            if (TryLoadMazeYaml(mapYamlPath, out mapConfig))
+            {
+                data = LoadMazePixelsAsOccupancy(mapConfig.ImagePath, out mapWidth, out mapHeight,
+                    mapConfig.Negate, mapConfig.OccupiedThresh, mapConfig.FreeThresh);
+                if (data == null)
+                {
+                    mapWidth = 3;
+                    mapHeight = 10;
+                    data = new byte[30];
+                    for (int i = 0; i < data.Length; i++) data[i] = 100;
+                    Console.WriteLine("YAML loaded but image failed; using default map 3x10");
+                }
+                else
+                {
+                    Console.WriteLine("Loaded map from {0}: {1}x{2}, resolution={3}, origin=({4},{5},{6})",
+                        mapConfig.ImagePath, mapWidth, mapHeight, mapConfig.Resolution,
+                        mapConfig.OriginX, mapConfig.OriginY, mapConfig.OriginZ);
+                }
+            }
+            else
+            {
+                mapWidth = 3;
+                mapHeight = 10;
+                data = new byte[30];
+                for (int i = 0; i < data.Length; i++) data[i] = 100;
+                mapConfig = default;
+                mapConfig.Resolution = 0.05f;
+                mapConfig.OriginX = mapConfig.OriginY = mapConfig.OriginZ = 0.0;
+                Console.WriteLine("maze.yaml not found, using default map 3x10");
+            }
+
+            Time mapLoadTime = NavigationAPI.time_create();
+            MapMetaData mapMetaData = new MapMetaData();
             mapMetaData.map_load_time = mapLoadTime;
-            mapMetaData.resolution = 0.05f;
+            mapMetaData.resolution = mapConfig.Resolution;
-            mapMetaData.width = 3;
+            mapMetaData.width = (uint)mapWidth;
-            mapMetaData.height = 10;
+            mapMetaData.height = (uint)mapHeight;
-            mapMetaData.origin = new NavigationAPI.Pose();
+            mapMetaData.origin = new Pose();
-            mapMetaData.origin.position.x = 0.0;
+            mapMetaData.origin.position.x = mapConfig.OriginX;
-            mapMetaData.origin.position.y = 0.0;
+            mapMetaData.origin.position.y = mapConfig.OriginY;
-            mapMetaData.origin.position.z = 0.0;
+            mapMetaData.origin.position.z = mapConfig.OriginZ;
             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();
+            OccupancyGrid occupancyGrid = new 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<NavigationAPI.OccupancyGrid>(),
+                Marshal.SizeOf<OccupancyGrid>(),
-                Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data"),
+                Marshal.OffsetOf<OccupancyGrid>("data"),
-                Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data_count"));
+                Marshal.OffsetOf<OccupancyGrid>("data_count"));
     
             NavigationAPI.navigation_add_static_map(navHandle, "/map",  occupancyGrid);
 
-            // Cleanup
+            Twist2DStamped twist = new Twist2DStamped();
+            if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
+            {
+                Console.WriteLine(
+                    "Twist: {0}, {1}, {2}, {3}", 
+                    NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
+            }  
+
+            // Build order (thao cách bom order): header + nodes + edges giống C++
+            Order order = new Order();
+            order.headerId = 1;
+            order.timestamp = Marshal.StringToHGlobalAnsi("2026-02-28 10:00:00");
+            order.version = Marshal.StringToHGlobalAnsi("1.0.0");
+            order.manufacturer = Marshal.StringToHGlobalAnsi("Manufacturer");
+            order.serialNumber = Marshal.StringToHGlobalAnsi("Serial Number");
+            order.orderId = Marshal.StringToHGlobalAnsi("Order ID");
+            order.orderUpdateId = 1;
+
+            // Nodes: giống for (auto node : order.nodes) { node_msg.nodeId = ...; node_msg.nodePosition.x = ...; order_msg.nodes.push_back(node_msg); }
+            int nodeCount = 1;
+            order.nodes = Marshal.AllocHGlobal(Marshal.SizeOf<Node>() * nodeCount);
+            order.nodes_count = new UIntPtr((uint)nodeCount);
+            Node node1 = new Node();
+            node1.nodeId = Marshal.StringToHGlobalAnsi("node-1");
+            node1.sequenceId = 0;
+            node1.nodeDescription = Marshal.StringToHGlobalAnsi("Goal node");
+            node1.released = 0;
+            node1.nodePosition.x = 1.0;
+            node1.nodePosition.y = 1.0;
+            node1.nodePosition.theta = 0.0;
+            node1.nodePosition.allowedDeviationXY = 0.1f;
+            node1.nodePosition.allowedDeviationTheta = 0.05f;
+            node1.nodePosition.mapId = Marshal.StringToHGlobalAnsi("map");
+            node1.nodePosition.mapDescription = Marshal.StringToHGlobalAnsi("");
+            node1.actions = IntPtr.Zero;
+            node1.actions_count = UIntPtr.Zero;
+            Marshal.StructureToPtr(node1, order.nodes, false);
+
+            // Edges: rỗng trong ví dụ này; nếu cần thì alloc và fill tương tự (edge_msg.edgeId, trajectory.controlPoints, ...)
+            order.edges = IntPtr.Zero;
+            order.edges_count = UIntPtr.Zero;
+            order.zoneSetId = Marshal.StringToHGlobalAnsi("");
+
+            PoseStamped goal = new PoseStamped();
+            goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
+            goal.pose.position.x = 0.01;
+            goal.pose.position.y = 0.01;
+            goal.pose.position.z = 0.0;
+            goal.pose.orientation.x = 0.0;
+            goal.pose.orientation.y = 0.0;
+            goal.pose.orientation.z = 0.0;
+            goal.pose.orientation.w = 1.0;
+
+
+            // Console.WriteLine("Docking to docking_point");
+            NavigationAPI.navigation_dock_to(navHandle, "charger", goal);
+
+            // NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);
+
+            // NavigationAPI.navigation_move_to_order(navHandle, order, goal);
+            NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
+            NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2);
+
+            // NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
+
+            while (true) 
+            {
+                System.Threading.Thread.Sleep(100);
+            //     NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
+            //     if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
+            //     {
+            //         if (feedback.navigation_state == NavigationAPI.NavigationState.Succeeded)
+            //         {
+            //             Console.WriteLine("Navigation is Succeeded");
+            //             break;
+            //         }
+            //     }
+
+            //     NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput();
+            //     if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData))
+            //     {
+            //         int n = (int)(uint)globalData.plan.poses_count;
+            //         int poseSize = Marshal.SizeOf<Pose2DStamped>();
+            //         for (int i = 0; i < n; i++)
+            //         {
+            //             IntPtr posePtr = IntPtr.Add(globalData.plan.poses, i * poseSize);
+            //             Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
+            //             double p_x = p.pose.x;
+            //             double p_y = p.pose.y;
+            //             double p_theta = p.pose.theta;
+            //             Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
+            //         }
+            //         if(globalData.is_costmap_updated) {
+            //             for(int i = 0; i < (int)(uint)globalData.costmap.data_count; i++) {
+            //                 byte cellValue = Marshal.ReadByte(globalData.costmap.data, i);
+            //                 Console.WriteLine("Costmap: {0} {1}", i, cellValue);
+            //             }
+            //         }
+            //         else {
+            //             Console.WriteLine("Global Costmap is not updated");
+            //         }
+            //     }
+
+            //     NavigationAPI.PlannerDataOutput localData = new NavigationAPI.PlannerDataOutput();
+            //     if(NavigationAPI.navigation_get_local_data(navHandle, ref localData)) 
+            //     {
+            //         int n = (int)(uint)localData.plan.poses_count;
+            //         int poseSize = Marshal.SizeOf<Pose2DStamped>();
+            //         for (int i = 0; i < n; i++)
+            //         {
+            //             IntPtr posePtr = IntPtr.Add(localData.plan.poses, i * poseSize);
+            //             Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
+            //             double p_x = p.pose.x;
+            //             double p_y = p.pose.y;
+            //             double p_theta = p.pose.theta;
+            //             Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
+            //         }
+            //         if(localData.is_costmap_updated) {
+            //             for(int i = 0; i < (int)(uint)localData.costmap.data_count; i++) {
+            //                 byte cellValue = Marshal.ReadByte(localData.costmap.data, i);
+            //                 Console.WriteLine("Costmap: {0} {1}", i, cellValue);
+            //             }
+            //         }
+            //         else {
+            //             Console.WriteLine("Local Costmap is not updated");
+            //         }
+            //     }
+            }
+            // Cleanup (destroy nav first, then tf3 buffer)
+
             NavigationAPI.navigation_destroy(navHandle);
-            NavigationAPI.tf_listener_destroy(tfHandle);
+            TF3API.tf3_buffer_destroy(tf3Buffer);
+            Console.WriteLine("Press any key to exit...");
+            try
+            {
+                Console.ReadKey(intercept: true);
+            }
+            catch (InvalidOperationException)
+            {
+                // Running without a real console (e.g. redirected/automated run).
+            }
         }
     }
 }
-

examples/NavigationExample/TF3API.cs

@@ -0,0 +1,133 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+
+   // ============================================================================
+    // TF3 C API - P/Invoke wrapper for libtf3 (tf3 BufferCore)
+    // ============================================================================
+    public static class TF3API
+    {
+        private const string Tf3DllName = "/usr/local/lib/libtf3.so"; // Linux; Windows: tf3.dll, macOS: libtf3.dylib
+
+        public enum TF3_ErrorCode
+        {
+            TF3_OK = 0,
+            TF3_ERROR_LOOKUP = 1,
+            TF3_ERROR_CONNECTIVITY = 2,
+            TF3_ERROR_EXTRAPOLATION = 3,
+            TF3_ERROR_INVALID_ARGUMENT = 4,
+            TF3_ERROR_TIMEOUT = 5,
+            TF3_ERROR_UNKNOWN = 99
+        }
+
+        [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
+        public struct TF3_Transform
+        {
+            public long timestamp_sec;
+            public long timestamp_nsec;
+            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
+            public string frame_id;
+            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
+            public string child_frame_id;
+            public double translation_x;
+            public double translation_y;
+            public double translation_z;
+            public double rotation_x;
+            public double rotation_y;
+            public double rotation_z;
+            public double rotation_w;
+        }
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern IntPtr tf3_buffer_create(int cache_time_sec);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_buffer_destroy(IntPtr buffer);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_set_transform(
+            IntPtr buffer,
+            ref TF3_Transform transform,
+            string authority,
+            [MarshalAs(UnmanagedType.I1)] bool is_static);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_lookup_transform(
+            IntPtr buffer,
+            string target_frame,
+            string source_frame,
+            long time_sec,
+            long time_nsec,
+            out TF3_Transform transform,
+            out TF3_ErrorCode error_code);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_lookup_transform_full(
+            IntPtr buffer,
+            string target_frame,
+            long target_time_sec,
+            long target_time_nsec,
+            string source_frame,
+            long source_time_sec,
+            long source_time_nsec,
+            string fixed_frame,
+            out TF3_Transform transform,
+            out TF3_ErrorCode error_code);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_can_transform(
+            IntPtr buffer,
+            string target_frame,
+            string source_frame,
+            long time_sec,
+            long time_nsec,
+            System.Text.StringBuilder error_msg,
+            int error_msg_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern int tf3_get_all_frame_names(IntPtr buffer, System.Text.StringBuilder frames, int frames_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_get_frame_tree(IntPtr buffer, System.Text.StringBuilder output, int output_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_clear(IntPtr buffer);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_get_current_time(out long sec, out long nsec);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_get_last_error(IntPtr buffer, System.Text.StringBuilder error_msg, int error_msg_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern IntPtr tf3_get_version();
+
+        /// <summary>Helper: create TF3_Transform for static transform (identity or given pose).</summary>
+        public static TF3_Transform CreateStaticTransform(string parentFrame, string childFrame,
+            double tx = 0, double ty = 0, double tz = 0,
+            double qx = 0, double qy = 0, double qz = 0, double qw = 1)
+        {
+            var t = new TF3_Transform();
+            t.timestamp_sec = 0;
+            t.timestamp_nsec = 0;
+            t.frame_id = parentFrame ?? "";
+            t.child_frame_id = childFrame ?? "";
+            t.translation_x = tx;
+            t.translation_y = ty;
+            t.translation_z = tz;
+            t.rotation_x = qx;
+            t.rotation_y = qy;
+            t.rotation_z = qz;
+            t.rotation_w = qw;
+            return t;
+        }
+    }
+}

examples/NavigationExample/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

examples/NavigationExample/msgs/CommonMsgs.cs

@@ -0,0 +1,274 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+    // ============================================================================
+    // Structures
+    // ============================================================================
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Point
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose2D
+    {
+        public double x;
+        public double y;
+        public double theta;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist2D
+    {
+        public double x;
+        public double y;
+        public double theta;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Quaternion
+    {
+        public double x;
+        public double y;
+        public double z;
+        public double w;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Position
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose
+    {
+        public Point position;
+        public Quaternion orientation;
+    }
+
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Vector3
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist
+    {
+        public Vector3 linear;
+        public Vector3 angular;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Header
+    {
+        public uint seq;
+        public uint sec;
+        public uint nsec;
+        public IntPtr frame_id; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PoseStamped
+    {
+        public Header header;
+        public Pose pose;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist2DStamped
+    {
+        public Header header;
+        public Twist2D velocity;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct LaserScan
+    {
+        public Header header;
+        public float angle_min;
+        public float angle_max;
+        public float angle_increment;
+        public float time_increment;
+        public float scan_time;
+        public float range_min;
+        public float range_max;
+        public IntPtr ranges;
+        public UIntPtr ranges_count;
+        public IntPtr intensities;
+        public UIntPtr intensities_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PointCloud
+    {
+        public Header header;
+        public IntPtr points;
+        public UIntPtr points_count;
+        public IntPtr channels;
+        public UIntPtr channels_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PointCloud2
+    {
+        public Header header;
+        public uint height;
+        public uint width;
+        public IntPtr fields;
+        public UIntPtr fields_count;
+        [MarshalAs(UnmanagedType.I1)]
+        public bool is_bigendian;
+        public uint point_step;
+        public uint row_step;
+        public IntPtr data;
+        public UIntPtr data_count;
+        [MarshalAs(UnmanagedType.I1)]
+        public bool is_dense;
+    } 
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PoseWithCovariance
+    {
+        public Pose pose;
+        public IntPtr covariance;
+        public UIntPtr covariance_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct TwistWithCovariance    {
+        public Twist twist;
+        public IntPtr covariance;
+        public UIntPtr covariance_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Odometry
+    {
+        public Header header;
+        public IntPtr child_frame_id;
+        public PoseWithCovariance pose;
+        public TwistWithCovariance twist;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct OccupancyGrid
+    {
+        public Header header;
+        public MapMetaData info;
+        public IntPtr data;
+        public UIntPtr data_count;
+    }
+
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct MapMetaData
+    {
+        public Time map_load_time;
+        public float resolution;
+        public uint width;
+        public uint height;
+        public Pose origin;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Time
+    {
+        public uint sec;
+        public uint nsec;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Point32
+    {
+        public float x;
+        public float y;
+        public float z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose2DStamped
+    {
+        public Header header;
+        public Pose2D pose;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Polygon
+    {
+        public IntPtr points;
+        public UIntPtr points_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PolygonStamped
+    {
+        public Header header;
+        public Polygon polygon;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct OccupancyGridUpdate
+    {
+        public Header header;
+        public int x;
+        public int y;
+        public uint width;
+        public uint height;
+        public IntPtr data;
+        public UIntPtr data_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Path2D
+    {
+        public Header header;
+        public IntPtr poses;
+        public UIntPtr poses_count;
+    }
+
+    /// <summary>Name (char*) + OccupancyGrid for static map list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedOccupancyGrid
+    {
+        public IntPtr name;
+        public OccupancyGrid grid;
+    }
+
+    /// <summary>Name (char*) + LaserScan for laser scan list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedLaserScan
+    {
+        public IntPtr name;
+        public LaserScan scan;
+    }
+
+    /// <summary>Name (char*) + PointCloud for point cloud list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedPointCloud
+    {
+        public IntPtr name;
+        public PointCloud cloud;
+    }
+
+    /// <summary>Name (char*) + PointCloud2 for point cloud2 list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedPointCloud2
+    {
+        public IntPtr name;
+        public PointCloud2 cloud;
+    }
+
+
+}

examples/NavigationExample/msgs/ProtocolMsgsTypes.cs

@@ -0,0 +1,109 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+    /// <summary>
+    /// C# struct layout cho protocol_msgs (robot_protocol_msgs C API).
+    /// Khớp với pnkx_nav_core/src/APIs/c_api/include/protocol_msgs/*.h
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct ControlPoint
+    {
+        public double x;
+        public double y;
+        public double weight;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct ActionParameter
+    {
+        public IntPtr key;   // char*
+        public IntPtr value; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NodePosition
+    {
+        public double x;
+        public double y;
+        public double theta;
+        public float allowedDeviationXY;
+        public float allowedDeviationTheta;
+        public IntPtr mapId;          // char*
+        public IntPtr mapDescription; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Trajectory
+    {
+        public uint degree;
+        public IntPtr knotVector;           // double*
+        public UIntPtr knotVector_count;
+        public IntPtr controlPoints;        // ControlPoint*
+        public UIntPtr controlPoints_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Action
+    {
+        public IntPtr actionType;       // char*
+        public IntPtr actionId;         // char*
+        public IntPtr actionDescription;// char*
+        public IntPtr blockingType;     // char*
+        public IntPtr actionParameters; // ActionParameter*
+        public UIntPtr actionParameters_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Node
+    {
+        public IntPtr nodeId;           // char*
+        public int sequenceId;
+        public IntPtr nodeDescription;  // char*
+        public byte released;
+        public NodePosition nodePosition;
+        public IntPtr actions;          // Action*
+        public UIntPtr actions_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Edge
+    {
+        public IntPtr edgeId;           // char*
+        public int sequenceId;
+        public IntPtr edgeDescription; // char*
+        public byte released;
+        public IntPtr startNodeId;     // char*
+        public IntPtr endNodeId;       // char*
+        public double maxSpeed;
+        public double maxHeight;
+        public double minHeight;
+        public double orientation;
+        public IntPtr orientationType; // char*
+        public IntPtr direction;       // char*
+        public byte rotationAllowed;
+        public double maxRotationSpeed;
+        public Trajectory trajectory;
+        public double length;
+        public IntPtr actions;          // Action*
+        public UIntPtr actions_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Order
+    {
+        public int headerId;
+        public IntPtr timestamp;        // char*
+        public IntPtr version;         // char*
+        public IntPtr manufacturer;    // char*
+        public IntPtr serialNumber;    // char*
+        public IntPtr orderId;         // char*
+        public uint orderUpdateId;
+        public IntPtr nodes;           // Node*
+        public UIntPtr nodes_count;
+        public IntPtr edges;           // Edge*
+        public UIntPtr edges_count;
+        public IntPtr zoneSetId;      // char*
+    }
+}

examples/run_example.sh

@@ -27,6 +27,7 @@ echo "Building C API library..."
 cd "$BUILD_DIR"
 cmake ..
 make 
+sudo make install
 echo "Library built successfully!"
 
 
@@ -83,9 +84,9 @@ fi
 # Luôn copy source code mới nhất (cập nhật file nếu đã có)
 cd "$EXAMPLE_DIR/NavigationExample"
 
-# Bước 3: Copy library
+# # Bước 3: Copy library
-echo "Copying library..."
+# echo "Copying library..."
-cp "$LIB_DIR/libnav_c_api.so" .
+# cp "$LIB_DIR/libnav_c_api.so" .
 
 # Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
 # Main build directory (contains libtf3.so, librobot_cpp.so, etc.)

pnkx_nav_core.sln

@@ -0,0 +1,29 @@
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio Version 17
+VisualStudioVersion = 17.5.2.0
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "examples", "examples", "{B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "NavigationExample", "examples\NavigationExample\NavigationExample.csproj", "{995839D6-1E72-F444-6587-97EF24F93814}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(NestedProjects) = preSolution
+		{995839D6-1E72-F444-6587-97EF24F93814} = {B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}
+	EndGlobalSection
+	GlobalSection(ExtensibilityGlobals) = postSolution
+		SolutionGuid = {1A56EED1-5E6A-48A6-9AEE-F39361B34305}
+	EndGlobalSection
+EndGlobal

src/APIs/c_api/CMakeLists.txt

@@ -13,13 +13,17 @@ endif()
 # Find Boost (filesystem needed for plugin path / boost::dll usage)
 find_package(Boost REQUIRED COMPONENTS filesystem system)
 
-# Dependencies
+# Dependencies (robot_nav_2d_utils_conversions provides poseStampedToPose2D used by move_base_core/navigation.h)
 set(PACKAGES_DIR
     move_base_core
+    robot_nav_2d_utils
     tf3
     robot_time
     robot_cpp
     geometry_msgs
+    angles
+    data_convert
+    robot_protocol_msgs
 )
 
 # Thư mục include

src/APIs/c_api/include/convertor.h

@@ -13,6 +13,10 @@
 #include "geometry_msgs/Pose2D.h"
 #include "nav_2d_msgs/Twist2D.h"
 #include "nav_2d_msgs/Twist2DStamped.h"
+#include "nav_2d_msgs/Path2D.h"
+#include "map_msgs/OccupancyGridUpdate.h"
+#include "geometry_msgs/PolygonStamped.h"
+#include "protocol_msgs/Order.h"
 
 // C++
 #include <robot_sensor_msgs/LaserScan.h>
@@ -24,6 +28,10 @@
 #include <robot_geometry_msgs/Pose2D.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
+#include <robot_nav_2d_msgs/Path2D.h>
+#include <robot_map_msgs/OccupancyGridUpdate.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
+#include <robot_protocol_msgs/Order.h>
 
 #include <move_base_core/navigation.h>
 #include <move_base_core/common.h>
@@ -49,6 +57,21 @@ robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occu
  */
 void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out);
 
+/**
+ * @brief Convert C++ Path2D to C Path2D
+ */
+void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out);
+
+/**
+ * @brief Convert C++ OccupancyGridUpdate to C OccupancyGridUpdate
+ */
+void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out);
+
+/**
+ * @brief Convert C++ PolygonStamped to C PolygonStamped
+ */
+void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out);
+
 /**
  * @brief Convert C++ LaserScan to C LaserScan
  * @param cpp C++ LaserScan
@@ -150,4 +173,34 @@ robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped
  */
 Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped);
 
+/**
+ * @brief Convert C Order to C++ Order
+ * @param order C Order
+ * @return C++ Order
+ */
+robot_protocol_msgs::Order convert2CppOrder(const Order& order);
+
+/**
+ * @brief Convert C++ Order to C Order
+ * @param cpp_order C++ Order
+ * @return C Order (caller must call order_free when done to release memory)
+ */
+Order convert2COrder(const robot_protocol_msgs::Order& cpp_order);
+
+/**
+ * @brief Check if a quaternion is valid
+ * @param q Quaternion
+ * @return True if valid, false otherwise
+ */
+bool isQuaternionValid(const Quaternion& q);
+
+/**
+ * @brief Free dynamic memory held by an Order returned from convert2COrder
+ * @param order Order to free (pointers and counts are zeroed)
+ */
+#ifdef __cplusplus
+extern "C"
+#endif
+void order_free(Order* order);
+
 #endif // C_API_CONVERTOR_H

src/APIs/c_api/include/nav_c_api.h

@@ -24,21 +24,14 @@ extern "C"
   #include "sensor_msgs/PointCloud.h"
   #include "sensor_msgs/PointCloud2.h"
   #include "geometry_msgs/PolygonStamped.h"
+  #include "protocol_msgs/Order.h"
 
   typedef struct { char *name; OccupancyGrid grid; } NamedOccupancyGrid;
   typedef struct { char *name; LaserScan scan; } NamedLaserScan;
   typedef struct { char *name; PointCloud cloud; } NamedPointCloud;
   typedef struct { char *name; PointCloud2 cloud; } NamedPointCloud2;
-
+  typedef void* NavigationHandle;
-  typedef struct
+  typedef void* TFListenerHandle;
-  {
-    void *ptr;
-  } NavigationHandle;
-  
-  typedef struct
-  {
-    void *ptr;
-  } TFListenerHandle;
   
   typedef enum
   {
@@ -98,53 +91,18 @@ extern "C"
    */
   void navigation_destroy(NavigationHandle handle);
 
-  /**
-   * @brief Create a TF listener instance
-   * @return TF listener handle, or NULL on failure
-   */
-  TFListenerHandle tf_listener_create(void);
-
-  /**
-   * @brief Destroy a TF listener instance
-   * @param handle TF listener handle to destroy
-   */
-  void tf_listener_destroy(TFListenerHandle handle);
-
-  /**
-   * @brief Inject a static transform into the TF buffer.
-   *
-   * This is a convenience for standalone usage where no external TF publisher exists yet.
-   * It will create/ensure the frames exist and become transformable.
-   *
-   * @param tf_handle TF listener handle
-   * @param parent_frame Parent frame id (e.g. "map")
-   * @param child_frame Child frame id (e.g. "base_link")
-   * @param x Translation x (meters)
-   * @param y Translation y (meters)
-   * @param z Translation z (meters)
-   * @param qx Rotation quaternion x
-   * @param qy Rotation quaternion y
-   * @param qz Rotation quaternion z
-   * @param qw Rotation quaternion w
-   * @return true on success, false on failure
-   */
-  bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
-                                        const char *parent_frame,
-                                        const char *child_frame,
-                                        double x, double y, double z,
-                                        double qx, double qy, double qz, double qw);
-
   // ============================================================================
   // Navigation Interface Methods
   // ============================================================================
 
   /**
-   * @brief Initialize the navigation system
+   * @brief Initialize the navigation system using an existing tf3 buffer (from libtf3 tf3_buffer_create).
+   * Caller retains ownership of the buffer and must call tf3_buffer_destroy when done (after navigation_destroy).
    * @param handle Navigation handle
-   * @param tf_handle TF listener handle
+   * @param tf3_buffer Pointer to tf3 BufferCore (TF3_BufferCore from libtf3)
    * @return true on success, false on failure
    */
-  bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
+  bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf3_buffer);
 
   /**
    * @brief Set the robot's footprint (outline shape)
@@ -169,71 +127,79 @@ extern "C"
    * @brief Send a goal for the robot to navigate to
    * @param handle Navigation handle
    * @param goal Target pose in the global frame
-   * @param xy_goal_tolerance Acceptable error in X/Y (meters)
-   * @param yaw_goal_tolerance Acceptable angular error (radians)
    * @return true if goal was accepted and sent successfully
    */
-  bool navigation_move_to(NavigationHandle handle, const PoseStamped goal,
+  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
+   * @brief Send a goal for the robot to navigate to
-  //  * @param handle Navigation handle
+   * @param handle Navigation handle
-  //  * @param order Order message
+   * @param order Order message
-  //  * @param goal Target pose in the global frame
+   * @param goal Target pose in the global frame
-  //  * @param xy_goal_tolerance Acceptable error in X/Y (meters)
+   * @return true if goal was accepted and sent successfully
-  //  * @param yaw_goal_tolerance Acceptable angular error (radians)
+   * @note If order was obtained from convert2COrder(), call order_free(&order) when done
-  //  * @return true if goal was accepted and sent successfully
+   */
-  //  */
+  bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal);
-  // 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 nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+  bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
 
   /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle
    * @param marker Marker name or ID (null-terminated string)
    * @param goal Target pose for docking
-   * @param xy_goal_tolerance Acceptable XY error (meters)
-   * @param yaw_goal_tolerance Acceptable heading error (radians)
    * @return true if docking command succeeded
    */
-  bool navigation_dock_to(NavigationHandle handle, const char *marker,
+  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
+   * @return true if docking command succeeded
+   */
+  bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal);
+
+    /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param marker Marker name or ID (null-terminated string)
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+   bool navigation_dock_to_nodes_edges(NavigationHandle handle, const char *marker, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
 
-  // /**
-  //  * @brief Send a docking goal to a predefined marker
-  //  * @param handle Navigation handle
-  //  * @param order Order message
-  //  * @param goal Target pose for docking
-  //  * @param xy_goal_tolerance Acceptable XY error (meters)
-  //  * @param yaw_goal_tolerance Acceptable heading error (radians)
-  //  * @return true if docking command succeeded
-  //  */
-  // bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order,
-  //                               const PoseStamped &goal,
-  //                               double xy_goal_tolerance, double yaw_goal_tolerance);
 
   /**
    * @brief Move straight toward the target position
    * @param handle Navigation handle
-   * @param goal Target pose
+   * @param distance Distance to move (meters)
-   * @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 double distance);
-                                   double xy_goal_tolerance);
 
   /**
    * @brief Rotate in place to align with target orientation
    * @param handle Navigation handle
-   * @param goal Pose containing desired heading (only Z-axis used)
+   * @param goal_yaw Desired heading (radians)
-   * @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);
 
   /**
    * @brief Pause the robot's movement

src/APIs/c_api/include/protocol_msgs/Action.h

@@ -0,0 +1,30 @@
+#ifndef C_API_PROTOCOL_MSGS_ACTION_H
+#define C_API_PROTOCOL_MSGS_ACTION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/ActionParameter.h"
+
+#define PROTOCOL_MSGS_ACTION_BLOCKING_NONE "NONE"
+#define PROTOCOL_MSGS_ACTION_BLOCKING_SOFT "SOFT"
+#define PROTOCOL_MSGS_ACTION_BLOCKING_HARD "HARD"
+
+typedef struct
+{
+  char *actionType;
+  char *actionId;
+  char *actionDescription;
+  char *blockingType;
+  ActionParameter *actionParameters;
+  size_t actionParameters_count;
+} Action;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ACTION_H

src/APIs/c_api/include/protocol_msgs/ActionParameter.h

@@ -0,0 +1,21 @@
+#ifndef C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H
+#define C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  char *key;
+  char *value;
+} ActionParameter;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H

src/APIs/c_api/include/protocol_msgs/ControlPoint.h

@@ -0,0 +1,22 @@
+#ifndef C_API_PROTOCOL_MSGS_CONTROLPOINT_H
+#define C_API_PROTOCOL_MSGS_CONTROLPOINT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double weight;
+} ControlPoint;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_CONTROLPOINT_H

src/APIs/c_api/include/protocol_msgs/Edge.h

@@ -0,0 +1,42 @@
+#ifndef C_API_PROTOCOL_MSGS_EDGE_H
+#define C_API_PROTOCOL_MSGS_EDGE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/Trajectory.h"
+#include "protocol_msgs/Action.h"
+
+#define PROTOCOL_MSGS_EDGE_ORIENTATION_GLOBAL "GLOBAL"
+#define PROTOCOL_MSGS_EDGE_ORIENTATION_TANGENTIAL "TANGENTIAL"
+
+typedef struct
+{
+  char *edgeId;
+  int32_t sequenceId;
+  char *edgeDescription;
+  uint8_t released;
+  char *startNodeId;
+  char *endNodeId;
+  double maxSpeed;
+  double maxHeight;
+  double minHeight;
+  double orientation;
+  char *orientationType;
+  char *direction;
+  uint8_t rotationAllowed;
+  double maxRotationSpeed;
+  Trajectory trajectory;
+  double length;
+  Action *actions;
+  size_t actions_count;
+} Edge;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_EDGE_H

src/APIs/c_api/include/protocol_msgs/Error.h

@@ -0,0 +1,28 @@
+#ifndef C_API_PROTOCOL_MSGS_ERROR_H
+#define C_API_PROTOCOL_MSGS_ERROR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/ErrorReference.h"
+
+#define PROTOCOL_MSGS_ERROR_LEVEL_WARNING "WARNING"
+#define PROTOCOL_MSGS_ERROR_LEVEL_FATAL "FATAL"
+
+typedef struct
+{
+  char *errorType;
+  ErrorReference *errorReferences;
+  size_t errorReferences_count;
+  char *errorDescription;
+  char *errorLevel;
+} Error;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ERROR_H

src/APIs/c_api/include/protocol_msgs/ErrorReference.h

@@ -0,0 +1,21 @@
+#ifndef C_API_PROTOCOL_MSGS_ERRORREFERENCE_H
+#define C_API_PROTOCOL_MSGS_ERRORREFERENCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  char *referenceKey;
+  char *referenceValue;
+} ErrorReference;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ERRORREFERENCE_H

src/APIs/c_api/include/protocol_msgs/Info.h

@@ -0,0 +1,28 @@
+#ifndef C_API_PROTOCOL_MSGS_INFO_H
+#define C_API_PROTOCOL_MSGS_INFO_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/InfoReference.h"
+
+#define PROTOCOL_MSGS_INFO_LEVEL_DEBUG "DEBUG"
+#define PROTOCOL_MSGS_INFO_LEVEL_INFO "INFO"
+
+typedef struct
+{
+  char *infoType;
+  InfoReference *infoReferences;
+  size_t infoReferences_count;
+  char *infoDescription;
+  char *infoLevel;
+} Info;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_INFO_H

src/APIs/c_api/include/protocol_msgs/InfoReference.h

@@ -0,0 +1,21 @@
+#ifndef C_API_PROTOCOL_MSGS_INFOREFERENCE_H
+#define C_API_PROTOCOL_MSGS_INFOREFERENCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  char *referenceKey;
+  char *referenceValue;
+} InfoReference;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_INFOREFERENCE_H

src/APIs/c_api/include/protocol_msgs/Information.h

@@ -0,0 +1,22 @@
+#ifndef C_API_PROTOCOL_MSGS_INFORMATION_H
+#define C_API_PROTOCOL_MSGS_INFORMATION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/Info.h"
+
+typedef struct
+{
+  Info *information;
+  size_t information_count;
+} Information;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_INFORMATION_H

src/APIs/c_api/include/protocol_msgs/Node.h

@@ -0,0 +1,28 @@
+#ifndef C_API_PROTOCOL_MSGS_NODE_H
+#define C_API_PROTOCOL_MSGS_NODE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/NodePosition.h"
+#include "protocol_msgs/Action.h"
+
+typedef struct
+{
+  char *nodeId;
+  int32_t sequenceId;
+  char *nodeDescription;
+  uint8_t released;
+  NodePosition nodePosition;
+  Action *actions;
+  size_t actions_count;
+} Node;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_NODE_H

src/APIs/c_api/include/protocol_msgs/NodePosition.h

@@ -0,0 +1,26 @@
+#ifndef C_API_PROTOCOL_MSGS_NODEPOSITION_H
+#define C_API_PROTOCOL_MSGS_NODEPOSITION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double theta;
+  float allowedDeviationXY;
+  float allowedDeviationTheta;
+  char *mapId;
+  char *mapDescription;
+} NodePosition;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_NODEPOSITION_H

src/APIs/c_api/include/protocol_msgs/Order.h

@@ -0,0 +1,33 @@
+#ifndef C_API_PROTOCOL_MSGS_ORDER_H
+#define C_API_PROTOCOL_MSGS_ORDER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/Node.h"
+#include "protocol_msgs/Edge.h"
+
+typedef struct
+{
+  int32_t headerId;
+  char *timestamp;
+  char *version;
+  char *manufacturer;
+  char *serialNumber;
+  char *orderId;
+  uint32_t orderUpdateId;
+  Node *nodes;
+  size_t nodes_count;
+  Edge *edges;
+  size_t edges_count;
+  char *zoneSetId;
+} Order;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_ORDER_H

src/APIs/c_api/include/protocol_msgs/Trajectory.h

@@ -0,0 +1,25 @@
+#ifndef C_API_PROTOCOL_MSGS_TRAJECTORY_H
+#define C_API_PROTOCOL_MSGS_TRAJECTORY_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "protocol_msgs/ControlPoint.h"
+
+typedef struct
+{
+  uint32_t degree;
+  double *knotVector;
+  size_t knotVector_count;
+  ControlPoint *controlPoints;
+  size_t controlPoints_count;
+} Trajectory;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_PROTOCOL_MSGS_TRAJECTORY_H

src/APIs/c_api/src/convertor.cpp

@@ -10,8 +10,6 @@ robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& 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;
@@ -59,13 +57,10 @@ robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occu
 
   size_t data_size = occupancy_grid.data ? occupancy_grid.data_count : 0;
 
-  // robot_occupancy_grid.data.resize(data_size);
+  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_occupancy_grid.data[i] = occupancy_grid.data[i];
   }
-  robot::log_info("occupancy_grid.data: %s", ss.str().c_str());
   return robot_occupancy_grid;
 }
 
@@ -75,9 +70,9 @@ robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& 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.header.frame_id = odometry.header.frame_id ? odometry.header.frame_id : "base_link";
   
-  robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : "";
+  robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : "base_link";
   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;
@@ -136,7 +131,7 @@ PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pos
   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.header.frame_id = cpp_pose.header.frame_id.empty() ? nullptr : strdup(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;
@@ -162,7 +157,7 @@ Header convert2CHeader(const robot_std_msgs::Header& cpp_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();
+  c_header.frame_id = cpp_header.frame_id.empty() ? nullptr : strdup(cpp_header.frame_id.c_str());
   return c_header;
 }
 
@@ -206,6 +201,70 @@ void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyG
   }
 }
 
+static void convert2CPose2DStamped(const robot_nav_2d_msgs::Pose2DStamped& cpp, Pose2DStamped& out)
+{
+  out.header = convert2CHeader(cpp.header);
+  out.pose = convert2CPose2D(cpp.pose);
+}
+
+void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out)
+{
+  out.header = convert2CHeader(cpp.header);
+  out.poses_count = cpp.poses.size();
+  out.poses = nullptr;
+  if (out.poses_count > 0)
+  {
+    out.poses = static_cast<Pose2DStamped*>(malloc(out.poses_count * sizeof(Pose2DStamped)));
+    if (out.poses)
+    {
+      for (size_t i = 0; i < out.poses_count; i++)
+        convert2CPose2DStamped(cpp.poses[i], out.poses[i]);
+    }
+  }
+}
+
+void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out)
+{
+  out.header = convert2CHeader(cpp.header);
+  out.x = cpp.x;
+  out.y = cpp.y;
+  out.width = cpp.width;
+  out.height = cpp.height;
+  out.data_count = cpp.data.size();
+  out.data = nullptr;
+  if (out.data_count > 0)
+  {
+    out.data = static_cast<int8_t*>(malloc(out.data_count * sizeof(int8_t)));
+    if (out.data)
+      memcpy(out.data, cpp.data.data(), out.data_count * sizeof(int8_t));
+  }
+}
+
+static void convert2CPolygon(const robot_geometry_msgs::Polygon& cpp, Polygon& out)
+{
+  out.points_count = cpp.points.size();
+  out.points = nullptr;
+  if (out.points_count > 0)
+  {
+    out.points = static_cast<Point32*>(malloc(out.points_count * sizeof(Point32)));
+    if (out.points)
+    {
+      for (size_t i = 0; i < out.points_count; i++)
+      {
+        out.points[i].x = cpp.points[i].x;
+        out.points[i].y = cpp.points[i].y;
+        out.points[i].z = cpp.points[i].z;
+      }
+    }
+  }
+}
+
+void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out)
+{
+  out.header = convert2CHeader(cpp.header);
+  convert2CPolygon(cpp.polygon, out.polygon);
+}
+
 void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out)
 {
   out.header.seq = cpp.header.seq;
@@ -366,3 +425,408 @@ robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c)
     memcpy(cpp.data.data(), c.data, c.data_count);
   return cpp;
 }
+
+// --- protocol_msgs helpers (C <-> C++ robot_protocol_msgs) ---
+
+static inline char* dup_cstr(const char* s)
+{
+  if (!s) return nullptr;
+  const size_t n = std::strlen(s) + 1;
+  char* p = static_cast<char*>(std::malloc(n));
+  if (p) std::memcpy(p, s, n);
+  return p;
+}
+
+static robot_protocol_msgs::ActionParameter convert2CppActionParameter(const ActionParameter& c)
+{
+  robot_protocol_msgs::ActionParameter cpp;
+  cpp.key = c.key ? c.key : "";
+  cpp.value = c.value ? c.value : "";
+  return cpp;
+}
+
+static ActionParameter convert2CActionParameter(const robot_protocol_msgs::ActionParameter& cpp)
+{
+  ActionParameter c;
+  c.key = dup_cstr(cpp.key.c_str());
+  c.value = dup_cstr(cpp.value.c_str());
+  return c;
+}
+
+static robot_protocol_msgs::ControlPoint convert2CppControlPoint(const ControlPoint& c)
+{
+  robot_protocol_msgs::ControlPoint cpp;
+  cpp.x = c.x;
+  cpp.y = c.y;
+  cpp.weight = c.weight;
+  return cpp;
+}
+
+static ControlPoint convert2CControlPoint(const robot_protocol_msgs::ControlPoint& cpp)
+{
+  ControlPoint c;
+  c.x = cpp.x;
+  c.y = cpp.y;
+  c.weight = cpp.weight;
+  return c;
+}
+
+static robot_protocol_msgs::NodePosition convert2CppNodePosition(const NodePosition& c)
+{
+  robot_protocol_msgs::NodePosition cpp;
+  cpp.x = c.x;
+  cpp.y = c.y;
+  cpp.theta = c.theta;
+  cpp.allowedDeviationXY = c.allowedDeviationXY;
+  cpp.allowedDeviationTheta = c.allowedDeviationTheta;
+  cpp.mapId = c.mapId ? c.mapId : "";
+  cpp.mapDescription = c.mapDescription ? c.mapDescription : "";
+  return cpp;
+}
+
+static NodePosition convert2CNodePosition(const robot_protocol_msgs::NodePosition& cpp)
+{
+  NodePosition c;
+  c.x = cpp.x;
+  c.y = cpp.y;
+  c.theta = cpp.theta;
+  c.allowedDeviationXY = cpp.allowedDeviationXY;
+  c.allowedDeviationTheta = cpp.allowedDeviationTheta;
+  c.mapId = dup_cstr(cpp.mapId.c_str());
+  c.mapDescription = dup_cstr(cpp.mapDescription.c_str());
+  return c;
+}
+
+static robot_protocol_msgs::Trajectory convert2CppTrajectory(const Trajectory& c)
+{
+  robot_protocol_msgs::Trajectory cpp;
+  cpp.degree = c.degree;
+  const size_t kv = c.knotVector ? c.knotVector_count : 0;
+  cpp.knotVector.resize(kv);
+  for (size_t i = 0; i < kv; i++)
+    cpp.knotVector[i] = c.knotVector[i];
+  const size_t ncp = c.controlPoints ? c.controlPoints_count : 0;
+  cpp.controlPoints.resize(ncp);
+  for (size_t i = 0; i < ncp; i++)
+    cpp.controlPoints[i] = convert2CppControlPoint(c.controlPoints[i]);
+  return cpp;
+}
+
+static Trajectory convert2CTrajectory(const robot_protocol_msgs::Trajectory& cpp)
+{
+  Trajectory c;
+  c.degree = cpp.degree;
+  c.knotVector_count = cpp.knotVector.size();
+  c.knotVector = c.knotVector_count ? static_cast<double*>(std::malloc(c.knotVector_count * sizeof(double))) : nullptr;
+  if (c.knotVector)
+    for (size_t i = 0; i < c.knotVector_count; i++)
+      c.knotVector[i] = cpp.knotVector[i];
+  c.controlPoints_count = cpp.controlPoints.size();
+  c.controlPoints = c.controlPoints_count ? static_cast<ControlPoint*>(std::malloc(c.controlPoints_count * sizeof(ControlPoint))) : nullptr;
+  if (c.controlPoints)
+    for (size_t i = 0; i < c.controlPoints_count; i++)
+      c.controlPoints[i] = convert2CControlPoint(cpp.controlPoints[i]);
+  return c;
+}
+
+static robot_protocol_msgs::Action convert2CppAction(const Action& c)
+{
+  robot_protocol_msgs::Action cpp;
+  cpp.actionType = c.actionType ? c.actionType : "";
+  cpp.actionId = c.actionId ? c.actionId : "";
+  cpp.actionDescription = c.actionDescription ? c.actionDescription : "";
+  cpp.blockingType = c.blockingType ? c.blockingType : "";
+  const size_t n = c.actionParameters ? c.actionParameters_count : 0;
+  cpp.actionParameters.resize(n);
+  for (size_t i = 0; i < n; i++)
+    cpp.actionParameters[i] = convert2CppActionParameter(c.actionParameters[i]);
+  return cpp;
+}
+
+static Action convert2CAction(const robot_protocol_msgs::Action& cpp)
+{
+  Action c;
+  c.actionType = dup_cstr(cpp.actionType.c_str());
+  c.actionId = dup_cstr(cpp.actionId.c_str());
+  c.actionDescription = dup_cstr(cpp.actionDescription.c_str());
+  c.blockingType = dup_cstr(cpp.blockingType.c_str());
+  c.actionParameters_count = cpp.actionParameters.size();
+  c.actionParameters = c.actionParameters_count ? static_cast<ActionParameter*>(std::malloc(c.actionParameters_count * sizeof(ActionParameter))) : nullptr;
+  if (c.actionParameters)
+    for (size_t i = 0; i < c.actionParameters_count; i++)
+      c.actionParameters[i] = convert2CActionParameter(cpp.actionParameters[i]);
+  return c;
+}
+
+static robot_protocol_msgs::Node convert2CppNode(const Node& c)
+{
+  robot_protocol_msgs::Node cpp;
+  cpp.nodeId = c.nodeId ? c.nodeId : "";
+  cpp.sequenceId = c.sequenceId;
+  cpp.nodeDescription = c.nodeDescription ? c.nodeDescription : "";
+  cpp.released = c.released;
+  cpp.nodePosition = convert2CppNodePosition(c.nodePosition);
+  const size_t n = c.actions ? c.actions_count : 0;
+  cpp.actions.resize(n);
+  for (size_t i = 0; i < n; i++)
+    cpp.actions[i] = convert2CppAction(c.actions[i]);
+  return cpp;
+}
+
+static Node convert2CNode(const robot_protocol_msgs::Node& cpp)
+{
+  Node c;
+  c.nodeId = dup_cstr(cpp.nodeId.c_str());
+  c.sequenceId = cpp.sequenceId;
+  c.nodeDescription = dup_cstr(cpp.nodeDescription.c_str());
+  c.released = cpp.released;
+  c.nodePosition = convert2CNodePosition(cpp.nodePosition);
+  c.actions_count = cpp.actions.size();
+  c.actions = c.actions_count ? static_cast<Action*>(std::malloc(c.actions_count * sizeof(Action))) : nullptr;
+  if (c.actions)
+    for (size_t i = 0; i < c.actions_count; i++)
+      c.actions[i] = convert2CAction(cpp.actions[i]);
+  return c;
+}
+
+static robot_protocol_msgs::Edge convert2CppEdge(const Edge& c)
+{
+  robot_protocol_msgs::Edge cpp;
+  cpp.edgeId = c.edgeId ? c.edgeId : "";
+  cpp.sequenceId = c.sequenceId;
+  cpp.edgeDescription = c.edgeDescription ? c.edgeDescription : "";
+  cpp.released = c.released;
+  cpp.startNodeId = c.startNodeId ? c.startNodeId : "";
+  cpp.endNodeId = c.endNodeId ? c.endNodeId : "";
+  cpp.maxSpeed = c.maxSpeed;
+  cpp.maxHeight = c.maxHeight;
+  cpp.minHeight = c.minHeight;
+  cpp.orientation = c.orientation;
+  cpp.orientationType = c.orientationType ? c.orientationType : "";
+  cpp.direction = c.direction ? c.direction : "";
+  cpp.rotationAllowed = c.rotationAllowed;
+  cpp.maxRotationSpeed = c.maxRotationSpeed;
+  cpp.trajectory = convert2CppTrajectory(c.trajectory);
+  cpp.length = c.length;
+  const size_t n = c.actions ? c.actions_count : 0;
+  cpp.actions.resize(n);
+  for (size_t i = 0; i < n; i++)
+    cpp.actions[i] = convert2CppAction(c.actions[i]);
+  return cpp;
+}
+
+static Edge convert2CEdge(const robot_protocol_msgs::Edge& cpp)
+{
+  Edge c;
+  c.edgeId = dup_cstr(cpp.edgeId.c_str());
+  c.sequenceId = cpp.sequenceId;
+  c.edgeDescription = dup_cstr(cpp.edgeDescription.c_str());
+  c.released = cpp.released;
+  c.startNodeId = dup_cstr(cpp.startNodeId.c_str());
+  c.endNodeId = dup_cstr(cpp.endNodeId.c_str());
+  c.maxSpeed = cpp.maxSpeed;
+  c.maxHeight = cpp.maxHeight;
+  c.minHeight = cpp.minHeight;
+  c.orientation = cpp.orientation;
+  c.orientationType = dup_cstr(cpp.orientationType.c_str());
+  c.direction = dup_cstr(cpp.direction.c_str());
+  c.rotationAllowed = cpp.rotationAllowed;
+  c.maxRotationSpeed = cpp.maxRotationSpeed;
+  c.trajectory = convert2CTrajectory(cpp.trajectory);
+  c.length = cpp.length;
+  c.actions_count = cpp.actions.size();
+  c.actions = c.actions_count ? static_cast<Action*>(std::malloc(c.actions_count * sizeof(Action))) : nullptr;
+  if (c.actions)
+    for (size_t i = 0; i < c.actions_count; i++)
+      c.actions[i] = convert2CAction(cpp.actions[i]);
+  return c;
+}
+
+static void free_action_parameter(ActionParameter* p)
+{
+  if (!p) return;
+  std::free(p->key);
+  std::free(p->value);
+  p->key = nullptr;
+  p->value = nullptr;
+}
+
+static void free_action(Action* p)
+{
+  if (!p) return;
+  std::free(p->actionType);
+  std::free(p->actionId);
+  std::free(p->actionDescription);
+  std::free(p->blockingType);
+  p->actionType = p->actionId = p->actionDescription = p->blockingType = nullptr;
+  if (p->actionParameters) {
+    for (size_t i = 0; i < p->actionParameters_count; i++)
+      free_action_parameter(&p->actionParameters[i]);
+    std::free(p->actionParameters);
+    p->actionParameters = nullptr;
+  }
+  p->actionParameters_count = 0;
+}
+
+static void free_node_position(NodePosition* p)
+{
+  if (!p) return;
+  std::free(p->mapId);
+  std::free(p->mapDescription);
+  p->mapId = p->mapDescription = nullptr;
+}
+
+static void free_trajectory(Trajectory* p)
+{
+  if (!p) return;
+  std::free(p->knotVector);
+  p->knotVector = nullptr;
+  p->knotVector_count = 0;
+  std::free(p->controlPoints);
+  p->controlPoints = nullptr;
+  p->controlPoints_count = 0;
+}
+
+static void free_node(Node* p)
+{
+  if (!p) return;
+  std::free(p->nodeId);
+  std::free(p->nodeDescription);
+  p->nodeId = p->nodeDescription = nullptr;
+  free_node_position(&p->nodePosition);
+  if (p->actions) {
+    for (size_t i = 0; i < p->actions_count; i++)
+      free_action(&p->actions[i]);
+    std::free(p->actions);
+    p->actions = nullptr;
+  }
+  p->actions_count = 0;
+}
+
+static void free_edge(Edge* p)
+{
+  if (!p) return;
+  std::free(p->edgeId);
+  std::free(p->edgeDescription);
+  std::free(p->startNodeId);
+  std::free(p->endNodeId);
+  std::free(p->orientationType);
+  std::free(p->direction);
+  p->edgeId = p->edgeDescription = p->startNodeId = p->endNodeId = nullptr;
+  p->orientationType = p->direction = nullptr;
+  free_trajectory(&p->trajectory);
+  if (p->actions) {
+    for (size_t i = 0; i < p->actions_count; i++)
+      free_action(&p->actions[i]);
+    std::free(p->actions);
+    p->actions = nullptr;
+  }
+  p->actions_count = 0;
+}
+
+robot_protocol_msgs::Order convert2CppOrder(const Order& order)
+{
+  robot_protocol_msgs::Order cpp_order;
+  cpp_order.headerId = order.headerId;
+  cpp_order.timestamp = order.timestamp ? order.timestamp : "";
+  cpp_order.version = order.version ? order.version : "";
+  cpp_order.manufacturer = order.manufacturer ? order.manufacturer : "";
+  cpp_order.serialNumber = order.serialNumber ? order.serialNumber : "";
+  cpp_order.orderId = order.orderId ? order.orderId : "";
+  cpp_order.orderUpdateId = order.orderUpdateId;
+  const size_t nn = order.nodes ? order.nodes_count : 0;
+  cpp_order.nodes.resize(nn);
+  for (size_t i = 0; i < nn; i++)
+    cpp_order.nodes[i] = convert2CppNode(order.nodes[i]);
+  const size_t ne = order.edges ? order.edges_count : 0;
+  cpp_order.edges.resize(ne);
+  for (size_t i = 0; i < ne; i++)
+    cpp_order.edges[i] = convert2CppEdge(order.edges[i]);
+  cpp_order.zoneSetId = order.zoneSetId ? order.zoneSetId : "";
+  return cpp_order;
+}
+
+Order convert2COrder(const robot_protocol_msgs::Order& cpp_order)
+{
+  Order order;
+  order.headerId = cpp_order.headerId;
+  order.timestamp = dup_cstr(cpp_order.timestamp.c_str());
+  order.version = dup_cstr(cpp_order.version.c_str());
+  order.manufacturer = dup_cstr(cpp_order.manufacturer.c_str());
+  order.serialNumber = dup_cstr(cpp_order.serialNumber.c_str());
+  order.orderId = dup_cstr(cpp_order.orderId.c_str());
+  order.orderUpdateId = cpp_order.orderUpdateId;
+  order.nodes_count = cpp_order.nodes.size();
+  order.nodes = order.nodes_count ? static_cast<Node*>(std::malloc(order.nodes_count * sizeof(Node))) : nullptr;
+  if (order.nodes)
+    for (size_t i = 0; i < order.nodes_count; i++)
+      order.nodes[i] = convert2CNode(cpp_order.nodes[i]);
+  order.edges_count = cpp_order.edges.size();
+  order.edges = order.edges_count ? static_cast<Edge*>(std::malloc(order.edges_count * sizeof(Edge))) : nullptr;
+  if (order.edges)
+    for (size_t i = 0; i < order.edges_count; i++)
+      order.edges[i] = convert2CEdge(cpp_order.edges[i]);
+  order.zoneSetId = dup_cstr(cpp_order.zoneSetId.c_str());
+  return order;
+}
+
+extern "C" void order_free(Order* order)
+{
+  if (!order) return;
+  std::free(order->timestamp);
+  std::free(order->version);
+  std::free(order->manufacturer);
+  std::free(order->serialNumber);
+  std::free(order->orderId);
+  std::free(order->zoneSetId);
+  order->timestamp = order->version = order->manufacturer = nullptr;
+  order->serialNumber = order->orderId = order->zoneSetId = nullptr;
+  if (order->nodes) {
+    for (size_t i = 0; i < order->nodes_count; i++)
+      free_node(&order->nodes[i]);
+    std::free(order->nodes);
+    order->nodes = nullptr;
+  }
+  order->nodes_count = 0;
+  if (order->edges) {
+    for (size_t i = 0; i < order->edges_count; i++)
+      free_edge(&order->edges[i]);
+    std::free(order->edges);
+    order->edges = nullptr;
+  }
+  order->edges_count = 0;
+}
+
+bool isQuaternionValid(const Quaternion& q)
+{
+  // first we need to check if the quaternion has nan's or infs
+  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
+  {
+    robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
+    return false;
+  }
+
+  tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
+
+  // next, we need to check if the length of the quaternion is close to zero
+  if (tf_q.length2() < 1e-6)
+  {
+    robot::log_error("Quaternion has length close to zero... discarding as navigation goal");
+    return false;
+  }
+
+  // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
+  tf_q.normalize();
+
+  tf3::Vector3 up(0, 0, 1);
+
+  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
+
+  if (fabs(dot - 1) > 1e-3)
+  {
+    robot::log_error("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical. dot: %f", dot);
+    return false;
+  }
+
+  return true;
+}

src/Algorithms/Libraries/kalman/src/kalman.cpp

@@ -24,7 +24,15 @@ KalmanFilter::KalmanFilter(
   I.setIdentity();
 }
 
-KalmanFilter::KalmanFilter() {}
+KalmanFilter::KalmanFilter()
+  : m(0),
+    n(0),
+    t0(0.0),
+    t(0.0),
+    dt(0.0),
+    initialized(false)
+{
+}
 
 void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) {
   x_hat = x0;
@@ -47,11 +55,34 @@ void KalmanFilter::update(const Eigen::VectorXd& y) {
   if(!initialized)
     throw std::runtime_error("Filter is not initialized!");
 
+  if (y.size() != m)
+    throw std::runtime_error("KalmanFilter::update(): measurement vector has wrong size");
+
+  if (A.rows() != n || A.cols() != n || C.rows() != m || C.cols() != n ||
+      Q.rows() != n || Q.cols() != n || R.rows() != m || R.cols() != m ||
+      P.rows() != n || P.cols() != n)
+    throw std::runtime_error("KalmanFilter::update(): matrix dimensions are inconsistent");
+
   x_hat_new = A * x_hat;
   P = A*P*A.transpose() + Q;
-  K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
+
+  const Eigen::MatrixXd S = C * P * C.transpose() + R;
+  Eigen::LDLT<Eigen::MatrixXd> ldlt(S);
+  if (ldlt.info() != Eigen::Success)
+    throw std::runtime_error("KalmanFilter::update(): failed to decompose innovation covariance");
+
+  // K = P*C' * inv(S)  -> solve(S * X = (P*C')^T)
+  const Eigen::MatrixXd PCt = P * C.transpose();
+  const Eigen::MatrixXd KT = ldlt.solve(PCt.transpose());
+  if (ldlt.info() != Eigen::Success)
+    throw std::runtime_error("KalmanFilter::update(): failed to solve for Kalman gain");
+  K = KT.transpose();
+
   x_hat_new += K * (y - C*x_hat_new);
-  P = (I - K*C)*P;
+
+  // Joseph form: keeps P symmetric / PSD under numeric errors
+  const Eigen::MatrixXd IKC = I - K * C;
+  P = IKC * P * IKC.transpose() + K * R * K.transpose();
   x_hat = x_hat_new;
 
   t += dt;

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h

@@ -15,9 +15,9 @@ namespace mkt_algorithm
     class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory
     {
     public:
-      GoStraight() {};
+      GoStraight();
 
-      virtual ~GoStraight() {};
+      virtual ~GoStraight();
 
       /**
        * @brief Initialize parameters as needed

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -30,8 +30,14 @@ namespace mkt_algorithm
     class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
     {
     public:
-      PredictiveTrajectory() : initialized_(false), nav_stop_(false) {};
+      /**
+       * @brief Constructor
+       */
+      PredictiveTrajectory();
 
+      /**
+       * @brief Destructor
+       */
       virtual ~PredictiveTrajectory();
 
       // Standard ScoreAlgorithm Interface
@@ -256,6 +262,19 @@ namespace mkt_algorithm
           const double &dt,
           robot_nav_2d_msgs::Twist2D &cmd_vel);
       
+      /**
+       * @brief PID controller
+       * @param error
+       * @param integral
+       * @param last_error
+       * @param dt
+       * @param kp
+       * @param ki
+       * @param kd
+       * @param output
+       */
+      void pid(const double &error, double &integral, double &last_error, const double &dt, const double &kp, const double &ki, const double &kd, double &output);
+
       /**
        * @brief  the robot is moving acceleration limits
        * @param  velocity The velocity of the robot
@@ -368,13 +387,14 @@ namespace mkt_algorithm
       double final_heading_angle_tolerance_;   // Angle threshold to consider heading reached
       double final_heading_min_velocity_;      // Minimum linear velocity during alignment
       double final_heading_kp_angular_;        // Proportional gain for angular control
+      double final_heading_ki_angular_;
+      double final_heading_kd_angular_;
 
-      // Regulated linear velocity scaling
+      double near_goal_heading_integral_;
-      bool use_regulated_linear_velocity_scaling_;
+      double near_goal_heading_last_error_;
+      bool near_goal_heading_was_active_;
       
       double min_approach_linear_velocity_;
-      double regulated_linear_scaling_min_radius_;
-      double regulated_linear_scaling_min_speed_;
 
       bool use_cost_regulated_linear_velocity_scaling_;
       double inflation_cost_scaling_factor_;
@@ -392,6 +412,14 @@ namespace mkt_algorithm
       Eigen::MatrixXd Q;
       Eigen::MatrixXd R;
       Eigen::MatrixXd P;
+
+      // Kalman filter tuning (for v and w filtering)
+      double kf_q_v_;
+      double kf_q_w_;
+      double kf_r_v_;
+      double kf_r_w_;
+      double kf_p0_;
+      bool kf_filter_angular_;
 #ifdef BUILD_WITH_ROS
       ros::Publisher lookahead_point_pub_;
 #endif

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h

@@ -14,9 +14,9 @@ namespace mkt_algorithm
     class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory
     {
     public:
-      RotateToGoal() {};
+      RotateToGoal();
 
-      virtual ~RotateToGoal() {};
+      virtual ~RotateToGoal();
 
       /**
        * @brief Initialize parameters as needed

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp

@@ -2,6 +2,13 @@
 #include <boost/dll/alias.hpp>
 #include <robot/robot.h>
 
+mkt_algorithm::diff::GoStraight::GoStraight()
+  : PredictiveTrajectory()
+{
+}
+
+mkt_algorithm::diff::GoStraight::~GoStraight() {}
+
 void mkt_algorithm::diff::GoStraight::initialize(
     robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
@@ -9,8 +16,6 @@ void mkt_algorithm::diff::GoStraight::initialize(
   {
     nh_ = nh;
     nh_priv_ = robot::NodeHandle(nh, name);
-    robot::log_info_at(__FILE__, __LINE__, "nh_ : %s", nh_.getNamespace().c_str());
-    robot::log_info_at(__FILE__, __LINE__, "nh_priv_ : %s", nh_priv_.getNamespace().c_str());
     name_ = name;
     tf_ = tf;
     traj_ = traj;
@@ -87,6 +92,7 @@ void mkt_algorithm::diff::GoStraight::initialize(
 mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
+  // robot::log_info("x %f y %f theta %f", pose.pose.x, pose.pose.y, pose.pose.theta);
   mkt_msgs::Trajectory2D result;
   result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
   if (!traj_)
@@ -145,7 +151,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
       lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
     }
     robot_nav_2d_msgs::Twist2D drive_target;
-    robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", transformed_plan.poses.front().pose.y, transformed_plan.poses.front().pose.theta);
     transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
     carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
 

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -1,6 +1,51 @@
 #include <mkt_algorithm/diff/diff_predictive_trajectory.h>
 #include <boost/dll/alias.hpp>
 
+
+mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
+  : initialized_(false),
+    nav_stop_(false),
+    x_direction_(0.0),
+    y_direction_(0.0),
+    theta_direction_(0.0),
+    use_velocity_scaled_lookahead_dist_(false),
+    lookahead_time_(0.0),
+    lookahead_dist_(0.0),
+    min_lookahead_dist_(0.0),
+    max_lookahead_dist_(0.0),
+    max_lateral_accel_(0.0),
+    use_rotate_to_heading_(false),
+    rotate_to_heading_min_angle_(0.0),
+    min_path_distance_(0.0),
+    max_path_distance_(0.0),
+    use_final_heading_alignment_(false),
+    final_heading_xy_tolerance_(0.0),
+    final_heading_angle_tolerance_(0.0),
+    final_heading_min_velocity_(0.0),
+    final_heading_kp_angular_(0.0),
+    final_heading_ki_angular_(0.0),
+    final_heading_kd_angular_(0.0),
+    near_goal_heading_integral_(0.0),
+    near_goal_heading_last_error_(0.0),
+    near_goal_heading_was_active_(false),
+    min_approach_linear_velocity_(0.0),
+    use_cost_regulated_linear_velocity_scaling_(false),
+    inflation_cost_scaling_factor_(0.0),
+    cost_scaling_dist_(0.0),
+    cost_scaling_gain_(0.0),
+    control_duration_(0.0),
+    kf_(nullptr),
+    kf_q_v_(0.25),
+    kf_q_w_(0.8),
+    kf_r_v_(0.05),
+    kf_r_w_(0.08),
+    kf_p0_(0.5),
+    kf_filter_angular_(false),
+    m_(0),
+    n_(0)
+{
+}
+
 mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}
 
 void mkt_algorithm::diff::PredictiveTrajectory::initialize(
@@ -44,16 +89,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     // kalman
     last_actuator_update_ = robot::Time::now();
-    n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
+    // State: [v, a, j, w, alpha, jw] where:
-    m_ = 2; // measurements: x, y, theta
+    //  - v: linear velocity, a: linear accel, j: linear jerk
+    //  - w: angular velocity, alpha: angular accel, jw: angular jerk
+    // Measurement: [v, w]
+    n_ = 6;
+    m_ = 2;
     double dt = control_duration_;
 
     // Khởi tạo ma trận
     A = Eigen::MatrixXd::Identity(n_, n_);
     C = Eigen::MatrixXd::Zero(m_, n_);
     Q = Eigen::MatrixXd::Zero(n_, n_);
-    R = Eigen::MatrixXd::Identity(m_, m_);
+    R = Eigen::MatrixXd::Zero(m_, m_);
-    P = Eigen::MatrixXd::Identity(n_, n_);
+    P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_);
 
     for (int i = 0; i < n_; i += 3)
     {
@@ -64,15 +113,11 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
 
     C(0, 0) = 1;
     C(1, 3) = 1;
-    Q(2, 2) = 0.1;
+    Q(2, 2) = std::max(1e-12, kf_q_v_);
-    Q(5, 5) = 0.6;
+    Q(5, 5) = std::max(1e-12, kf_q_w_);
 
-    R(0, 0) = 0.1;
+    R(0, 0) = std::max(1e-12, kf_r_v_);
-    R(1, 1) = 0.2;
+    R(1, 1) = std::max(1e-12, kf_r_w_);
-
-    P(3, 3) = 0.4;
-    P(4, 4) = 0.4;
-    P(5, 5) = 0.4;
 
     kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
     Eigen::VectorXd x0(n_);
@@ -117,6 +162,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   nh_priv_.param<double>("final_heading_angle_tolerance", final_heading_angle_tolerance_, 0.05);
   nh_priv_.param<double>("final_heading_min_velocity", final_heading_min_velocity_, 0.05);
   nh_priv_.param<double>("final_heading_kp_angular", final_heading_kp_angular_, 1.5);
+  nh_priv_.param<double>("final_heading_ki_angular", final_heading_ki_angular_, 0.0);
+  nh_priv_.param<double>("final_heading_kd_angular", final_heading_kd_angular_, 0.0);
 
   nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.01);
   nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.01);
@@ -125,11 +172,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   if (trans_stopped_velocity_ > min_approach_linear_velocity_)
     trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01;
 
-  // Regulated linear velocity scaling
-  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
-  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
-  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
-
   // Inflation cost scaling (Limit velocity by proximity to obstacles)
   nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
   nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
@@ -141,6 +183,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
+
+  // Kalman filter tuning (filtering v and w commands)
+  nh_priv_.param<double>("kf_q_v", kf_q_v_, kf_q_v_);
+  nh_priv_.param<double>("kf_q_w", kf_q_w_, kf_q_w_);
+  nh_priv_.param<double>("kf_r_v", kf_r_v_, kf_r_v_);
+  nh_priv_.param<double>("kf_r_w", kf_r_w_, kf_r_w_);
+  nh_priv_.param<double>("kf_p0", kf_p0_, kf_p0_);
+  nh_priv_.param<bool>("kf_filter_angular", kf_filter_angular_, kf_filter_angular_);
+
   double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
   control_duration_ = 1.0 / control_frequency;
 
@@ -161,6 +212,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
     traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
     traj_.get()->getNodeHandle().param<double>("min_speed_xy", min_speed_xy_, 0.0);
+    if(fabs(min_speed_xy_) > sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+    if(fabs(min_speed_xy_) > sqrt(min_vel_x_ * min_vel_x_ + min_vel_y_ * min_vel_y_))
+    {
+      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
+    }
+
     traj_.get()->getNodeHandle().param<double>("max_speed_xy", max_speed_xy_, 0.0);
   }
 }
@@ -178,6 +238,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::reset()
     this->nav_stop_ = false;
     last_actuator_update_ = robot::Time::now();
     prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D();
+    near_goal_heading_integral_ = 0.0;
+    near_goal_heading_last_error_ = 0.0;
+    near_goal_heading_was_active_ = false;
 
     if (kf_)
     {
@@ -387,7 +450,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
 }
 
 mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
-    const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
+  const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
   mkt_msgs::Trajectory2D result;
   result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
@@ -472,16 +535,16 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       // Use Arc Motion controller for final heading alignment
       alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
 #ifdef BUILD_WITH_ROS
-      ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+      ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
-                        xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+                        heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
 #endif
     }
     else
     {
-      if(fabs(carrot_pose.pose.y) > 0.2)
+      // if(fabs(carrot_pose.pose.y) > 0.2)
-      {
+      // {
-        lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
+      //   lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
-      }
+      // }
       robot_nav_2d_msgs::Twist2D drive_target;
       transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
       carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
@@ -498,7 +561,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       drive_cmd);
     }
     applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
-
     if (this->nav_stop_)
     {
       if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
@@ -511,27 +573,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       result.velocity = drive_cmd;
       return result;
     }
-
-    Eigen::VectorXd y(2);
-    y << drive_cmd.x, drive_cmd.theta;
-
-    // Cập nhật lại A nếu dt thay đổi
-    for (int i = 0; i < n_; ++i)
-      for (int j = 0; j < n_; ++j)
-        A(i, j) = (i == j ? 1.0 : 0.0);
-    for (int i = 0; i < n_; i += 3)
-    {
-      A(i, i + 1) = dt;
-      A(i, i + 2) = 0.5 * dt * dt;
-      A(i + 1, i + 2) = dt;
-    }
-    kf_->update(y, dt, A);
-    double v_min = min_approach_linear_velocity_;
-    drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
-    drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
   }
-
   result.poses.clear();
   result.poses.reserve(transformed_plan.poses.size());
   for (const auto &pose_stamped : transformed_plan.poses)
@@ -560,7 +602,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 {
   // 1) Curvature from pure pursuit
   const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
-  const double L2_min = 0.05;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
+  const double L2_min = 0.01;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
   const double L2_safe = std::max(L2, L2_min);
   const double L = std::sqrt(L2_safe);
 
@@ -571,7 +613,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   const double L_min = 0.1;  // m, chỉnh theo nhu cầu
   double scale_close = std::clamp(L / L_min, 0.0, 1.0);
   v_target *= scale_close;
-
   const double y_abs = std::fabs(carrot_pose.pose.y);
   const double y_soft = 0.1;
   if (y_abs > y_soft) 
@@ -590,10 +631,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
     v_target = std::copysign(min_approach_linear_velocity, sign_x);
 
   // 5) Angular speed from curvature
-  double w_target = v_target * kappa;
+  double w_target = v_target * kappa + std::copysign(carrot_pose.pose.theta * dt, kappa);
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
   {
-    const double k_heading = 0.3;
     if (trajectory.poses.size() >= 2) {
       const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
       double heading_ref = 0.0;
@@ -611,16 +651,30 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
         }
       }
 
-      // robot heading in base_link = 0, so error = heading_ref
+      const double error = angles::normalize_angle(heading_ref);
-      double w_heading = k_heading * angles::normalize_angle(heading_ref);
+      double w_heading = 0.0;
-    
+      pid(error, 
-      w_target += w_heading;
+        near_goal_heading_integral_, 
+        near_goal_heading_last_error_, 
+        dt, 
+        final_heading_kp_angular_, 
+        final_heading_ki_angular_, 
+        final_heading_kd_angular_, 
+        w_heading);
+      // Apply acceleration limits
+      double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
+      w_target = velocity.theta + dw_heading;
     }
     else
     {
       w_target = 0.0;
+      near_goal_heading_was_active_ = false;
     }
   }
+  else
+  {
+    near_goal_heading_was_active_ = false;
+  }
   w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
 
   // 6) Apply acceleration limits (linear + angular)
@@ -629,7 +683,27 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
-  // robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta);
+
+
+  Eigen::VectorXd y(2);
+  y << drive_cmd.x, drive_cmd.theta;
+
+  // Cập nhật lại A nếu dt thay đổi
+  for (int i = 0; i < n_; ++i)
+    for (int j = 0; j < n_; ++j)
+      A(i, j) = (i == j ? 1.0 : 0.0);
+  for (int i = 0; i < n_; i += 3)
+  {
+    A(i, i + 1) = dt;
+    A(i, i + 2) = 0.5 * dt * dt;
+    A(i + 1, i + 2) = dt;
+  }
+  kf_->update(y, dt, A);
+  double v_min = min_approach_linear_velocity_;
+  drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
+  drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
+  if (kf_filter_angular_)
+    drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -715,14 +789,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   //   (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
 
   bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
-
+// #ifdef BUILD_WITH_ROS
   // if (result)
   //   ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+
   // else if(fabs(velocity.x) < min_speed_xy_)
   // {
   //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
   //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
   // }
+// #endif
   return result;
 }
 
@@ -879,7 +955,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
   if (abs_heading_error > final_heading_angle_tolerance_)
   {
     // Method 1: Proportional control
-    double omega_p = final_heading_kp_angular_ * heading_error;
+    double omega_p = 0.0;
+    pid(heading_error, 
+      near_goal_heading_integral_, 
+      near_goal_heading_last_error_, 
+      dt, 
+      final_heading_kp_angular_, 
+      final_heading_ki_angular_, 
+      final_heading_kd_angular_, 
+      omega_p);
 
     // Method 2: Arc-based feedforward (coordinate linear and angular motion)
     double omega_arc = 0.0;
@@ -911,7 +995,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
     }
   }
   
-
   // --- Apply acceleration limits ---
   // Linear
   double v_current = velocity.x;
@@ -944,6 +1027,26 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
 #endif
 }
 
+void mkt_algorithm::diff::PredictiveTrajectory::pid(
+  const double &error, double &integral, double &last_error, const double &dt, const double &kp, const double &ki, const double &kd, double &output)
+{
+  if (!near_goal_heading_was_active_)
+  {
+    integral = 0.0;
+    last_error = error;
+    near_goal_heading_was_active_ = true;
+  }
+  const double dt_safe = std::max(dt, 1e-6);
+  double w_p = kp * error;
+  integral += error * dt_safe;
+  const double integral_cap = (std::fabs(ki) > 1e-9)? (2.0 / ki) : 1e6;
+  integral = std::clamp(integral, -integral_cap, integral_cap);
+  double w_i = ki * integral;
+  double w_d = kd * (error - last_error) / dt_safe;
+  last_error = error;
+  output = w_p + w_i + w_d;
+}
+
 double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity)
 {
   // If using velocity-scaled look ahead distances, find and clamp the dist
@@ -957,71 +1060,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob
   return lookahead_dist;
 }
 
-// std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
-// mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
-// {
-//   if (global_plan.poses.empty())
-//   {
-//     throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
-//   }
-
-//   if ((unsigned)global_plan.poses.size() < 2)
-//   {
-//     auto goal_pose_it = std::prev(global_plan.poses.end());
-//     return goal_pose_it;
-//   }
-
-//   unsigned int goal_index = (unsigned)global_plan.poses.size() - 1;
-  
-//   double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
-//   double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
-//   double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
-//   double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;
-
-//   // make sure that atan2 is defined
-//   double start_angle = atan2(start_direction_y, start_direction_x);
-//   double end_angle = atan2(end_direction_y, end_direction_x);
-//   double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
-
-//   for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
-//   {
-//     if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
-//     {
-//       const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
-//       const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
-
-//       if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
-//       {
-//         double current_angle = atan2(current_direction_y, current_direction_x);
-//         goal_index = i;
-//         if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
-//           continue;
-
-//         if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
-//         {
-//           goal_index = i;
-//           break;
-//         }
-//       }
-//     }
-//   }
-
-//   // Find the first pose which is at a distance greater than the lookahead distance
-//   auto goal_pose_it = std::find_if(
-//   global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
-//   [&](const auto &ps)
-//   {
-//     return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist;
-//   });
-
-//   // If the number of poses is not far enough, take the last pose
-//   if (goal_pose_it == global_plan.poses.end())
-//   {
-//     goal_pose_it = std::prev(global_plan.poses.end());
-//   }
-//   return goal_pose_it;
-// }
-
 std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
 mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
 {
@@ -1144,24 +1182,36 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto
   double v_target,
   const double &sign_x)
 {
-  if (journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
+  if(trajectory.poses.size() < 2)
-    return min_speed_xy_ * sign_x;
+    return min_approach_linear_velocity_ * sign_x;
 
   double preview_distance = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_);
 
   double max_kappa = calculateMaxKappa(trajectory, preview_distance);
   double v_limit = std::fabs(v_target);
+  double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
+  
+  if (journey_distance < min_journey_squared_)
+  {
+    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x;
+  }
+
   if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
   {
     const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa);
     v_limit = std::min(v_limit, v_curve);
   }
+
+  if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_)
+    v_limit = min_speed_xy_ * sign_x;
+
   if (fabs(decel_lim_x_) > 1e-6)
   {
     const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
     const double v_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, remaining));
     v_limit = std::min(v_limit, v_stop);
   }
+  v_limit = std::min(v_limit, fabs(v_target));
 
   return std::copysign(v_limit, sign_x);
 }
@@ -1179,22 +1229,18 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
     drive_cmd.theta = 0.0;
     return robot_nav_2d_msgs::Path2D();
   }
-  robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", path.poses.front().pose.y, path.poses.front().pose.theta);
   double max_kappa = calculateMaxKappa(path);
   const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
   drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
   drive_cmd.theta = max_vel_theta_;
 
-  if (max_kappa <= straight_threshold) // nếu đường thẳng
+  if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
   { 
-    if(journey(path.poses, 0, path.poses.size() - 1) <= min_journey_squared_)
+    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_))
     {
-      drive_cmd.theta = 0.05;
-    }
-    if(fabs(path.poses.front().pose.y) > 0.02)
-      return generateHermiteTrajectory(path.poses.back(), sign_x);
-    else
       return generateParallelPath(path, sign_x);
+    }
+    return generateHermiteTrajectory(path.poses.back(), sign_x);
   }
   else // nếu đường cong
   {
@@ -1204,7 +1250,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   }
 }
 
-robot_nav_2d_msgs::Path2D generateParallelPath(
+robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateParallelPath(
   const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {
   robot_nav_2d_msgs::Path2D parallel_path;
@@ -1232,8 +1278,8 @@ robot_nav_2d_msgs::Path2D generateParallelPath(
     }
 
     double theta = atan2(dy, dx);
-    double x_off = p.x - offset_y * sin(theta);
+    double x_off = p.x - offset_y * sin(theta)*sign_x;
-    double y_off = p.y + offset_y * cos(theta);
+    double y_off = p.y - offset_y * cos(theta)*sign_x;
 
     parallel_path.poses[i].pose.x = x_off;
     parallel_path.poses[i].pose.y = y_off;

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp

@@ -4,6 +4,14 @@
 #include <boost/dll/alias.hpp>
 #include <angles/angles.h>
 
+
+mkt_algorithm::diff::RotateToGoal::RotateToGoal()
+  : PredictiveTrajectory()
+{
+}
+
+mkt_algorithm::diff::RotateToGoal::~RotateToGoal() {}
+
 void mkt_algorithm::diff::RotateToGoal::initialize(
     robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h

@@ -15,6 +15,7 @@ namespace mkt_plugins
   {
   public:
     KinematicParameters();
+    virtual ~KinematicParameters();
     void initialize(const robot::NodeHandle &nh);
 
     /**

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h

@@ -12,6 +12,15 @@ namespace mkt_plugins
 class LimitedAccelGenerator : public StandardTrajectoryGenerator
 {
 public:
+  /**
+   * @brief Constructor
+   */
+  LimitedAccelGenerator();
+  /**
+   * @brief Destructor
+   */
+  virtual ~LimitedAccelGenerator();
+  
   /**
    * @brief Initialize the generator with parameters from NodeHandle
    * @param nh NodeHandle for loading acceleration_time parameter

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h

@@ -17,6 +17,16 @@ namespace mkt_plugins
   class StandardTrajectoryGenerator : public score_algorithm::TrajectoryGenerator
   {
   public:
+
+    /**
+     * @brief Constructor
+     */
+    StandardTrajectoryGenerator();
+
+    /**
+     * @brief Destructor
+     */
+    virtual ~StandardTrajectoryGenerator();
     /**
      * @brief Initialize the trajectory generator with parameters from NodeHandle
      * @param nh NodeHandle for loading configuration parameters

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h

@@ -21,7 +21,12 @@ namespace mkt_plugins
      * @brief Default constructor
      * Initializes all iterators to nullptr
      */
-    XYThetaIterator() : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
+    XYThetaIterator();
+
+    /**
+     * @brief Destructor
+     */
+    virtual ~XYThetaIterator();
     
     /**
      * @brief Initialize the iterator with parameters and kinematics

src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp

@@ -7,7 +7,12 @@
 #include <angles/angles.h>
 #include <cmath>
 
-mkt_plugins::GoalChecker::GoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25)
+mkt_plugins::GoalChecker::GoalChecker()
+  : nh_(),
+    line_generator_(nullptr),
+    yaw_goal_tolerance_(0.25),
+    xy_goal_tolerance_(0.25),
+    old_xy_goal_tolerance_(0.0)
 {
 }
 
@@ -29,8 +34,6 @@ void mkt_plugins::GoalChecker::intParam(const robot::NodeHandle &nh)
   {
     yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
   }
-  // ROS_INFO_THROTTLE(0.5, "%s", nh_.getNamespace().c_str());
-  // ROS_INFO_THROTTLE(0.5, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_);
 }
 
 void mkt_plugins::GoalChecker::reset()
@@ -49,19 +52,27 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
   if (!path.poses.empty() && path.poses.size() > 2)
   {
     double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta);
+    for(int i = path.poses.size() - 1; i >= 0; i--)
+    {
+      double dx = path.poses[i].pose.x - path.poses.back().pose .x;
+      double dy = path.poses[i].pose.y - path.poses.back().pose.y;
+      if(std::sqrt(dx * dx + dy * dy) > 0.05 // TODO: get resolution from costmap
+         && fabs(dx) > 1e-6 && fabs(dy) > 1e-6)
+      {
+        theta =  angles::normalize_angle(query_pose.pose.theta - atan2(dy, dx));
+        break;
+      }
+    }
     double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy;
     double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy;
-    if (xy_tolerance <= xy_goal_tolerance_ + 0.3)
+    if (xy_tolerance <= xy_goal_tolerance_ + 0.1)
     {
       double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
-      // ROS_INFO_THROTTLE(0.1, "Goal checker 1 %f %f %f %f %f %f", tolerance, old_xy_goal_tolerance_, goal_pose.pose.theta, x, y, theta);
+      if(fabs(tolerance) <= xy_goal_tolerance_) 
-      if(
-          (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) 
-          // && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4)
-        )
       {
-        robot::log_info_throttle(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
+        robot::log_info_at(__FILE__, __LINE__, "%x %x", fabs(tolerance) <= xy_goal_tolerance_, tolerance * old_xy_goal_tolerance_ < 0);
-        robot::log_info_throttle(0.1, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+        robot::log_info_at(__FILE__, __LINE__, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
+        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
         return true;
       }
     }
@@ -73,16 +84,15 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
     double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta);
     double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy;
     double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy;
-    if (xy_tolerance <= xy_goal_tolerance_ + 0.3)
+    if (xy_tolerance <= xy_goal_tolerance_ + 0.1)
     {
       double tolerance = fabs(x) > fabs(y) ? x : y;
       if(
           (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) 
-          // && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4)
         )
       {
-        robot::log_info_throttle(0.1, "%f %f", fabs(cos(theta)), fabs(sin(theta)));
+        robot::log_info_at(__FILE__, __LINE__, "%f %f", fabs(cos(theta)), fabs(sin(theta)));
-        robot::log_info_throttle(0.1, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+        robot::log_info_at(__FILE__, __LINE__, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
         return true;
       }
     }
@@ -92,7 +102,7 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
 
   if (xy_tolerance <= xy_goal_tolerance_)
   {
-    robot::log_info_at(__FILE__, __LINE__, "Goal checker 0 %f %f", xy_tolerance, xy_goal_tolerance_);
+    robot::log_info_at(__FILE__, __LINE__, "Goal checker ok %f %f", xy_tolerance, xy_goal_tolerance_);
     return true;
   }
   return false;

src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp

@@ -32,12 +32,42 @@ namespace mkt_plugins
     nh.setParam(decel_param, -accel);
   }
 
-  KinematicParameters::KinematicParameters() : xytheta_direct_(),
+  KinematicParameters::KinematicParameters()
-                                               min_vel_x_(0.0), min_vel_y_(0.0), max_vel_x_(0.0), max_vel_y_(0.0), max_vel_theta_(0.0),
+    : xytheta_direct_(),
-                                               min_speed_xy_(0.0), max_speed_xy_(0.0), min_speed_theta_(0.0),
+      min_vel_x_(0.0),
-                                               acc_lim_x_(0.0), acc_lim_y_(0.0), acc_lim_theta_(0.0),
+      min_vel_y_(0.0),
-                                               decel_lim_x_(0.0), decel_lim_y_(0.0), decel_lim_theta_(0.0),
+      max_vel_x_(0.0),
-                                               min_speed_xy_sq_(0.0), max_speed_xy_sq_(0.0)
+      max_vel_y_(0.0),
+      max_vel_theta_(0.0),
+      min_speed_xy_(0.0),
+      max_speed_xy_(0.0),
+      min_speed_theta_(0.0),
+      acc_lim_x_(0.0),
+      acc_lim_y_(0.0),
+      acc_lim_theta_(0.0),
+      decel_lim_x_(0.0),
+      decel_lim_y_(0.0),
+      decel_lim_theta_(0.0),
+      min_speed_xy_sq_(0.0),
+      max_speed_xy_sq_(0.0),
+      original_min_vel_x_(0.0),
+      original_min_vel_y_(0.0),
+      original_max_vel_x_(0.0),
+      original_max_vel_y_(0.0),
+      original_max_vel_theta_(0.0),
+      original_min_speed_xy_(0.0),
+      original_max_speed_xy_(0.0),
+      original_min_speed_theta_(0.0),
+      original_acc_lim_x_(0.0),
+      original_acc_lim_y_(0.0),
+      original_acc_lim_theta_(0.0),
+      original_decel_lim_x_(0.0),
+      original_decel_lim_y_(0.0),
+      original_decel_lim_theta_(0.0)
+  {
+  }
+
+  KinematicParameters::~KinematicParameters()
   {
   }
 

src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp

@@ -8,6 +8,16 @@
 namespace mkt_plugins
 {
 
+LimitedAccelGenerator::LimitedAccelGenerator()
+  : StandardTrajectoryGenerator(),
+    acceleration_time_(0.0)
+{
+}
+
+LimitedAccelGenerator::~LimitedAccelGenerator()
+{
+}
+
 void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
 {
   StandardTrajectoryGenerator::initialize(nh);

src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp

@@ -7,8 +7,13 @@
 namespace mkt_plugins
 {
 
-SimpleGoalChecker::SimpleGoalChecker() :
+SimpleGoalChecker::SimpleGoalChecker()
-  xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625)
+  : nh_(),
+    xy_goal_tolerance_(0.25),
+    yaw_goal_tolerance_(0.25),
+    stateful_(true),
+    check_xy_(true),
+    xy_goal_tolerance_sq_(0.0625)
 {
 }
 

src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp

@@ -13,6 +13,24 @@ using robot_nav_2d_utils::loadParameterWithDeprecation;
 
 namespace mkt_plugins
 {
+
+  StandardTrajectoryGenerator::StandardTrajectoryGenerator()
+    : nh_kinematics_(),
+      kinematics_(nullptr),
+      velocity_iterator_(nullptr),
+      sim_time_(0.0),
+      discretize_by_time_(false),
+      time_granularity_(0.0),
+      linear_granularity_(0.0),
+      angular_granularity_(0.0),
+      include_last_point_(false)
+  {
+  }
+
+  StandardTrajectoryGenerator::~StandardTrajectoryGenerator()
+  {
+  }
+
   void StandardTrajectoryGenerator::initialize(const robot::NodeHandle &nh)
   {
     nh_kinematics_ = nh;

src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp

@@ -4,6 +4,21 @@
 
 namespace mkt_plugins
 {
+XYThetaIterator::XYThetaIterator()
+  : vx_samples_(0),
+    vy_samples_(0),
+    vtheta_samples_(0),
+    kinematics_(nullptr),
+    x_it_(nullptr),
+    y_it_(nullptr),
+    th_it_(nullptr)
+{
+}
+
+XYThetaIterator::~XYThetaIterator()
+{
+}
+
 void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) 
 {
   kinematics_ = kinematics;

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -13,7 +13,17 @@
 
 namespace two_points_planner 
 {
-  TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {}
+  TwoPointsPlanner::TwoPointsPlanner()
+    : initialized_(false),
+      lethal_obstacle_(0),
+      inscribed_inflated_obstacle_(0),
+      circumscribed_cost_(0),
+      allow_unknown_(false),
+      costmap_robot_(nullptr),
+      current_env_width_(0),
+      current_env_height_(0)
+  {
+  }
 
   TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
     : initialized_(false), costmap_robot_(NULL)
@@ -26,8 +36,6 @@ namespace two_points_planner
     if (!initialized_)
     {
       robot::NodeHandle nh_priv_("~/" + name);
-      robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());
-
       int lethal_obstacle;
       nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
       lethal_obstacle_ = (unsigned char)lethal_obstacle;
@@ -111,6 +119,7 @@ namespace two_points_planner
       robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
       return false;
     }
+    
     robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start);
     robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);
     robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", 
@@ -138,21 +147,21 @@ namespace two_points_planner
     }
 
     plan.clear();
-    plan.push_back(start);
+    // plan.push_back(start);
     
-    unsigned int mx_start, my_start;
+    // unsigned int mx_start, my_start;
-    unsigned int mx_end, my_end;
+    // unsigned int mx_end, my_end;
-    if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
+    // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
-      start.pose.position.y,
+    //   start.pose.position.y,
-      mx_start, my_start)
+    //   mx_start, my_start)
 
-      || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
+    //   || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
-        goal.pose.position.y,
+    //     goal.pose.position.y,
-        mx_end, my_end))
+    //     mx_end, my_end))
-    {
+    // {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
+    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
-      return false;
+    //   return false;
-    }
+    // }
     // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
     // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
     // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)

src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt

@@ -101,6 +101,7 @@ add_library(${PROJECT_NAME} SHARED
   src/pnkx_local_planner.cpp
   src/pnkx_go_straight_local_planner.cpp
   src/pnkx_rotate_local_planner.cpp
+  src/pnkx_docking_local_planner.cpp
 )
 
 if(BUILDING_WITH_CATKIN)

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h

@@ -1,8 +1,9 @@
 #ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 #define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 
-#include <robot_nav_core2/base_global_planner.h>
+#include <robot_nav_core/base_global_planner.h>
 #include <pnkx_local_planner/pnkx_local_planner.h>
+#include <robot_xmlrpcpp/XmlRpcValue.h>
 
 namespace pnkx_local_planner
 {
@@ -137,7 +138,7 @@ namespace pnkx_local_planner
       std::string docking_planner_name_;
       std::string docking_nav_name_;
 
-      robot_nav_core2::BaseGlobalPlanner::Ptr docking_planner_;
+      robot_nav_core::BaseGlobalPlanner::Ptr docking_planner_;
       score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
 
       robot_geometry_msgs::Vector3 linear_;
@@ -158,7 +159,7 @@ namespace pnkx_local_planner
       TFListenerPtr tf_;
       robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
       score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
-      std::function<robot_nav_core2::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
+      std::function<robot_nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
       std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
 
       robot::WallTimer detected_timeout_wt_, delayed_wt_;
@@ -169,7 +170,7 @@ namespace pnkx_local_planner
 
     bool start_docking_;
     double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
-    XmlRpc::XmlRpcValue original_papams_;
+    robot_xmlrpcpp::XmlRpcValue original_papams_;
     std::vector<DockingPlanner*> dkpl_;
 
     bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity);

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h

@@ -79,7 +79,6 @@ namespace pnkx_local_planner
      */
     robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;
 
-    bool is_ready_;
   };
 
 } // namespace pnkx_local_planner

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp

@@ -14,7 +14,10 @@
 #include <boost/dll/alias.hpp>
 
 pnkx_local_planner::PNKXDockingLocalPlanner::PNKXDockingLocalPlanner()
-    : start_docking_(false)
+  : PNKXLocalPlanner(),
+    start_docking_(false),
+    original_xy_goal_tolerance_(0.0),
+    original_yaw_goal_tolerance_(0.0)
 {
 }
 
@@ -53,102 +56,106 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     if (!costmap_robot_->isCurrent())
       throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
 
-    nh_ = robot::NodeHandle("~");
     parent_ = parent;
     planner_nh_ = robot::NodeHandle(parent_, name);
-    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s", planner_nh_.getNamespace().c_str());
+    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str());
-
     this->getParams(planner_nh_);
 
     std::string traj_generator_name;
-    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
+    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("StandardTrajectoryGenerator"));
     robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(traj_generator_name);
       traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
         path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
       traj_generator_ = traj_gen_loader_();
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
-      robot::NodeHandle nh_traj_gen = robot::NodeHandle(nh_, traj_generator_name);
+      robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str());
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_nav_name; 
-    planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
+    planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
     robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
-      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
-      nav_algorithm_ = algorithm_loader_();
+      nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
-      nav_algorithm_->initialize(nh_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
+      nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
-    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff"));
+    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
-      algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
+      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
-      rotate_algorithm_ = algorithm_loader_();
+      rotate_algorithm_ = rotate_algorithm_loader_();
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
-      rotate_algorithm_->initialize(nh_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
+      rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
+    robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
-      boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
+      std::string path_file_so = loader.findLibraryPath(goal_checker_name);
+      goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
         path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
       goal_checker_ = goal_checker_loader_();
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
-      goal_checker_->initialize(nh_);
+      goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
+
     this->initializeOthers();
     this->getMaker();
     robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
@@ -159,8 +166,10 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
 void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
 {
   std::string maker_name, maker_sources;
-  nh_.param("maker_name", maker_name, std::string(""));
+  parent_.param("maker_name", maker_name, std::string(""));
-  nh_.param("maker_sources", maker_sources, std::string(""));
+  parent_.param("maker_sources", maker_sources, std::string(""));
+  robot::log_info_at(__FILE__, __LINE__, "maker_name: %s", maker_name.c_str());
+  robot::log_info_at(__FILE__, __LINE__, "maker_sources: %s", maker_sources.c_str());
   std::stringstream ss(maker_sources);
   std::string source;
 
@@ -168,24 +177,23 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
   {
     if (maker_name == source)
     {
-      robot::NodeHandle source_node(nh_, source);
+      robot::NodeHandle source_node(parent_, source);
 
       if (source_node.hasParam("plugins"))
       {
-        XmlRpc::XmlRpcValue plugins;
+        robot_xmlrpcpp::XmlRpcValue plugins;
         source_node.getParam("plugins", plugins);
 
-        if (plugins.getType() == XmlRpc::XmlRpcValue::TypeArray)
+        if (plugins.getType() == robot_xmlrpcpp::XmlRpcValue::TypeArray)
         {
           for (int i = 0; i < plugins.size(); ++i)
           {
-            if (plugins[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
+            if (plugins[i].getType() == robot_xmlrpcpp::XmlRpcValue::TypeStruct)
             {
               std::stringstream name;
               name << source << "/" << static_cast<std::string>(plugins[i]["name"]);
               std::string docking_planner_name = static_cast<std::string>(plugins[i]["docking_planner"]);
               std::string docking_nav_name = static_cast<std::string>(plugins[i]["docking_nav"]);
-
               // std::shared_ptr<DockingPlanner> dkpl = std::make_shared<DockingPlanner>(name.str());
               DockingPlanner* dkpl = new DockingPlanner(name.str());
               dkpl->docking_planner_name_ = docking_planner_name;
@@ -212,7 +220,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initializeOthers()
 {
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-  if (!nh_.getParam(algorithm_nav_name, original_papams_))
+  if (!parent_.getParam(algorithm_nav_name, original_papams_))
     robot::log_warning_at(__FILE__, __LINE__, "No found in %s in yaml-file, please check configuration", algorithm_nav_name.c_str());
   original_xy_goal_tolerance_ = xy_goal_tolerance_;
   original_yaw_goal_tolerance_ = yaw_goal_tolerance_;
@@ -242,11 +250,16 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
   if(rotate_algorithm_) rotate_algorithm_->reset();
   ret_nav_ = ret_angle_ = false;
 
+  robot::log_info_at(__FILE__, __LINE__, "Debug");
+  parent_.printParams();
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-  robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
+  // parent_.setParam(algorithm_nav_name, original_papams_);
-  nh_.setParam(algorithm_nav_name, original_papams_);
+  robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
   nh_algorithm.setParam("allow_rotate", false);
+
+  robot::log_info_at(__FILE__, __LINE__, "Debug ở đây");
+  parent_.printParams();
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -276,7 +289,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
   try
   {
-    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
   }
   catch(const robot_nav_core2::LocalPlannerException& e)
@@ -287,7 +300,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   double x_direction, y_direction, theta_direction;
   if (!ret_nav_)
   {
-    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -306,15 +319,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
       std::string algorithm_nav_name;
       planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-      robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
+      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
       nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_);
       nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_);
       nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_);
       nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
       nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
 
-      nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
+      planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
-      nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
+      planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
 
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
       {
@@ -327,7 +340,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
             local_goal_pose = follow_pose;
           }
         }
-        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+        if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
         {
           throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
           robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
@@ -337,7 +350,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   }
   else
   {
-    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
@@ -385,7 +398,6 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
   else if (!ret_nav_)
   {
     traj = nav_algorithm_->calculator(pose, velocity);
-
     if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
     {
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
@@ -393,9 +405,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
         traj = dkpl_.front()->docking_nav_->calculator(pose, velocity);
       }
     }
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
   }
   else
+  {
     traj = rotate_algorithm_->calculator(pose, velocity);
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
+  }
 
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
@@ -415,7 +435,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
   robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
-  robot_nav_2d_msgs::Path2D plan = transformed_plan_;
+  robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
   if (start_docking_)
   {
     local_goal = goal_pose_;
@@ -459,8 +479,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     // nh_.setParam("yaw_goal_tolerance", original_yaw_goal_tolerance_);
     std::string algorithm_nav_name;
     planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
-    robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name);
+    robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
-    nh_.setParam(algorithm_nav_name, original_papams_);
+    parent_.setParam(algorithm_nav_name, original_papams_);
     nh_algorithm.setParam("allow_rotate", false);
   }
   return ret_nav_ && ret_angle_ && dock_ok;
@@ -476,7 +496,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
       {
         if (!dkpl_.front()->initialized_)
         {
-          dkpl_.front()->initialize(nh_, tf_, costmap_robot_, traj_generator_);
+          dkpl_.front()->initialize(parent_, tf_, costmap_robot_, traj_generator_);
           dkpl_.front()->initialized_ = true;
         }
         else
@@ -540,7 +560,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
         {
           if(!dkpl_.empty()) 
           {
-            if(dkpl_.front()) delete(dkpl_.front());
+            // if(dkpl_.front()) delete(dkpl_.front());
             dkpl_.erase(dkpl_.begin());
           }
           start_docking_ = false;
@@ -559,17 +579,50 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name)
-    : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false),
+    : initialized_(false),
-      is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
+      is_detected_(false),
+      is_goal_reached_(false),
+      following_(false),
+      allow_rotate_(false),
+      xy_goal_tolerance_(0.05),
+      yaw_goal_tolerance_(0.05),
+      min_lookahead_dist_(0.4),
+      max_lookahead_dist_(1.0),
+      lookahead_time_(1.5),
+      angle_threshold_(0.4),
+      name_(name),
+      docking_planner_name_(),
+      docking_nav_name_(),
+      docking_planner_(nullptr),
+      docking_nav_(nullptr),
+      linear_(),
+      angular_(),
+      nh_(),
+      nh_priv_(),
+      tf_(nullptr),
+      costmap_robot_(nullptr),
+      traj_generator_(),
+      docking_planner_loader_(),
+      docking_nav_loader_(),
+      detected_timeout_wt_(),
+      delayed_wt_(),
+      delayed_(false),
+      detected_timeout_(false),
+      robot_base_frame_(),
+      maker_goal_frame_()
 {
 }
 
 pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
 {
-  if (docking_planner_)
+  detected_timeout_wt_.stop();
+  delayed_wt_.stop();
+  if (docking_planner_ != nullptr) {
     docking_planner_.reset();
-  if (docking_nav_)
+  }
+  if (docking_nav_ != nullptr) {
     docking_nav_.reset();
+  }
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
@@ -585,14 +638,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
-      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>(
+      std::string path_file_so = loader.findLibraryPath(docking_planner_name_);
+      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
         path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
       docking_planner_ = docking_planner_loader_();
       if (!docking_planner_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create global planner \"%s\": returned null pointer", docking_planner_name_.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create global planner");
       }
       docking_planner_->initialize(name_, costmap_robot_);
       robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str());
@@ -608,16 +662,17 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath(docking_nav_name_);
       docking_nav_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations);
       docking_nav_ = docking_nav_loader_();
       if (!docking_nav_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create docking nav");
       }
-      robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_, docking_nav_name_);
+      robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_);
       docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str());
     }
@@ -654,13 +709,22 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   nh_priv_.param("lookahead_time", lookahead_time_, 1.5);
   nh_priv_.param("angle_threshold", angle_threshold_, 0.4);
 
-  detected_timeout_wt_ =
+  robot::log_info("delay: %f", delay_time);
-      nh_priv_.createWallTimer(::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this);
+  robot::log_info("timeout: %f", time_out);
+  robot::log_info("xy_goal_tolerance: %f", xy_goal_tolerance_);
+  robot::log_info("yaw_goal_tolerance: %f", yaw_goal_tolerance_);
+  robot::log_info("min_lookahead_dist: %f", min_lookahead_dist_);
+  robot::log_info("max_lookahead_dist: %f", max_lookahead_dist_);
+  robot::log_info("lookahead_time: %f", lookahead_time_);
+  robot::log_info("angle_threshold: %f", angle_threshold_);
+
+  detected_timeout_wt_ = ::robot::WallTimer(
+      ::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this, false, false);
   detected_timeout_wt_.start();
   robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
 
-  delayed_wt_ =
+  delayed_wt_ = ::robot::WallTimer(
-      nh_priv_.createWallTimer(::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this);
+      ::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this, false, false);
   delayed_wt_.start();
   robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
 

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp

@@ -15,6 +15,7 @@
 #include <boost/dll/alias.hpp>
 
 pnkx_local_planner::PNKXGoStraightLocalPlanner::PNKXGoStraightLocalPlanner()
+  : PNKXLocalPlanner()
 {
 }
 
@@ -35,6 +36,16 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
   if (!initialized_)
   {
     robot::log_info_at(__FILE__, __LINE__, "Using parent name \"%s\"", name.c_str());
+    if(costmap_robot == nullptr)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "Costmap2DROBOT is nullptr");
+      throw robot_nav_core2::LocalPlannerException("Costmap2DROBOT is nullptr");
+    }
+    if(tf == nullptr)
+    {
+      robot::log_error_at(__FILE__, __LINE__, "TFListener is nullptr");
+      throw robot_nav_core2::LocalPlannerException("TFListener is nullptr");
+    }
     tf_ = tf;
     costmap_robot_ = costmap_robot;
     costmap_ = costmap_robot_->getCostmap();
@@ -69,7 +80,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -77,7 +88,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_nav_name = std::string("pnkx_local_planner::GoStraight");
@@ -93,14 +104,14 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string goal_checker_name;
@@ -115,7 +126,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
       goal_checker_->initialize(nh_goal_checker);
@@ -123,7 +134,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str());

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -17,7 +17,21 @@
 #include <robot/robot.h>
 
 pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner()
-  : initialized_(false)
+  : initialized_(false),
+    traj_generator_(nullptr),
+    nav_algorithm_(nullptr),
+    rotate_algorithm_(nullptr),
+    goal_checker_(nullptr),
+    tf_(nullptr),
+    costmap_(nullptr),
+    costmap_robot_(nullptr),
+    info_(),
+    update_costmap_before_planning_(false),
+    ret_angle_(false),
+    ret_nav_(false),
+    yaw_goal_tolerance_(0.0),
+    xy_goal_tolerance_(0.0),
+    lock_(false)
 {
 }
 
@@ -74,7 +88,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -83,7 +97,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
     
     std::string algorithm_nav_name; 
@@ -93,20 +107,21 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
+      robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str());
       nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
       nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
-      // exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string algorithm_rotate_name;
@@ -121,7 +136,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!rotate_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
       }
       rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@@ -129,7 +144,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      // exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
     }
 
     std::string goal_checker_name;
@@ -145,7 +160,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       goal_checker_->initialize(parent_);
       robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
@@ -153,7 +168,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     catch (const std::exception &ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
 
     this->initializeOthers();

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp

@@ -14,6 +14,7 @@
 #include <boost/dll/alias.hpp>
 
 pnkx_local_planner::PNKXRotateLocalPlanner::PNKXRotateLocalPlanner()
+  : PNKXLocalPlanner()
 {
 }
 
@@ -63,7 +64,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!traj_generator_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
       }
       robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
       traj_generator_->initialize(nh_traj_gen);
@@ -71,7 +72,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
     }
 
     std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
@@ -87,7 +88,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
       }
       robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
       nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
@@ -95,7 +96,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
     }
 
     std::string goal_checker_name;
@@ -110,7 +111,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
       if (!goal_checker_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
-        exit(1);
+        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
       }
       robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
       goal_checker_->initialize(nh_goal_checker);
@@ -118,7 +119,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     catch (const std::exception& ex)
     {
       robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
-      exit(1);
+      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
     }
     
     robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());

src/Libraries/robot_cpp/include/robot/node_handle.h

@@ -283,6 +283,14 @@ namespace robot
        */
       bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const;
 
+      /**
+       * @brief Get a parameter as robot_xmlrpcpp::XmlRpcValue (converted from YAML).
+       * @param key The parameter key (supports nested keys with '/' separator).
+       * @param v Storage for the retrieved value. Left unchanged if key not found.
+       * @return true if the parameter was retrieved and converted successfully, false otherwise.
+       */
+      bool getParam (const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const;
+
       /**
        * @brief Template method to get a parameter value (without default).
        *

src/Libraries/robot_cpp/include/robot/robot.h

@@ -2,11 +2,17 @@
 #define ROBOT_ROBOT_H
 
 #include <robot/init.h>
-#include <robot/time.h>
-#include <robot/timer.h>
-#include <robot/rate.h>
 #include <robot/console.h>
 #include <robot/node_handle.h>
 #include <robot/plugin_loader_helper.h>
 
+
+#include <robot/time.h>
+#include <robot/timer.h>
+#include <robot/duration.h>
+#include <robot/wall_timer.h>
+#include <robot/rate.h>
+#include <robot/exception.h>
+#include <robot/macros.h>
+#include <robot/platform.h>
 #endif

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -651,7 +651,8 @@ namespace robot
           std::string key = it->first.as<std::string>();
           const YAML::Node &value = it->second;
           std::string full_key = prefix.empty() ? key : prefix + "/" + key;
-
+          if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos)
+            continue;
           if (value.IsMap())
           {
             std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl;
@@ -1727,6 +1728,87 @@ namespace robot
     return true;
   }
 
+  namespace
+  {
+    bool yamlToXmlRpc(const YAML::Node &y, robot_xmlrpcpp::XmlRpcValue &out)
+    {
+      if (!y.IsDefined())
+        return false;
+      if (y.IsScalar())
+      {
+        try
+        {
+          std::string s = y.as<std::string>();
+          std::string lower = s;
+          std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
+          if (lower == "true" || lower == "1" || lower == "yes" || lower == "on")
+          {
+            out = robot_xmlrpcpp::XmlRpcValue(true);
+            return true;
+          }
+          if (lower == "false" || lower == "0" || lower == "no" || lower == "off")
+          {
+            out = robot_xmlrpcpp::XmlRpcValue(false);
+            return true;
+          }
+          try
+          {
+            int i = y.as<int>();
+            out = robot_xmlrpcpp::XmlRpcValue(i);
+            return true;
+          }
+          catch (...) {}
+          try
+          {
+            double d = y.as<double>();
+            out = robot_xmlrpcpp::XmlRpcValue(d);
+            return true;
+          }
+          catch (...) {}
+          out = robot_xmlrpcpp::XmlRpcValue(s);
+          return true;
+        }
+        catch (...)
+        {
+          return false;
+        }
+      }
+      if (y.IsSequence())
+      {
+        out = robot_xmlrpcpp::XmlRpcValue();
+        out.setSize(static_cast<int>(y.size()));
+        for (size_t i = 0; i < y.size(); ++i)
+        {
+          robot_xmlrpcpp::XmlRpcValue item;
+          if (yamlToXmlRpc(y[i], item))
+            out[static_cast<int>(i)] = item;
+        }
+        return true;
+      }
+      if (y.IsMap())
+      {
+        out = robot_xmlrpcpp::XmlRpcValue();
+        for (YAML::const_iterator it = y.begin(); it != y.end(); ++it)
+        {
+          std::string k = it->first.as<std::string>();
+          robot_xmlrpcpp::XmlRpcValue item;
+          if (yamlToXmlRpc(it->second, item))
+            out[k] = item;
+        }
+        return true;
+      }
+      return false;
+    }
+  }
+
+  bool NodeHandle::getParam(const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const
+  {
+    YAML::Node value = getNestedValue(key);
+    if (!value.IsDefined())
+      return false;
+    return yamlToXmlRpc(value, v);
+  }
+
   template <typename T>
   T NodeHandle::param(const std::string &param_name) const
   {

src/Libraries/robot_nav_2d_utils/CMakeLists.txt

@@ -132,6 +132,11 @@ else()
   )
   target_link_libraries(${PROJECT_NAME}
       INTERFACE
+        ${PROJECT_NAME}_conversions
+        ${PROJECT_NAME}_path_ops
+        ${PROJECT_NAME}_polygons
+        ${PROJECT_NAME}_bounds
+        ${PROJECT_NAME}_tf_help
         ${PACKAGES_DIR}
         ${Boost_LIBRARIES}
         ${TF3_LIBRARY}

src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h

@@ -407,6 +407,7 @@ namespace robot
 			 * @return True if docking command succeeded.
 			 */
 			virtual bool dockTo(const robot_protocol_msgs::Order &msg, 
+													const std::string &marker,
 													const robot_geometry_msgs::PoseStamped &goal,
 													double xy_goal_tolerance = 0.0,
 													double yaw_goal_tolerance = 0.0) = 0;

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h

@@ -41,6 +41,7 @@
 #include <robot_geometry_msgs/Twist.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <tf3/buffer_core.h>
+#include <robot_nav_msgs/Odometry.h>
 #include <memory>
 
 namespace robot_nav_core
@@ -148,6 +149,12 @@ namespace robot_nav_core
      */
     virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
 
+    /**
+     * @brief Set the odometry of the robot
+     * @param odom The odometry of the robot
+     */
+    virtual void setOdom(robot_nav_msgs::Odometry *odom) { odom_ = odom; }
+
     /**
      * @brief  Virtual destructor for the interface
      */
@@ -155,6 +162,11 @@ namespace robot_nav_core
 
   protected:
     BaseLocalPlanner() {}
+
+    /**
+     * @brief The odometry of the robot
+     */
+     robot_nav_msgs::Odometry *odom_;
   };
 } // namespace robot_nav_core
 

src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h

@@ -189,11 +189,6 @@ namespace robot_nav_core_adapter
     robot_nav_2d_msgs::Pose2DStamped last_goal_;
     bool has_active_goal_;
 
-    /**
-     * @brief The odometry of the robot
-     */
-    robot_nav_msgs::Odometry odom_;
-
     // Plugin handling
     boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
     robot_nav_core2::LocalPlanner::Ptr planner_;

src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp

@@ -49,7 +49,11 @@
 namespace robot_nav_core_adapter
 {
 
-  LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false)
+  LocalPlannerAdapter::LocalPlannerAdapter() 
+  : initialized_(false), 
+    tf_(nullptr), 
+    costmap_robot_(nullptr), 
+    has_active_goal_(false)
   {
   }
 
@@ -139,6 +143,15 @@ namespace robot_nav_core_adapter
     {
       return false;
     }
+    if(tf_ == nullptr)
+    {
+      robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__);
+      throw std::runtime_error("Failed to create the tf");
+    }
+    else
+    {
+      robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
+    }
     if (planner_name != last_config_.planner_name)
     {
       robot_nav_core2::LocalPlanner::Ptr old_planner = planner_;
@@ -278,7 +291,7 @@ namespace robot_nav_core_adapter
 
   robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
   {
-    return odom_.twist.twist;
+    return odom_->twist.twist;
   }
 
   bool LocalPlannerAdapter::islock()
@@ -343,7 +356,7 @@ namespace robot_nav_core_adapter
     if (!getRobotPose(pose2d))
       return false;
 
-    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist);
+    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist);
     bool ret = planner_->isGoalReached(pose2d, velocity);
     if (ret)
     {

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -19,6 +19,7 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 find_package(Boost REQUIRED COMPONENTS filesystem system)
 find_package(Threads REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(console_bridge REQUIRED)
 
 if (NOT BUILDING_WITH_CATKIN)
 
@@ -44,6 +45,7 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_nav_2d_utils
     robot_cpp
     robot_move_base_msgs
+    laser_filter
   )
   find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()
@@ -108,6 +110,7 @@ if(BUILDING_WITH_CATKIN)
     PRIVATE Boost::filesystem Boost::system
     dl
     ${TF3_LIBRARY}
+    ${console_bridge_LIBRARIES}
   )
 
 else()
@@ -123,6 +126,7 @@ else()
     PUBLIC
       ${PACKAGES_DIR}
       ${TF3_LIBRARY}
+      ${console_bridge_LIBRARIES}
     PRIVATE
       Boost::filesystem Boost::system
       yaml-cpp
@@ -149,6 +153,7 @@ if(BUILDING_WITH_CATKIN)
     Boost::filesystem Boost::system
     dl
     ${TF3_LIBRARY}
+    ${console_bridge_LIBRARIES}
   )
 else()
   target_link_libraries(${PROJECT_NAME}_main PRIVATE
@@ -157,6 +162,7 @@ else()
     Boost::filesystem Boost::system
     dl
     ${TF3_LIBRARY}
+    ${console_bridge_LIBRARIES}
   )
 
   # Configure RPATH to find libraries in devel space

src/Navigations/Packages/move_base/include/move_base/move_base.h

@@ -35,6 +35,17 @@
 
 namespace move_base
 {
+  template <class T>
+  void move_parameter(robot::NodeHandle& old_h, robot::NodeHandle& new_h, std::string name,T& value)
+  {
+    if (!old_h.hasParam(name))
+      return;
+  
+    old_h.getParam(name, value);
+    new_h.setParam(name, value);
+  }
+
+
   // typedefs to help us out with the action server so that we don't hace to type so much
   typedef move_base::SimpleActionServer<robot_move_base_msgs::MoveBaseAction> MoveBaseActionServer;
 
@@ -292,6 +303,7 @@ namespace move_base
      * @return True if docking command succeeded.
      */
     virtual bool dockTo(const robot_protocol_msgs::Order &msg,
+                        const std::string &marker,
                         const robot_geometry_msgs::PoseStamped &goal,
                         double xy_goal_tolerance = 0.0,
                         double yaw_goal_tolerance = 0.0) override;