uodate

src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp

@@ -201,7 +201,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     {
       sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front();
       sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
-      std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl;
+      // std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl;
     }
 
     // Process index_s with multiple elements

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

@@ -790,7 +790,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
     robot_nav_2d_msgs::Pose2DStamped robot;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot))
     {
-      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame");
     }
 
     double dist_thresh_sq = dist_behind_robot * dist_behind_robot;
@@ -855,7 +855,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
     robot_nav_2d_msgs::Pose2DStamped robot_pose;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
     {
-      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame");
     }
 
     // we'll discard points on the plan that are outside the local costmap

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

@@ -226,15 +226,16 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
 
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
+  // robot::log_info("pose: %f %f %f", pose.pose.x, pose.pose.y, pose.pose.theta);
   robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
                              local_goal_pose = this->transformPoseToLocal(goal_pose_);
-
-  if (pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+  // robot::log_info("local_start_pose: %f %f %f", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
+  // robot::log_info("local_goal_pose: %f %f %f", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
+  if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
   {
-    robot::log_debug_at(__FILE__, __LINE__, "Transform global plan is successful");
-  }
-  else
     robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
+    throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
+  }
 
   double x_direction, y_direction, theta_direction;
   if (!ret_nav_)
@@ -276,6 +277,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeV
   }
   catch (const robot_nav_core2::PlannerException &e)
   {
+    robot::log_warning_at(__FILE__, __LINE__, "PNKXLocalPlanner computing velocity commands has been broken: %s", e.what());
     throw robot_nav_core2::LocalPlannerException("computing velocity commands has been broken");
     return cmd_vel;
   }

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

