update

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();
   }