update

examples/NavigationExample/Program.cs

@@ -497,13 +497,14 @@ namespace NavigationExample
 
         static void Main(string[] args)
         {
-            // Create TF listener
+            // Create TF listener (required for costmap and local planner; must be valid when calling navigation_initialize)
             NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create();
             if (tfHandle.ptr == IntPtr.Zero)
             {
                 LogError("Failed to create TF listener");
                 return;
             }
+            Console.WriteLine($"[NavigationExample] TF listener created, handle = 0x{tfHandle.ptr.ToInt64():X16}");
 
             // Inject a static TF so costmap can immediately canTransform(map <-> base_link).
             // If you already publish TF from localization/odometry, you can remove this call.
@@ -543,15 +544,35 @@ namespace NavigationExample
                 return;
             }
 
-            // Initialize navigation
+            // Initialize navigation (passes TF to move_base; navigation keeps its own copy, so tfHandle can be destroyed later)
             if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
             {
-                LogError("Failed to initialize navigation");
+                LogError("Failed to initialize navigation (check native log for 'Invalid TF listener' or 'tf is nullptr')");
                 NavigationAPI.navigation_destroy(navHandle);
                 NavigationAPI.tf_listener_destroy(tfHandle);
                 return;
             }
 
+            while(true)
+            {
+            // Get navigation feedback
+                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 with TF successfully");
+
             // Set robot footprint
             NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
             {
@@ -562,17 +583,6 @@ namespace NavigationExample
             };
             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);
-            }
   
             IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
             NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
@@ -650,8 +660,7 @@ namespace NavigationExample
             NavigationAPI.navigation_add_odometry(navHandle, "odometry",  odometryHandle);
 
             // Add static map: đọc maze.yaml rồi load ảnh và cập nhật mapMetaData
-            string mapYamlPath = args.Length > 0 ? args[0] : Path.Combine(
-                "/home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws/src/Managerments/maps/maze", "maze.yaml");
+            string mapYamlPath =  "maze.yaml";
             int mapWidth, mapHeight;
             byte[] data;
             MazeMapConfig mapConfig;
@@ -715,8 +724,6 @@ namespace NavigationExample
     
             NavigationAPI.navigation_add_static_map(navHandle, "/map",  occupancyGrid);
 
-            System.Threading.Thread.Sleep(500);
-
             NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
             if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
             {

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

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

@@ -143,7 +143,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/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp

@@ -157,6 +157,15 @@ namespace robot_nav_core_adapter
           robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str());
           throw std::runtime_error("Failed to load planner " + planner_name);
         }
+        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__);
+        }
         new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
         planner_ = new_planner;
         planner_loader_ = new_loader;

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -802,7 +802,11 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
   // Swap planner
   try
   {
-    tc_->swapPlanner(position_planner_name_);
+    if(!tc_->swapPlanner(position_planner_name_))
+    {
+      robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
+      return false;
+    }
   }
   catch (const std::exception &e)
   {
@@ -917,7 +921,11 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
 
   try
   {
-    tc_->swapPlanner(position_planner_name_);
+    if(!tc_->swapPlanner(position_planner_name_))
+    {
+      robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
+      return false;
+    }
   }
   catch (const std::exception &e)
   {
@@ -1027,7 +1035,11 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   try
   {
-    tc_->swapPlanner(docking_planner_name_);
+    if(!tc_->swapPlanner(docking_planner_name_))
+    {
+      robot::log_error("[MoveBase::dockTo] Failed to swapPlanner");
+      return false;
+    }
   }
   catch (const std::exception &e)
   {
@@ -1091,7 +1103,20 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
 
   try
   {
-    tc_->swapPlanner(go_straight_planner_name_);
+    robot::log_info("[MoveBase::moveStraightTo] Entry swapPlanner");
+    if(!tc_->swapPlanner(go_straight_planner_name_))
+    {
+      if(tf_ == nullptr)
+      {
+        robot::log_error("[MoveBase::moveTo] tf_ pointer is null!");
+        throw std::runtime_error("Null 'tf_' pointer encountered");
+      }
+      else
+      {
+        robot::log_info("[MoveBase::moveTo] tf_ pointer is not null!");
+      }
+      return false;
+    }
   }
   catch (const std::exception &e)
   {
@@ -1198,7 +1223,11 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
 
   try
   {
-    tc_->swapPlanner(rotate_planner_name_);
+    if(!tc_->swapPlanner(rotate_planner_name_))
+    {
+      robot::log_error("[MoveBase::rotateTo] Failed to swapPlanner");
+      return false;
+    }
   }
   catch (const std::exception &e)
   {