@@ -111,7 +111,7 @@ bool pnkx_local_planner::transformGlobalPlan(
     robot_nav_2d_msgs::Pose2DStamped robot_pose;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
     {
-      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("transformGlobalPlan: Unable to transform robot pose into global plan's frame");
     }
 
     transformed_plan.header.frame_id = costmap->getGlobalFrameID();
@@ -172,8 +172,8 @@ bool pnkx_local_planner::transformGlobalPlan(
       // Fixed error bound of 1 meter for now. Can reduce to a portion of the map size or based on the resolution
       if (pnkx_local_planner::getSquareDistance(costmap_pose.pose, w.pose) < sq_prune_dist)
       {
-        robot::log_debug_at(__FILE__, __LINE__, "Nearest waypoint to <%f, %f> is <%f, %f>\n",
-          costmap_pose.pose.x, costmap_pose.pose.y, w.pose.x, w.pose.y);
+        // robot::log_debug_at(__FILE__, __LINE__, "Nearest waypoint to <%f, %f> is <%f, %f>\n",
+        //   costmap_pose.pose.x, costmap_pose.pose.y, w.pose.x, w.pose.y);
         break;
       }
 

src/Libraries/robot_nav_2d_utils/CMakeLists.txt

@@ -37,7 +37,7 @@ if (NOT BUILDING_WITH_CATKIN)
     tf3
     robot_tf3_geometry_msgs
     robot_cpp 
-    console_bridge
+    data_convert
   )
 
 else()

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h

@@ -42,6 +42,18 @@
 
 namespace robot_nav_2d_utils
 {
+
+  /**
+   * @brief Transform a PoseStamped from one frame to another
+   *
+   * @param tf Smart pointer to TFListener
+   * @param in_pose Pose to transform
+   * @param out_pose Place to store the resulting transformed pose
+   * @param frame Frame to transform the pose into
+   * @return True if successful transform
+   */
+  bool transform(const TFListenerPtr tf, const ::robot_geometry_msgs::PoseStamped &in_pose,::robot_geometry_msgs::PoseStamped &out_pose, const ::std::string &frame);
+
   /**
    * @brief Transform a PoseStamped from one frame to another while catching exceptions
    *

src/Libraries/robot_nav_2d_utils/package.xml

@@ -25,6 +25,7 @@
   <build_depend>nav_grid</build_depend>
   <build_depend>robot_nav_core2</build_depend>
   <build_depend>robot_cpp</build_depend>
+  <build_depend>data_convert</build_depend>
   
   <run_depend>robot_nav_2d_msgs</run_depend>
   <run_depend>robot_geometry_msgs</run_depend>
@@ -32,4 +33,5 @@
   <run_depend>nav_grid</run_depend>
   <run_depend>robot_nav_core2</run_depend>
   <run_depend>robot_cpp</run_depend>
+  <run_depend>data_convert</run_depend>
 </package>

src/Libraries/robot_nav_2d_utils/src/conversions.cpp

@@ -31,6 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
+#include <robot/robot.h>
+#include <data_convert/data_convert.h>
 #include <robot_nav_2d_utils/conversions.h>
 #include <vector>
 #include <string>
@@ -105,7 +107,7 @@ namespace robot_nav_2d_utils
     pose2d.header = pose.header;
     pose2d.pose.x = pose.pose.position.x;
     pose2d.pose.y = pose.pose.position.y;
-    // pose2d.pose.theta = tf::getYaw(pose.pose.orientation);
+    pose2d.pose.theta = data_convert::getYaw(pose.pose.orientation);
     return pose2d;
   }
 
@@ -114,7 +116,7 @@ namespace robot_nav_2d_utils
     robot_geometry_msgs::Pose pose;
     pose.position.x = pose2d.x;
     pose.position.y = pose2d.y;
-    // pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
+    pose.orientation = data_convert::createQuaternionMsgFromYaw(pose2d.theta);
     return pose;
   }
 
@@ -134,7 +136,7 @@ namespace robot_nav_2d_utils
     pose.header.stamp = stamp;
     pose.pose.position.x = pose2d.x;
     pose.pose.position.y = pose2d.y;
-    // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
+    pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(pose2d.theta);
     return pose;
   }
 
@@ -284,10 +286,10 @@ namespace robot_nav_2d_utils
     info.height = metadata.height;
     info.origin_x = metadata.origin.position.x;
     info.origin_y = metadata.origin.position.y;
-    // if (std::abs(tf::getYaw(metadata.origin.orientation)) > 1e-5)
-    // {
-    //   std::cout <<  "Conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring" << std::endl;
-    // }
+      if (std::abs(data_convert::getYaw(metadata.origin.orientation)) > 1e-5)
+      {
+        robot::log_warning("[conversions] conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring");
+      }
     return info;
   }
 

src/Libraries/robot_nav_2d_utils/src/tf_help.cpp

@@ -41,36 +41,54 @@
 
 namespace robot_nav_2d_utils
 {
+
+  bool transform(const TFListenerPtr tf, 
+                     const ::robot_geometry_msgs::PoseStamped &in_pose, 
+                     ::robot_geometry_msgs::PoseStamped &out_pose, const ::std::string &frame)
+  {
+    const std::string target_frame = frame;
+    const std::string source_frame = in_pose.header.frame_id;
+    std::string *error_msg;
+
+    if(tf->canTransform(target_frame, source_frame, tf3::Time(), error_msg))
+    {
+      tf3::TransformStampedMsg transform =tf->lookupTransform(target_frame, source_frame, tf3::Time());
+      tf3::doTransform(in_pose, out_pose, transform);
+      return true;
+    }
+    return false;
+  }
+
   bool transformPose(const TFListenerPtr tf, const ::std::string frame,
                      const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose,
                      const bool extrapolation_fallback)
   {
-    // if (in_pose.header.frame_id == frame)
-    // {
-    //   out_pose = in_pose;
-    //   return true;
-    // }
+    if (in_pose.header.frame_id == frame)
+    {
+      out_pose = in_pose;
+      return true;
+    }
 
-    // try
-    // {
-    //   tf->transform(in_pose, out_pose, frame);
-    //   return true;
-    // }
-    // catch (tf3::ExtrapolationException &ex)
-    // {
-    //   if (!extrapolation_fallback)
-    //     throw;
-    //   robot_geometry_msgs::PoseStamped latest_in_pose;
-    //   latest_in_pose.header.frame_id = in_pose.header.frame_id;
-    //   latest_in_pose.pose = in_pose.pose;
-    //   tf->transform(latest_in_pose, out_pose, frame);
-    //   return true;
-    // }
-    // catch (tf3::TransformException &ex)
-    // {
-    //   robot::log_error("Exception in transformPose: %s", ex.what());
-    //   return false;
-    // }
+    try
+    {
+      transform(tf, in_pose, out_pose, frame);
+      return true;
+    }
+    catch (tf3::ExtrapolationException &ex)
+    {
+      if (!extrapolation_fallback)
+        throw;
+      robot_geometry_msgs::PoseStamped latest_in_pose;
+      latest_in_pose.header.frame_id = in_pose.header.frame_id;
+      latest_in_pose.pose = in_pose.pose;
+      transform(tf, latest_in_pose, out_pose, frame);
+      return true;
+    }
+    catch (tf3::TransformException &ex)
+    {
+      robot::log_error("Exception in transformPose: %s", ex.what());
+      return false;
+    }
     return false;
   }
 

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

@@ -188,7 +188,7 @@ namespace robot_nav_core_adapter
     }
     catch (const robot_nav_core2::PlannerException &e)
     {
-      std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl;
+      robot::log_error_at(__FILE__, __LINE__, "LocalPlannerAdapter computeVelocityCommands exception: %s", e.what());
       return false;
     }
     cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);

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

@@ -66,10 +66,13 @@ namespace move_base
   public:
     /**
      * @brief  Constructor for the actions
-     * @param name The name of the action
      * @param tf A reference to a TransformListener
      */
     MoveBase(robot::TFListenerPtr tf);
+    
+    /**
+     * @brief Default constructor for the actions
+     */
     MoveBase();
 
     /**
@@ -78,7 +81,9 @@ namespace move_base
      */
     virtual void initialize(robot::TFListenerPtr tf) override;
 
-
+    /**
+     * @brief Initialize the cost-to-occupancy lookup table for costmap conversion
+     */
     static void initializeCostToOccupancyLUT();
 
     /**
@@ -423,8 +428,8 @@ namespace move_base
     void setYawGoalTolerance(double yaw_goal_tolerance);
 
     /**
-     * @brief Set velocity for yaw goal tolerance of the robot
-     * @param xy_goal_tolerance the value command
+     * @brief Set the XY goal tolerance for the robot
+     * @param xy_goal_tolerance The XY tolerance value in meters
      */
     void setXyGoalTolerance(double xy_goal_tolerance);
 
@@ -442,6 +447,15 @@ namespace move_base
      */
     bool makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);
 
+    /**
+     * @brief  Make a new global plan
+     * @param  msg The order message
+     * @param  goal The goal to plan to
+     * @param  plan Will be filled in with the plan made by the planner
+     * @return  True if planning succeeds, false otherwise
+     */
+    bool makePlan(const robot_protocol_msgs::Order &msg, const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);
+
     /**
      * @brief  Load the recovery behaviors for the navigation stack from the parameter server
      * @return True if the recovery behaviors were loaded successfully, false otherwise
@@ -470,10 +484,16 @@ namespace move_base
      */
     void resetState();
 
-    // void goalCB(const robot_geometry_msgs::PoseStamped &goal);
 
+    /**
+     * @brief Thread function that continuously plans paths to the goal
+     */
     void planThread();
 
+    /**
+     * @brief Callback function to execute a move_base goal
+     * @param move_base_goal The goal message to execute
+     */
     void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
 
     /**
@@ -481,12 +501,34 @@ namespace move_base
      */
     void setupActionServerCallbacks();
 
+    /**
+     * @brief Check if a quaternion is valid (normalized)
+     * @param q The quaternion to validate
+     * @return True if the quaternion is valid, false otherwise
+     */
     bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);
 
