update

src/Libraries/robot_cpp

@@ -1 +1 @@
-Subproject commit 5b23baae3ace0f5acd7dc7e9cd346ffc75210630
+Subproject commit 0da1b1b262323d0fb45208dc9edf1fab6fe15c8d

src/Navigations/Cores/nav_core/CMakeLists.txt

@@ -19,7 +19,7 @@ include_directories(
 
 # Tạo INTERFACE library (header-only)
 add_library(nav_core INTERFACE)
-target_link_libraries(nav_core INTERFACE costmap_2d tf3)
+target_link_libraries(nav_core INTERFACE costmap_2d tf3 robot_protocol_msgs)
 
 # Set include directories
 target_include_directories(nav_core

src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h

@@ -39,6 +39,7 @@
 
 #include <geometry_msgs/PoseStamped.h>
 #include <costmap_2d/costmap_2d_robot.h>
+#include <robot_protocol_msgs/Order.h>
 #include <memory>
 
 namespace nav_core {
@@ -73,9 +74,24 @@ namespace nav_core {
                             double& cost)
       {
         cost = 0;
-        return makePlan(start, goal, plan);
+        return false;
       }
 
+      /**
+       * @brief Given a goal pose in the world, compute a plan
+       * @param start The start pose 
+       * @param goal The goal pose 
+       * @param plan The plan... filled by the planner
+       * @return True if a valid plan was found, false otherwise
+       */
+       virtual bool makePlan(const robot_protocol_msgs::Order& msg,
+        const geometry_msgs::PoseStamped& start, 
+        const geometry_msgs::PoseStamped& goal, 
+        std::vector<geometry_msgs::PoseStamped>& plan)
+        {
+          return false;
+        }
+
       /**
        * @brief  Initialization function for the BaseGlobalPlanner
        * @param  name The name of this planner

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

@@ -156,23 +156,40 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
     // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
     //  From config param
     double inscribed_radius;
+    robot::log_info("[%s:%d] inscribed_radius: %f", __FILE__, __LINE__, inscribed_radius);
     private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
     double circumscribed_radius; 
     private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
+    robot::log_info("[%s:%d] circumscribed_radius: %f", __FILE__, __LINE__, circumscribed_radius);
     inscribed_radius_ = inscribed_radius;
     circumscribed_radius_ = circumscribed_radius;
     private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
+    robot::log_info("[%s:%d] clearing_radius: %f", __FILE__, __LINE__, clearing_radius_);
     double conservative_reset_dist;
     private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
+    robot::log_info("[%s:%d] conservative_reset_dist: %f", __FILE__, __LINE__, conservative_reset_dist);
     conservative_reset_dist_ = conservative_reset_dist;
+    robot::log_info("[%s:%d] shutdown_costmaps: %f", __FILE__, __LINE__, shutdown_costmaps_);
     private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
+    robot::log_info("[%s:%d] clearing_rotation_allowed: %f", __FILE__, __LINE__, clearing_rotation_allowed_);
     private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
+    robot::log_info("[%s:%d] recovery_behavior_enabled: %f", __FILE__, __LINE__, recovery_behavior_enabled_);
     private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
     
     // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
-    planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
-    planner_costmap_robot_->pause();
-
+    robot::log_info("[%s:%d] create the ros wrapper for the planner's costmap...");
+    try
+    {
+      planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
+      robot::log_info("[%s:%d] planner_costmap_robot_->pause()");
+      planner_costmap_robot_->pause();
+      robot::log_info("[%s:%d] planner_costmap_robot_->pause()");
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
+      throw std::runtime_error("Failed to create the planner_costmap_robot_");
+    }
     // initialize the global planner
     try
     {