Added publishZeroVelocity() when state_ is changed to PLANNING.

move_base/cfg/MoveBase.cfg

@@ -29,5 +29,7 @@ gen.add("oscillation_distance", double_t, 0, "How far in meters the robot must m
 gen.add("make_plan_clear_costmap", bool_t, 0, "Whether or not to clear the global costmap on make_plan service call.", True)
 gen.add("make_plan_add_unreachable_goal", bool_t, 0, "Whether or not to add the original goal to the path if it is unreachable in the make_plan service call.", True)
 
+gen.add("stop_before_planning", bool_t, 0, "Whether or not to publish a zero velocity command before planning.", False)
+
 gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False)
 exit(gen.generate(PACKAGE, "move_base_node", "MoveBase"))

move_base/include/move_base/move_base.h

@@ -200,6 +200,7 @@ namespace move_base {
       ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
       bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
       bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
+      bool stop_before_planning_;
       double oscillation_timeout_, oscillation_distance_;
 
       MoveBaseState state_;

move_base/src/move_base.cpp

@@ -80,6 +80,8 @@ namespace move_base {
     private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
     private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
 
+    private_nh.param("stop_before_planning", stop_before_planning_, false);
+
     // parameters of make_plan service
     private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
     private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);
@@ -269,6 +271,8 @@ namespace move_base {
     make_plan_clear_costmap_ = config.make_plan_clear_costmap;
     make_plan_add_unreachable_goal_ = config.make_plan_add_unreachable_goal;
 
+    stop_before_planning_ = config.stop_before_planning;
+
     last_config_ = config;
   }
 
@@ -706,6 +710,9 @@ namespace move_base {
           //we'll make sure that we reset our state for the next execution cycle
           recovery_index_ = 0;
           state_ = PLANNING;
+          if(stop_before_planning_){
+            publishZeroVelocity();
+          }
 
           //we have a new goal so make sure the planner is awake
           lock.lock();
@@ -744,6 +751,9 @@ namespace move_base {
         //we want to go back to the planning state for the next execution cycle
         recovery_index_ = 0;
         state_ = PLANNING;
+        if(stop_before_planning_){
+          publishZeroVelocity();
+        }
 
         //we have a new goal so make sure the planner is awake
         lock.lock();