+    /**
+     * @brief Get the robot's pose in the global frame using a specific costmap
+     * @param global_pose Output parameter with the robot's current pose
+     * @param costmap The costmap to use for pose lookup
+     * @return True if pose was successfully retrieved
+     */
     bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap);
 
+    /**
+     * @brief Calculate the Euclidean distance between two poses
+     * @param p1 First pose
+     * @param p2 Second pose
+     * @return The distance between the two poses in meters
+     */
     double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);
 
+    /**
+     * @brief Transform a goal pose to the global frame
+     * @param goal_pose_msg The goal pose to transform
+     * @return The transformed pose in the global frame
+     */
     robot_geometry_msgs::PoseStamped goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg);
 
     /**
@@ -495,72 +537,155 @@ namespace move_base
     void wakePlanner(const robot::TimerEvent &event);
 
   private:
+    /** @brief Flag indicating if the move_base has been initialized */
     bool initialized_;
+    /** @brief Transform listener for coordinate frame transformations */
     robot::TFListenerPtr tf_;
+    /** @brief Private node handle for ROS parameters and topics */
     robot::NodeHandle private_nh_;
 
+    /** @brief Action server for move_base goals */
     MoveBaseActionServer *as_;
 
+    /** @brief Function loader for global planner plugins */
     boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
