ROS-Navigation Move_base 源码阅读学习--恢复行为recovery_behavior(旋转恢复行为、代价地图清理恢复行为) - 教程

在ROS的move_base导航栈中，恢复行为是确保系统鲁棒性的关键组件。它定义了一组当机器人无法正常前进时所执行的策略，旨在使机器人状态恢复到可继续执行全局规划路径的水平。

根据导航状态机的设计，在PLANNING状态失败后，系统会依次执行用户定义的恢复行为。move_base架构中的恢复行为类都是通过插件plugind的方式动态加载，派生类中主要对基类RecoveryBehavior中的initialize和runBehavior两个函数进行重载，已实现自己定义的恢复行为。
其中，move_base中定义了两个最为核心和常用的恢复行为：clear_costmap_recovery和rotate_recovery

• clear_costmap_recovery：通过清除局部/全局代价地图中指定层（如障碍物层）的信息，来解决因动态障碍物遗留、传感器噪点或代价地图膨胀区过大导致的“虚假堵塞”问题。

• rotate_recovery：一种简单却极其有效的策略，通过控制机器人在原地旋转360度，利用其传感器重新感知周围环境，从而可能发现之前被遮挡的自由空间。

本篇博客将深入这两个恢复行为的实现原理。

旋转恢复行为 RotateRecovery

RotateRecovery的核心思路是：

1. 让机器人原地 旋转一整圈（360°）。
2. 这样它的传感器（激光雷达、深度相机等）可以扫描周围环境，刷新局部 costmap。
3. 如果之前 costmap 上残留了错误的障碍物（比如动态障碍物已经走开），这个旋转动作会帮助清理。

核心代码：

double dist_left;
if (!got_180)  // 转到180°
{
  // 先强制让机器人至少旋转半圈(起点朝向 + 180°)
  // distance_to_180表示还差多少角度到180°
  double distance_to_180 = std::fabs(angles::shortest_angular_distance(current_angle, start_angle + M_PI));
  // 再继续旋转直到回到起点
  dist_left = M_PI + distance_to_180;
  if (distance_to_180 < tolerance_)
  {
    got_180 = true;
  }
}
else  // 回到起点角度
{
  // 此时已经旋转半圈，计算当前角度与起点角度的差值
  dist_left = std::fabs(angles::shortest_angular_distance(current_angle, start_angle));
}
double x = global_pose.pose.position.x, y = global_pose.pose.position.y;
double sim_angle = 0.0;
while (sim_angle < dist_left)
{
  double theta = current_angle + sim_angle;
  // 每隔sim_granularity_ 检查一次 footprint 是否会碰撞
  double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
  if (footprint_cost < 0.0)
  {
    ROS_ERROR("Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f",
              footprint_cost);
    return;  // 发生碰撞，直接退出
  }
  sim_angle += sim_granularity_;
}
// 计算旋转速度，确保能在剩余角度内停下
double vel = sqrt(2 * acc_lim_th_ * dist_left);
// 限制在最小、最大旋转速度范围内
vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);

这里仔细思考，为什么要分成先假装转180，然后再回到起点角度？如果单纯判断"还没有转360°"，会出现一个问题：机器人转过一半的时候，当前角度和起点角度刚好相差 180°，这时候它其实有两个候选方向能继续往下走（顺时针 / 逆时针），可能会引发歧义。
所以代码里采用了两阶段的一个技巧，使得机器人可以保证确实转了一圈：

1. 阶段一：先强制机器人至少转到起点角度+180°
2. 阶段二：再继续旋转直到回到起点角度

对于为什么旋转恢复行为是转一圈，而不是转半圈或者其他圈数呢？
这是因为一整圈的扫描可以让局部代价地图刷新，尤其是对动态障碍物(人走开、椅子被挪动等)。半圈是无法照顾到的，多圈也没必要，显得很呆。

地图清理恢复行为 ClearCostmapRecovery

ClearCostmapRecovery 的本质就是在机器人位置附近定义一个窗口，然后根据参数选择清理 costmap（全局/局部）的障碍物层，清理范围可以是窗口内或外，从而“重置”代价地图，帮助机器人恢复可行的导航环境。

ClearCostmapRecovery::runBehavior

if (!invert_area_to_clear_){ // 清理内部区域
    ROS_WARN("Clearing %s costmap%s outside a square (%.2fm) large centered on the robot.", affected_maps_.c_str(),
           affected_maps_ == "both" ? "s" : "", reset_distance_);
  }else {
    ROS_WARN("Clearing %s costmap%s inside a square (%.2fm) large centered on the robot.", affected_maps_.c_str(),
           affected_maps_ == "both" ? "s" : "", reset_distance_);
  }
  ros::WallTime t0 = ros::WallTime::now();
  // 清理全局代价地图
  if (affected_maps_ == "global" || affected_maps_ == "both")
  {
    clear(global_costmap_);
    if (force_updating_)
      global_costmap_->updateMap(); // 立即刷新全局地图
    ROS_DEBUG("Global costmap cleared in %fs", (ros::WallTime::now() - t0).toSec());
  }
  t0 = ros::WallTime::now();
  // 清理局部代价地图
  if (affected_maps_ == "local" || affected_maps_ == "both")
  {
    clear(local_costmap_);
    if (force_updating_)
      local_costmap_->updateMap(); // 立即刷新局部代价地图
    ROS_DEBUG("Local costmap cleared in %fs", (ros::WallTime::now() - t0).toSec());
  }

ClearCostmapRecovery::clear

获取 costmap 的所有 Layer 插件，如果该 Layer 在 clearable_layers_ 集合中(可被清理)，并且确实是 CostmapLayer 类型，那么调用clearMap()

for (std::vector >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
    boost::shared_ptr plugin = *pluginp;
    // 去掉namespace前缀
    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){ // 判断该层是否在可清理层集合中
      // 判断该层是否是CostmapLayer
      if(!dynamic_cast(plugin.get())){
        ROS_ERROR_STREAM("Layer " << name << " is not derived from costmap_2d::CostmapLayer");
        continue;
      }
      boost::shared_ptr costmap;
      costmap = boost::static_pointer_cast(plugin);
      clearMap(costmap, x, y);
    }
  }

ClearCostmapRecovery::clearMap

计算以机器人为中心、边长为 reset_distance 的正方形区域。如果 invert_area_to_clear=false，那么清理正方形外部区域；为true，则清理正方形内部区域

void ClearCostmapRecovery::clearMap(boost::shared_ptr costmap,
                                        double pose_x, double pose_y){
  boost::unique_lock lock(*(costmap->getMutex()));
  // 计算以机器人为中心的清理矩形区域,start_point(左下角)、end_point(右上角)
  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);
  // 清除区域
  costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
  double ox = costmap->getOriginX(), oy = costmap->getOriginY();
  double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
  // 更新边界，保证代价地图线程能刷新变化
  costmap->addExtraBounds(ox, oy, ox + width, oy + height);
  return;
}

我已经将代码的注释开源，需要的自取(目前还在更新)。如果感觉对你有帮助，请点一个star，谢谢。

navigation代价注释仓库