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

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的核心思路是：

让机器人原地旋转一整圈（360°）。
这样它的传感器（激光雷达、深度相机等）可以扫描周围环境，刷新局部 costmap。
如果之前 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°，这时候它其实有两个候选方向能继续往下走（顺时针 / 逆时针），可能会引发歧义。
所以代码里采用了两阶段的一个技巧，使得机器人可以保证确实转了一圈：

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

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

地图清理恢复行为 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){ // 判断该层是否在可清理层集合中// 判断该层是否是CostmapLayerif(!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;
}