+    /** @brief Function loader for local planner (controller) plugins */
     boost::function<robot_nav_core::BaseLocalPlanner::Ptr()> controller_loader_;
+    /** @brief Map of recovery behavior loaders by name */
     std::map<std::string, boost::function<robot_nav_core::RecoveryBehavior::Ptr()>> recovery_loaders_;
 
+    /** @brief Local planner (trajectory controller) instance */
     robot_nav_core::BaseLocalPlanner::Ptr tc_;
+    /** @brief Global planner instance */
     robot_nav_core::BaseGlobalPlanner::Ptr planner_;
+    /** @brief Vector of recovery behavior instances */
     std::vector<robot_nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
     
+    /** @brief Costmap for the local planner/controller */
     robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
+    /** @brief Costmap for the global planner */
     robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_;    
+    /** @brief Robot base frame name and global frame name */
     std::string robot_base_frame_, global_frame_;
+    /** @brief Names of recovery behaviors to use */
     std::vector<std::string> recovery_behavior_names_;
+    /** @brief Current index in the recovery behavior list */
     unsigned int recovery_index_;
+    /** @brief Flag indicating if recovery behaviors are enabled */
     bool recovery_behavior_enabled_;
 
+    /** @brief Current global pose of the robot */
     robot_geometry_msgs::PoseStamped global_pose_;
-    double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
-    double planner_patience_, controller_patience_;
+    /** @brief Frequency for planner execution (Hz) */
+    double planner_frequency_;
+    /** @brief Frequency for controller execution (Hz) */
+    double controller_frequency_;
+    /** @brief Inscribed radius of the robot footprint (meters) */
+    double inscribed_radius_;
+    /** @brief Circumscribed radius of the robot footprint (meters) */
+    double circumscribed_radius_;
+    /** @brief Maximum time to wait for a valid plan before triggering recovery (seconds) */
+    double planner_patience_;
+    /** @brief Maximum time to wait for valid control before triggering recovery (seconds) */
+    double controller_patience_;
+    /** @brief Maximum number of planning retries before giving up */
     int32_t max_planning_retries_;
+    /** @brief Current number of planning retries */
     uint32_t planning_retries_;
