update

src/APIs/c_api/CMakeLists.txt

@@ -13,9 +13,10 @@ 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

src/APIs/c_api/include/nav_c_api.h

@@ -218,12 +218,10 @@ extern "C"
   /**
    * @brief Move straight toward the target position
    * @param handle Navigation handle
-   * @param goal Target pose
-   * @param xy_goal_tolerance Acceptable positional error (meters)
+   * @param distance Distance to move (meters)
    * @return true if command issued successfully
    */
-  bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped goal,
-                                   double xy_goal_tolerance);
+  bool navigation_move_straight_to(NavigationHandle handle, const double distance);
 
   /**
    * @brief Rotate in place to align with target orientation

src/APIs/c_api/src/nav_c_api.cpp

@@ -354,8 +354,7 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker,
 //         return false;
 //     }
 
-extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped goal,
-                                            double xy_goal_tolerance)
+extern "C" bool navigation_move_straight_to(NavigationHandle handle, const double distance)
 {
   if (!handle.ptr)
   {
@@ -368,8 +367,11 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS
     if (!nav_ptr || !*nav_ptr)
       return false;
     auto *nav = nav_ptr->get();
-    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
-    return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
+    robot_geometry_msgs::PoseStamped pose;
+    if (!nav->getRobotPose(pose))
+      return false;
+    robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance);
+    return nav->moveStraightTo(goal);
   }
   catch (...)
   {

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

@@ -9,8 +9,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;

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

@@ -35,6 +35,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();

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/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;
 

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

@@ -32,7 +32,6 @@
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_nav_2d_utils/conversions.h>
 
-
 move_base::MoveBase::MoveBase()
     : initialized_(false),
       planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
@@ -54,6 +53,7 @@ move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
 
 move_base::MoveBase::~MoveBase()
 {
+  robot::log_warning("Destroying MoveBase instance...");
   if (as_ != NULL)
   { 
     as_->stop();
@@ -124,8 +124,16 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
   printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
   if (!initialized_)
   {
+    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__);
+    }
     tf_ = tf;
-
     as_ = new MoveBaseActionServer("move_base", std::bind(&MoveBase::executeCb, this, std::placeholders::_1), true);
     setupActionServerCallbacks();
 
@@ -250,6 +258,11 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     try
     {
       controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
+      if(controller_costmap_robot_ == nullptr)
+      {
+        robot::log_error("[%s:%d]\n ERROR: controller_costmap_robot_ is nullptr", __FILE__, __LINE__);
+        throw std::runtime_error("Failed to create the controller_costmap_robot_");
+      }
       controller_costmap_robot_->pause();
       robot_costmap_2d::LayeredCostmap *layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
     }
@@ -1128,6 +1141,12 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
     }
     
     robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
+    if(controller_costmap_robot_ == nullptr)
+    {
+      robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ is null!");
+      return false;
+    }
+
     as_->processGoal(action_goal);
     robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server");
   }
@@ -1771,7 +1790,7 @@ void move_base::MoveBase::resetState()
   // if we shutdown our costmaps when we're deactivated... we'll do that now
   if (shutdown_costmaps_)
   {
-    std::cout << "Stopping costmaps" << std::endl;
+    robot::log_info("Stopping costmaps");
     planner_costmap_robot_->stop();
     controller_costmap_robot_->stop();
   }