From c98f2cb0eb8a351eeb391a8df6235a9cb5e0dac5 Mon Sep 17 00:00:00 2001 From: Ryan Date: Fri, 19 Nov 2021 16:50:40 -0800 Subject: [PATCH 1/2] Added publishZeroVelocity() when state_ is changed to PLANNING. --- move_base/src/move_base.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp index 0141fb1876..61854f1444 100644 --- a/move_base/src/move_base.cpp +++ b/move_base/src/move_base.cpp @@ -706,6 +706,7 @@ namespace move_base { //we'll make sure that we reset our state for the next execution cycle recovery_index_ = 0; state_ = PLANNING; + publishZeroVelocity(); //we have a new goal so make sure the planner is awake lock.lock(); @@ -744,6 +745,7 @@ namespace move_base { //we want to go back to the planning state for the next execution cycle recovery_index_ = 0; state_ = PLANNING; + publishZeroVelocity(); //we have a new goal so make sure the planner is awake lock.lock(); From 6d3950c0888d795872049931a6b569ff102e1096 Mon Sep 17 00:00:00 2001 From: Ryan Date: Mon, 20 Jun 2022 17:19:43 -0700 Subject: [PATCH 2/2] Added parameter stop_before_planning --- move_base/cfg/MoveBase.cfg | 2 ++ move_base/include/move_base/move_base.h | 1 + move_base/src/move_base.cpp | 12 ++++++++++-- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/move_base/cfg/MoveBase.cfg b/move_base/cfg/MoveBase.cfg index 9eef29ba7e..3db443ce87 100755 --- a/move_base/cfg/MoveBase.cfg +++ b/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")) diff --git a/move_base/include/move_base/move_base.h b/move_base/include/move_base/move_base.h index f1bedc2d73..ee9f47821b 100644 --- a/move_base/include/move_base/move_base.h +++ b/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_; diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp index 61854f1444..464a4fe42a 100644 --- a/move_base/src/move_base.cpp +++ b/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,7 +710,9 @@ namespace move_base { //we'll make sure that we reset our state for the next execution cycle recovery_index_ = 0; state_ = PLANNING; - publishZeroVelocity(); + if(stop_before_planning_){ + publishZeroVelocity(); + } //we have a new goal so make sure the planner is awake lock.lock(); @@ -745,7 +751,9 @@ namespace move_base { //we want to go back to the planning state for the next execution cycle recovery_index_ = 0; state_ = PLANNING; - publishZeroVelocity(); + if(stop_before_planning_){ + publishZeroVelocity(); + } //we have a new goal so make sure the planner is awake lock.lock();