Skip to content

Commit

Permalink
Add a max_planning_retries parameter to move_base [kinetic] (ros-plan…
Browse files Browse the repository at this point in the history 
…ning#539)

* Implements issue ros-planning#496: add a max_planning_retries parameter as an alternative to planner_patience to limit the failed calls to global planner
* Includes ros-planning#498 and ros-planning#524 for kinetic and above
  • Loading branch information
@corot @gerkey
corot authored and gerkey committed Jan 19, 2018
1 parent d04ad96 commit 2a13b9e
Show file tree
Hide file tree
Showing 3 changed files with 63 additions and 38 deletions.
3 changes: 2 additions & 1 deletion move_base/cfg/MoveBase.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

PACKAGE = 'move_base'

from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t

gen = ParameterGenerator()

Expand All @@ -15,6 +15,7 @@ gen.add("planner_frequency", double_t, 0, "The rate in Hz at which to run the pl
gen.add("controller_frequency", double_t, 0, "The rate in Hz at which to run the control loop and send velocity commands to the base.", 20, 0, 100)
gen.add("planner_patience", double_t, 0, "How long the planner will wait in seconds in an attempt to find a valid plan before space-clearing operations are performed.", 5.0, 0, 100)
gen.add("controller_patience", double_t, 0, "How long the controller will wait in seconds without receiving a valid control before space-clearing operations are performed.", 5.0, 0, 100)
gen.add("max_planning_retries", int_t, 0, "How many times we will recall the planner in an attempt to find a valid plan before space-clearing operations are performed", -1, -1, 1000)
gen.add("conservative_reset_dist", double_t, 0, "The distance away from the robot in meters at which obstacles will be cleared from the costmap when attempting to clear space in the map.", 3, 0, 50)

gen.add("recovery_behavior_enabled", bool_t, 0, "Whether or not to enable the move_base recovery behaviors to attempt to clear out space.", True)
Expand Down
2 changes: 2 additions & 0 deletions move_base/include/move_base/move_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,8 @@ namespace move_base {
tf::Stamped<tf::Pose> global_pose_;
double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
double planner_patience_, controller_patience_;
int32_t max_planning_retries_;
uint32_t planning_retries_;
double conservative_reset_dist_, clearing_radius_;
ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_;
ros::Subscriber goal_sub_;
Expand Down
96 changes: 59 additions & 37 deletions move_base/src/move_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ namespace move_base {
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default

private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
Expand Down Expand Up @@ -203,6 +204,7 @@ namespace move_base {

planner_patience_ = config.planner_patience;
controller_patience_ = config.controller_patience;
max_planning_retries_ = config.max_planning_retries;
conservative_reset_dist_ = config.conservative_reset_dist;

recovery_behavior_enabled_ = config.recovery_behavior_enabled;
Expand Down Expand Up @@ -560,48 +562,62 @@ namespace move_base {
}
ros::Time start_time = ros::Time::now();

//time to plan! get a copy of the goal and unlock the mutex
geometry_msgs::PoseStamped temp_goal = planner_goal_;
lock.unlock();
ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
if(state_==PLANNING){
//time to plan! get a copy of the goal and unlock the mutex
geometry_msgs::PoseStamped temp_goal = planner_goal_;
lock.unlock();
ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");

//run planner
planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
//run planner
planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

if(gotPlan){
ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
//pointer swap the plans under mutex (the controller will pull from latest_plan_)
std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;
if(gotPlan){
ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
//pointer swap the plans under mutex (the controller will pull from latest_plan_)
std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;

lock.lock();
planner_plan_ = latest_plan_;
latest_plan_ = temp_plan;
last_valid_plan_ = ros::Time::now();
new_global_plan_ = true;
lock.lock();
planner_plan_ = latest_plan_;
latest_plan_ = temp_plan;
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
new_global_plan_ = true;

ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");

//make sure we only start the controller if we still haven't reached the goal
if(runPlanner_)
state_ = CONTROLLING;
if(planner_frequency_ <= 0)
runPlanner_ = false;
lock.unlock();
}
//if we didn't get a plan and we are in the planning state (the robot isn't moving)
else if(state_==PLANNING){
ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
//make sure we only start the controller if we still haven't reached the goal
if(runPlanner_)
state_ = CONTROLLING;
if(planner_frequency_ <= 0)
runPlanner_ = false;
lock.unlock();
}
//if we didn't get a plan and we are in the planning state (the robot isn't moving)
else{
ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);

//check if we've tried to make a plan for over our time limit
lock.lock();
if(ros::Time::now() > attempt_end && runPlanner_){
//we'll move into our obstacle clearing mode
state_ = CLEARING;
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
//check if we've tried to make a plan for over our time limit or our maximum number of retries
//issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
//is negative (the default), it is just ignored and we have the same behavior as ever
lock.lock();
planning_retries_++;
if(runPlanner_ &&
(ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
//we'll move into our obstacle clearing mode
state_ = CLEARING;
wait_for_wake = true;
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
}

lock.unlock();
}
}
else
{
//not planning, so just unlock the mutex
lock.unlock();
}

Expand Down Expand Up @@ -649,6 +665,7 @@ namespace move_base {
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;

ros::NodeHandle n;
while(n.ok())
Expand Down Expand Up @@ -687,10 +704,11 @@ namespace move_base {
ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
current_goal_pub_.publish(goal);

//make sure to reset our timeouts
//make sure to reset our timeouts and counters
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;
}
else {
//if we've been preempted explicitly we need to shut things down
Expand Down Expand Up @@ -724,10 +742,11 @@ namespace move_base {
ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
current_goal_pub_.publish(goal);

//make sure to reset our timeouts
//make sure to reset our timeouts and counters
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
planning_retries_ = 0;
}

//for timing that gives real time even in simulation
Expand Down Expand Up @@ -901,6 +920,7 @@ namespace move_base {
else{
//otherwise, if we can't find a valid control, we'll go back to planning
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
state_ = PLANNING;
publishZeroVelocity();

Expand Down Expand Up @@ -928,6 +948,8 @@ namespace move_base {

//we'll check if the recovery behavior actually worked
ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
state_ = PLANNING;

//update the index of the next recovery behavior that we'll try
Expand Down

0 comments on commit 2a13b9e

Please sign in to comment.