ROS Move Base恢复机制

1、什么情况下启动恢复机制

movebase的三种状态：

enum MoveBaseState {
  PLANNING,//在规划路径中
  CONTROLLING,//在控制机器人运动中
  CLEARING//规划或者控制失败在恢复或者清除中。
};

一般默认状态或者接收到一个有效goal时是PLANNING，在规划出全局路径后state_会由PLANNING->CONTROLLING，如果规划失败则由PLANNING->CLEARING。在MoveBase::executeCycle中，会分别对这三种状态做处理：

还在PLANNING中则唤醒规划线程让它干活
如果已经在CONTROLLING中，判断是否已经到达目的地，否则判断是否出现震动？否则调用局部路径规划，如果成功得到速度则直接发布到cmd_vel，失败则判断是否控制超时，不超时的话让全局再规划一个路径。
如果出现了问题需要CLEARING（仅有全局规划失败、局部规划失败、长时间困在一片小区域三种原因），则每次尝试一种recovery方法，直到所有尝试完
movebase为recovery行为定义了如下三种原因

enum RecoveryTrigger
{
  PLANNING_R,//全局规划失败
  CONTROLLING_R,//局部规划失败
  OSCILLATION_R//长时间困在一片小区域
};

以下三种情况，开始clearing

（1）

        //check for an oscillation condition

        if(oscillation_timeout_ > 0.0 &&

            last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())

        {

          publishZeroVelocity();

          state_ = CLEARING;

          recovery_trigger_ = OSCILLATION_R;

        }

（2）

//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;

          runPlanner_ = false;  // proper solution for issue #523

          publishZeroVelocity();

          recovery_trigger_ = PLANNING_R;

        }

（3）

          //check if we've tried to find a valid control for longer than our time limit

          if(ros::Time::now() > attempt_end){

            //we'll move into our obstacle clearing mode

            publishZeroVelocity();

            state_ = CLEARING;

            recovery_trigger_ = CONTROLLING_R;

          }

2、恢复机制有哪些？

三种终止方式

if(recovery_trigger_ == CONTROLLING_R){

            ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");

            as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");

          }

          else if(recovery_trigger_ == PLANNING_R){

            ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");

            as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");

          }

          else if(recovery_trigger_ == OSCILLATION_R){

            ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");

            as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");

          }

3、如果通过调整参数调整恢复机制

震荡时间与距离

    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);

    private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

路径规划等待时间

private_nh.param("planner_patience", planner_patience_, 5.0);