-    double conservative_reset_dist_, clearing_radius_;
-    bool shutdown_costmaps_, clearing_rotation_allowed_;
-    bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
-    double oscillation_timeout_, oscillation_distance_;
+    /** @brief Conservative distance threshold for resetting costmaps (meters) */
+    double conservative_reset_dist_;
+    /** @brief Radius around robot to clear obstacles during recovery (meters) */
+    double clearing_radius_;
+    /** @brief Flag to shutdown costmaps when not in use */
+    bool shutdown_costmaps_;
+    /** @brief Flag allowing rotation during costmap clearing */
+    bool clearing_rotation_allowed_;
+    /** @brief Flag to clear costmap when making a new plan */
+    bool make_plan_clear_costmap_;
+    /** @brief Flag to add unreachable goals to costmap */
+    bool make_plan_add_unreachable_goal_;
+    /** @brief Time threshold for detecting oscillation (seconds) */
+    double oscillation_timeout_;
+    /** @brief Distance threshold for detecting oscillation (meters) */
+    double oscillation_distance_;
 
+    /** @brief Current state of the move_base (PLANNING, CONTROLLING, CLEARING) */
     MoveBaseState state_;
+    /** @brief Trigger that caused recovery behavior (PLANNING_R, CONTROLLING_R, OSCILLATION_R) */
     RecoveryTrigger recovery_trigger_;
 
-    robot::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
+    /** @brief Timestamp of the last valid plan */
+    robot::Time last_valid_plan_;
+    /** @brief Timestamp of the last valid control command */
+    robot::Time last_valid_control_;
+    /** @brief Timestamp of the last oscillation reset */
+    robot::Time last_oscillation_reset_;
+    /** @brief Pose used for oscillation detection */
     robot_geometry_msgs::PoseStamped oscillation_pose_;
 
-    // set up plan triple buffer
+    /** @brief Plan triple buffer: plan from the planner thread */
     std::vector<robot_geometry_msgs::PoseStamped> *planner_plan_;
+    /** @brief Plan triple buffer: latest plan (intermediate buffer) */
     std::vector<robot_geometry_msgs::PoseStamped> *latest_plan_;
+    /** @brief Plan triple buffer: plan used by the controller */
     std::vector<robot_geometry_msgs::PoseStamped> *controller_plan_;
 
-    // set up the planner's thread
+    /** @brief Flag to control planner thread execution */
     bool runPlanner_;
+    /** @brief Mutex for protecting planner thread data */
     boost::recursive_mutex planner_mutex_;
+    /** @brief Condition variable to wake planner thread */
     boost::condition_variable_any planner_cond_;
+    /** @brief Goal pose for the planner thread */
     robot_geometry_msgs::PoseStamped planner_goal_;
+    /** @brief Order message for the planner thread (nullptr if not using Order-based planning) */
+    boost::shared_ptr<robot_protocol_msgs::Order> planner_order_;
+    /** @brief Thread handle for the planner */
     boost::thread *planner_thread_;
 
+    /** @brief Mutex for protecting configuration changes */
     boost::recursive_mutex configuration_mutex_;
+    /** @brief Last configuration used */
     move_base::MoveBaseConfig last_config_;
+    /** @brief Default configuration */
     move_base::MoveBaseConfig default_config_;
 
-    bool setup_, p_freq_change_, c_freq_change_;
+    /** @brief Flag indicating if setup is complete */
+    bool setup_;
+    /** @brief Flag indicating planner frequency changed */
+    bool p_freq_change_;
+    /** @brief Flag indicating controller frequency changed */
+    bool c_freq_change_;
+    /** @brief Flag indicating a new global plan is available */
     bool new_global_plan_;
+    /** @brief Flag indicating cancel control request */
     bool cancel_ctr_;
-    bool pause_ctr_, paused_;
+    /** @brief Flag indicating pause control request */
+    bool pause_ctr_;
+    /** @brief Flag indicating if robot is currently paused */
+    bool paused_;
+    /** @brief Minimum linear velocity when approaching goal (m/s) */
     double min_approach_linear_velocity_{0.1};
