ROS自带的恢复行为

原理图

所有的recovery behaviors都必须实现nav_core::RecoveryBehavior定义的接口(纯虚函数)。目前有类ClearCostmapRecovery, MoveSlowAndClear, RotateRecovery， RotateRecovery一般不用

每个恢复行为类主要实现两个函数，一个负责初始化，另一个负责执行恢复行为。

1 2	virtual void initialize() virtual void runBehavior() = 0;

参数

恢复行为在move_base_params.yaml中定义，如果不定义，会使用默认的配置。MoveBase构造函数中的使用：

1 2	if(!loadRecoveryBehaviors(private_nh)) loadDefaultRecoveryBehaviors();

如果加载自己的恢复行为不成功，就加载系统默认的。如果用默认参数，则是按顺序加载了①conservative_clear, ②rotate, ③aggressive_clear, ④rotate. ①/③都是ClearCostmapRecovery类，负责清理机器人一定范围外的costmap上的数据，区别在于③保留的范围更小，清理的更多。恢复行为是按照列表顺序调用的，当①②无效后，才会执行③，清理更多区域。默认配置中，aggressive_reset清除的范围是4 * local_costmap/circumscribed_radius以外

我的配置是这样的：

recovery_behaviors:
  - name: 'conservative_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'aggressive_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
conservative_reset:
  reset_distance: 3.0
  layer_names: 
     - obstacles
    
aggressive_reset:
  reset_distance: 0.0
  layer_names:
     - obstacles

函数中的变量recovery_behaviors_插入的就是conservative_reset和aggressive_reset。

两个恢复行为都是ClearCostmapRecovery类，相关参数:

reset_distance (double, default: 3.0)指定距离机器人半径几米的区域之外的障碍在恢复代价地图为静态地图时候将被清除。
layer_names (std::vector, default:”obstacles”)哪一层的信息将会被清除

还有两个重要参数：

oscillation_distance (double, default: 0.5) 来回震荡的距离，超过一定的时间没有移动足够的距离就会进行recovery behavior。移动达到了这个距离，就会重置oscillation_timeout的计时器
oscillation_timeout (double, default: 0.0) 在执行恢复行为前允许的振荡时间，0代表无穷的时间

我的小车不允许有一些奇怪的行为，比如振荡。所以不用rotate recovery，oscillation_distance设置为0.05也是为了避免振荡

状态机

Movebase有三个状态：

PLANNING：全局规划中，尚未得到全局路径
CONTROLLING：全局规划完成，进入局部规划
CLEARING：恢复行为

当 ①全局规划失败 ②局部规划失败 ③机器人震荡时会进入到恢复行为。

三种情况对应的恢复行为起因

enum RecoveryTrigger
{
  PLANNING_R,      // 全局规划失败，即无法算出 plan
  CONTROLLING_R,  // 局部规划失败，从而无法得到速度命令
  OSCILLATION_R   // 机器人在不断抖动，长时间被困在一小片区域
};

日志：

move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
Clearing costmap to unstuck robot (3.000000m).

move_base_recovery,state machine is CLEARING, Executing behavior 1 of 2
Clearing costmap to unstuck robot (0.000000m).

Aborting because a valid plan could not be found. Even after executing all recovery behaviors

RotateRecovery::runBehavior()

只是在原地逆时针旋转180度，这样costmap就会自己更新，mark或者clear一点的值，有没有路是局部代价地图在转一圈过程中发现的。

这里的目标是转过180度，将当前角度与目标180的偏差记录在distleft中，并通过这个偏差产生角速度的控制量，具体为 `vel = sqrt(2 * acc_lim_th * distleft)` ，并且要确保这个速度在角速度阈值之内，发布这个角速度，并循环检测偏差是否满足tolerance范围，当符合时退出。

按默认情况，它先清理周围一定范围以外的costmap（障碍层），接下来重新执行规划，若不奏效，则旋转180度，再执行规划。交替两次后，已转过360度，若还是没能排除故障，则恢复行为失败，关闭Movebase规划器。

ClearCostmapRecovery::runBehavior()

它将距离机器人一定距离的正方形区域外的代价地图重置，理解不了看看参数reset_distance就明白了。 ClearCostmapRecovery自身不会让机器人运动

