From 83f0e85e4a247c5858ae599e13ec157fd2b9271e Mon Sep 17 00:00:00 2001
From: hoangson02 <sonvh@phenikaa-x.com>
Date: Thu, 26 Feb 2026 11:12:07 +0000
Subject: [PATCH] update

---
 .../Packages/move_base/src/move_base.cpp      | 39 ++++++++++++++++---
 1 file changed, 34 insertions(+), 5 deletions(-)

diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp
index 7c31ce2..3fb3859 100644
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/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)
   {