-    robot_geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_;
-    robot_geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_;
-    double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
+    /** @brief Previous forward linear velocity command */
+    robot_geometry_msgs::Vector3 old_linear_fwd_;
+    /** @brief Previous backward linear velocity command */
+    robot_geometry_msgs::Vector3 old_linear_bwd_;
+    /** @brief Previous forward angular velocity command */
+    robot_geometry_msgs::Vector3 old_angular_fwd_;
+    /** @brief Previous reverse angular velocity command */
+    robot_geometry_msgs::Vector3 old_angular_rev_;
+    /** @brief Original XY goal tolerance value (meters) */
+    double original_xy_goal_tolerance_;
+    /** @brief Original yaw goal tolerance value (radians) */
+    double original_yaw_goal_tolerance_;
 
-    // defined planner name
+    /** @brief Name of the position/local planner plugin */
     std::string position_planner_name_{"MKTLocalPlanner"};
+    /** @brief Name of the docking planner plugin */
     std::string docking_planner_name_{"NAVDockingLocalPlanner"};
+    /** @brief Name of the go-straight planner plugin */
     std::string go_straight_planner_name_{"NAVGoStraightLocalPlanner"};
+    /** @brief Name of the rotate planner plugin */
     std::string rotate_planner_name_{"NAVRotateLocalPlanner"};
   
   };

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

@@ -721,6 +721,13 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
     robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
     robot::log_info("[MoveBase::moveTo] Goal stamp: %ld.%09ld", 
                     action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
+    
+    // Clear Order message since this is a non-Order moveTo call
+    {
+      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
+      planner_order_.reset();
+    }
+    
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
@@ -808,6 +815,14 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
   action_goal->goal_id.id = goal_id_stream.str();
   
   robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
+  
+  // Store Order message for use in planThread
+  {
+    boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
+    planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
+    robot::log_info("[MoveBase::moveTo] Stored Order message for planning");
+  }
+  
   as_->processGoal(action_goal);
 
   lock.unlock();
@@ -1209,7 +1224,7 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
   // since this gets called on handle activate
   if (!planner_costmap_robot_)
   {
-    std::cerr << "Planner costmap ROS is NULL, unable to create global plan" << std::endl;
+    robot::log_error("Planner costmap ROS is NULL, unable to create global plan");
     return false;
   }
 
@@ -1219,7 +1234,7 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
   robot_geometry_msgs::PoseStamped global_pose;
   if (!getRobotPose(global_pose, planner_costmap_robot_))
   {
-    std::cerr << "Unable to get starting pose of robot, unable to create global plan" << std::endl;
+    robot::log_error("Unable to get starting pose of robot, unable to create global plan");
     return false;
   }
 
@@ -1228,13 +1243,54 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
   // if the planner fails or returns a zero length plan, planning failed
   if (!planner_->makePlan(start, goal, plan) || plan.empty())
   {
-    std::cout << "Failed to find a  plan to point (" << goal.pose.position.x << ", " << goal.pose.position.y << ")" << std::endl;
+    robot::log_error("Failed to find a plan without Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
     return false;
   }
+  else
+  {
+    global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan);
+  }
   lock.unlock();
   return true;
 }
 
+bool move_base::MoveBase::makePlan(const robot_protocol_msgs::Order &msg, const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan)
+{
+  // make sure to set the plan to be empty initially
+  plan.clear();
+
+  // since this gets called on handle activate
+  if (!planner_costmap_robot_)
+  {
+    robot::log_error("Planner costmap ROS is NULL, unable to create global plan");
+    return false;
+  }
+
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
+
+  // get the starting pose of the robot
+  robot_geometry_msgs::PoseStamped global_pose;
+  if (!getRobotPose(global_pose, planner_costmap_robot_))
+  {
+    robot::log_error("Unable to get starting pose of robot, unable to create global plan");
+    return false;
+  }
+
+  const robot_geometry_msgs::PoseStamped &start = global_pose;
+
+  // if the planner fails or returns a zero length plan, planning failed
+  if (!planner_->makePlan(msg, start, goal, plan) || plan.empty())
+  {
+    robot::log_error("Failed to find a plan with Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y);
+    return false;
+  }
+  else
+  {
+    global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan);
+  }
+  lock.unlock();
+  return true;
+}
 bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
 {
   YAML::Node behavior_list = node.getParamValue("recovery_behaviors");
@@ -1506,7 +1562,6 @@ void move_base::MoveBase::wakePlanner(const robot::TimerEvent &event)
   planner_cond_.notify_one();
 }
 