void ClearCostmapRecovery::runBehavior(){
  // 省略：判断是否初始化，两个代价地图是否为空，否则return
  // reset_distance_ 就是定义的参数
  clear_costmap_log.warn("Clearing costmap to unstuck robot (%fm).", reset_distance_);
  clear(global_costmap_);
  clear(local_costmap_);
}

void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap)
{
  std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();

  tf::Stamped<tf::Pose> pose;
  // 在代价地图的global坐标系中的坐标 x  y
  if(!costmap->getRobotPose(pose)){
    ROS_ERROR("Cannot clear map because pose cannot be retrieved");
    return;
  }
  double x = pose.getOrigin().x();
  double y = pose.getOrigin().y();
  // 遍历代价地图的每一层，转为costmap_2d::Layer，获取名称
  for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp)
  {
    boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
    std::string name = plugin->getName();
    int slash = name.rfind('/');
    if( slash != std::string::npos ){
        name = name.substr(slash+1);
  }
    if(clearable_layers_.count(name)!=0){
      boost::shared_ptr<costmap_2d::CostmapLayer> costmap;
      costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);
      clearMap(costmap, x, y);
    }
  }
}

void ClearCostmapRecovery::clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap, 
                                    double pose_x, double pose_y)
{
  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
 
  double start_point_x = pose_x - reset_distance_ / 2;
  double start_point_y = pose_y - reset_distance_ / 2;
  double end_point_x = start_point_x + reset_distance_;
  double end_point_y = start_point_y + reset_distance_;

  int start_x, start_y, end_x, end_y;
  costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
  costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
  // 获取字符格式的代价地图
  unsigned char* grid = costmap->getCharMap();
  for(int x=0; x<(int)costmap->getSizeInCellsX(); x++)
  {
    bool xrange = x>start_x && x<end_x;   
    for(int y=0; y<(int)costmap->getSizeInCellsY(); y++)
    {
    	// 注意是不处理 reset_distance_正方形范围内的代价值
      if(xrange && y>start_y && y<end_y)
        continue;
      int index = costmap->getIndex(x,y);
      // 范围外的代价值重置为 NO_INFORMATION
      if(grid[index]!=NO_INFORMATION){
        grid[index] = NO_INFORMATION;
      }
    }
  }
  double ox = costmap->getOriginX(), oy = costmap->getOriginY();
  double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();

  costmap->addExtraBounds(ox, oy, ox + width, oy + height);
  return;
}

MoveSlowAndClear

这个使用较少。它实现了缓慢移动的恢复行为，清理 costmap后什么都不管，按照前进速度和转角速度走。从代码里可以看到，根据指定的距离，这是通过先清除全局 costmap 跟局部 costmap 一圈的 obstacle layer 的障碍，然后直接发指令实现的。由于只清除了 obstacle layer ，其实 static layer 的障碍还在，而且这里清不清除跟发指令关系不大，该撞上去的还是会撞的，相当于闭着眼睛往前走。

禁止所有恢复行为造成的问题

不如自己不定义，还会使用默认的参数，所以将下面代码去掉:

1 2	if(!loadRecoveryBehaviors(private_nh)) loadDefaultRecoveryBehaviors();

结果发现平时正常导航的情况居然出错了

found_legal is 0
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan 
[ERROR] 17:51:42.074 (): Aborting because a valid plan could not be found. Even after executing all recovery behaviors

并不是每次的导航都会出错，这要看全局路径，全局路径部分的bool found_legal = planner_->calculatePotentials有时会返回0，也就是找不到全局路径，此时会触发恢复行为。所以如果全禁止，会直接 Abort

正常的日志是这样的:

Received move_base goal (x: 62.766, y: -1.474, yaw deg: -5, yaw rad: -0.09)
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan 
[INFO]  move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
[ WARN] Clearing costmap to unstuck robot (0.000000m).
[ERROR] Failed to get a plan.
[ INFO] Robot in controlling, but can't get new global plan 
[ INFO]  move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
[ WARN] Clearing costmap to unstuck robot (0.000000m).
[ INFO] 到达目标

也就是获取不到全局路径时，会很快执行一次恢复行为，clear costmap，然后又找到了路径，这个过程之快，几乎看不到。

参考：导航recovery机制