-// void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal);
 void move_base::MoveBase::planThread()
 {
   robot::Timer timer;
@@ -1524,14 +1579,29 @@ void move_base::MoveBase::planThread()
     }
     robot::Time start_time = robot::Time::now();
 
-    // time to plan! get a copy of the goal and unlock the mutex
+    // time to plan! get a copy of the goal and Order message (if available) and unlock the mutex
     robot_geometry_msgs::PoseStamped temp_goal = planner_goal_;
+    boost::shared_ptr<robot_protocol_msgs::Order> temp_order = planner_order_;
     lock.unlock();
     std::cout << "Planning..." << std::endl;
     // run planner
     planner_plan_->clear();
     // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y);
-    bool gotPlan =  makePlan(temp_goal, *planner_plan_);
+    
+    // Choose makePlan based on whether Order message is available
+    bool gotPlan = false;
+    if (temp_order)
+    {
+      // Use Order-based makePlan
+      robot::log_info("[MoveBase::planThread] Using Order-based planning");
+      gotPlan = makePlan(*temp_order, temp_goal, *planner_plan_);
+    }
+    else
+    {
+      // Use standard makePlan without Order
+      robot::log_info("[MoveBase::planThread] Using standard planning (no Order)");
+      gotPlan = makePlan(temp_goal, *planner_plan_);
+    }
 
     if (gotPlan)
     {
@@ -1602,8 +1672,6 @@ void move_base::MoveBase::planThread()
   }
 }
 
-
-
 void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal)
 {
   if (!isQuaternionValid(move_base_goal->target_pose.pose.orientation))
@@ -1691,6 +1759,8 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
         // we have a new goal so make sure the planner is awake
         lock.lock();
         planner_goal_ = goal;
+        // Clear Order message for new goal (will be set if moveTo with Order is called)
+        planner_order_.reset();
         runPlanner_ = true;
         cancel_ctr_ = false;
         planner_cond_.notify_one();
@@ -1736,6 +1806,8 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
       // we have a new goal so make sure the planner is awake
       lock.lock();
       planner_goal_ = goal;
+      // Clear Order message for new goal (will be set if moveTo with Order is called)
+      planner_order_.reset();
       runPlanner_ = true;
       cancel_ctr_ = false;
       planner_cond_.notify_one();
@@ -1822,6 +1894,9 @@ void move_base::MoveBase::setupActionServerCallbacks()
     [this](const robot_actionlib_msgs::GoalIDConstPtr& cancel)
     {
       robot::log_info("[MoveBase] Cancel callback: id=%s", cancel->id.c_str());
+      // Clear Order message for new goal (will be set if moveTo with Order is called)
+      boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
+      planner_order_.reset();
     }
   );
 
@@ -2069,7 +2144,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
   // if we're controlling, we'll attempt to find valid velocity commands
   case move_base::CONTROLLING:
-    robot::log_debug("In controlling state.");
+    // robot::log_debug("In controlling state.");
 
     // check to see if we've reached our goal
     try
@@ -2131,12 +2206,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       {
         if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
         {
-          robot::log_debug("Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
-                          cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
-
           last_valid_control_ = robot::Time::now();
           // make sure that we send the velocity command to the base
-          // vel_pub_.publish(cmd_vel);
 
           if (nav_feedback_)
           {
@@ -2149,8 +2220,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
         }
         else
         {
-          robot::log_debug("The local planner could not find a valid plan.");
-          robot::log_warning("The local planner could not find a valid plan.");
+          robot::log_warning_at(__FILE__, __LINE__, "The local planner could not find a valid plan.");
           robot::Time attempt_end = last_valid_control_ + robot::Duration(controller_patience_);
 
           // check if we've tried to find a valid control for longer than our time limit
@@ -2179,34 +2249,34 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
             // enable the planner thread in case it isn't running on a clock
             boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
-            // if (as_->isPreemptRequested())
-            // {
-            //   if (as_->isNewGoalAvailable())
-            //   {
-            //     // if we're active and a new goal is available, we'll accept it, but we won't shut anything down
-            //     move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
-            //     if (!isQuaternionValid(new_goal.target_pose.pose.orientation))
-            //     {
-            //       if (nav_feedback_)
-            //       {
-            //         nav_feedback_->navigation_state = move_base_core::State::ABORTED;
-            //         nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion");
-            //       }
-            //       as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
-            //       return false;
-            //     }
-            //     else
-            //     {
-            //       if (nav_feedback_)
-            //       {
-            //         nav_feedback_->navigation_state = move_base_core::State::ACTIVE;
-            //         nav_feedback_->feed_back_str = std::string("The new goal is received");
-            //       }
-            //     }
-            //     goal = goalToGlobalFrame(new_goal.target_pose);
-            //     planner_goal_ = goal;
-            //   }
-            // }
+            if (as_->isPreemptRequested())
+            {
+              if (as_->isNewGoalAvailable())
+              {
+                // if we're active and a new goal is available, we'll accept it, but we won't shut anything down
+                robot_move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
+                if (!isQuaternionValid(new_goal.target_pose.pose.orientation))
+                {
+                  if (nav_feedback_)
+                  {
+                    nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
+                    nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion");
+                  }
+                  as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
+                  return false;
+                }
+                else
+                {
+                  if (nav_feedback_)
+                  {
+                    nav_feedback_->navigation_state = robot::move_base_core::State::ACTIVE;
+                    nav_feedback_->feed_back_str = std::string("The new goal is received");
+                  }
+                }
+                goal = goalToGlobalFrame(new_goal.target_pose);
+                planner_goal_ = goal;
+              }
+            }
             runPlanner_ = true;
             cancel_ctr_ = false;
             planner_cond_.notify_one();
@@ -2251,14 +2321,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     if (recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())
     {
       robot::log_debug("[%s:%d] Executing behavior %u of %zu", __FILE__, __LINE__, recovery_index_ + 1, recovery_behaviors_.size());
-
-      // move_base_msgs::RecoveryStatus msg;
-      // msg.pose_stamped = current_position;
-      // msg.current_recovery_number = recovery_index_;
-      // msg.total_number_of_recoveries = recovery_behaviors_.size();
-      // msg.recovery_behavior_name = recovery_behavior_names_[recovery_index_];
-      // recovery_status_pub_.publish(msg);
-
       recovery_behaviors_[recovery_index_]->runBehavior();
 
       // we at least want to give the robot some time to stop oscillating after executing the behavior
@@ -2296,7 +2358,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
           nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
           nav_feedback_->feed_back_str = std::string("Failed to find a valid control. Even after executing recovery behaviors.");
         }
-        // as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
+        as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
       }
       else if (recovery_trigger_ == PLANNING_R)
       {
@@ -2306,7 +2368,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
           nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
           nav_feedback_->feed_back_str = "Failed to find a valid plan. Even after executing recovery behaviors.";
         }
-        // as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
+        as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
       }
       else if (recovery_trigger_ == OSCILLATION_R)
       {
@@ -2316,7 +2378,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
           nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;
           nav_feedback_->feed_back_str = std::string("Robot is oscillating. Even after executing recovery behaviors.");
         }
-        // as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
+        as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
       }
       return true;
     }
@@ -2333,7 +2395,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED;  
       nav_feedback_->feed_back_str = std::string("Reached a case that should not be hit in move_base. This is a bug, please report it.");
     }
-    // as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
+    as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
     return true;
   }