navigation功能包提供了三种恢复性操作：ClearCostmapRecovery、RotateRecovery和MoveSlowAndClear。
一、trose_recovery
trose_recovery是rose_ros新增的恢复性操作，也是唯一使用的。行为只是清除(clearCostmaps)本地代价地图。
既然需清除本地代价地图，全局不需要清除吗？——要的，只是清除全局代价地图放在move_base。
在图1，左侧显示的雷达点云，机器人正前方没有障碍物。右侧显示的全局代价地图，由于历史雷达点云影响，机器人前方却出了障碍物，导致无法makePlan出一条到“M”栅格的路径。在这种窄道，只要一个错误障碍栅格就可能造成阻塞。这时，恢复全局代价地图时希望做到两点。
- 清除(clearCostmaps)全局代价地图。
- makePlan前，需画上、并只画上最近那个雷达点云。“需画上”是因为当前障碍物可能和建图时有变化了，像门上关。“只画上”是避免历史点云影响。
为此，“清除+updateMap”被放在了planThread线程中执行的makePlan前。既然CLEARING后一定会执行makePlan，反正那里要清除全局代价地图，trose_recovery就没要执行了。以下是相关实现逻辑。
- (move_base)进入CLEARING时，执行trose_recovery恢复性操作，即清除本地代价地图。
- (move_base)变量just_clearing_置为true。
- (move_base)makePlan发现just_clearing_是true，执行全局代价地图的“清除+updateMap”，并把just_clearing_置为false。
二、ClearCostmapRecovery
- reset_distance_：0.1米
- invert_area_to_clear_：false
- affected_maps_：both
- force_updating_：false
- clearable_layers_：{obstacle_layer}
清除代价地图中指定层的部分区域，所谓清除是指把该栅格代价置为NO_INFORMATION。那这个区域在哪呢？——首先以当前机器人中心为中心，向上、下、左、右各扩展reset_distance_米生成一个矩形R。然后判断invert_area_to_clear_。是false时，清除R矩形内栅格。是true时，清除代价地图内除R矩形外的栅格。
对代价地图执行清除操作后，如果fore_upateing_，还会对该地图执行updateMap。
代阶地图有两种：全局代价地图和局部代价地图。affected_maps_参数决定要处理哪些地图，是both时同时处理两种，global只处理全局代价地图，local则只处理局部代价地图。
只是清除各层的私有costmap，不会影响主costmap。clearable_layers_决定要清除的是哪些层，一般设置的是{obstacle_layer}，即只清除障碍层。
<libros>/clear_costmap_recovery/src/clear_costmap_recovery.cpp
------
void ClearCostmapRecovery::runBehavior(){
...
ros::WallTime t0 = ros::WallTime::now();
if (affected_maps_ == "global" || affected_maps_ == "both")
{
clear(global_costmap_);
if (force_updating_)
global_costmap_->updateMap();清除并updateMap全局代价地图。
}
t0 = ros::WallTime::now();
if (affected_maps_ == "local" || affected_maps_ == "both")
{
clear(local_costmap_);
if (force_updating_)
local_costmap_->updateMap();清除并updateMap局部代价地图。
} }
clear(...)作用是获取枚举代价地图中的各层costmap，如果该层在clearable_layers_，对其调用clearMap进行清理。
void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap){
std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();
geometry_msgs::PoseStamped pose;
if(!costmap->getRobotPose(pose)){
ROS_ERROR("Cannot clear map because pose cannot be retrieved");
return;
}
double x = pose.pose.position.x;
double y = pose.pose.position.y;
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){plugin->getName()值示例：global_costmap/static_layer。name是它的后半部static_layer。
... boost::shared_ptr<costmap_2d::CostmapLayer> costmap; costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin); clearMap(costmap, x, y); } } }
三、RotateRecovery
一篇关于RotateRecovery的文章：navigation导航包中rotate_recovery源码解读。
- tolerance_ = 0.1孤度;
- acc_lim_th_ = 3.2米/秒^2;
- frequency_ = 20; 间隔1000/20=50毫秒。
- min_rotational_vel_ = 0.4米/秒;
- max_rotational_vel_ = 1米/秒;
RotateRecovery行为是让机器人原地逆时针转一圈。如果转一圈时间有点长，那最多转5秒钟。为实现转圈，它工作在局部代价地图。以下是大致逻辑。
- 1、得到机器人中心在global坐标系下的位姿，从中提取出角度，记为start_angle。
- 2、进入while循环，直到机器人转了360度，或累计时间已超过5秒。
- 2.1、用计算start_angle一样方法，得到机器人当前角度，记为current_angle。
- 2.2、算出现在离转完360度还剩多少度，记为dist_left。
- 2.3、通过一个对累加角度(sim_angle)的while循环，判断机器人是否能转完剩下的dist_left度。判断是否能转，用的是模拟机器人将走路径中的N个位置。while一次称为一轮，在N轮模拟中，对机器人位姿三要素x、y和theta，x、y固定不变，theta则从current_angle一轮轮增大到current_angle + dist_left。检查每一轮(x, y, theta)时，机器人是否会碰到障碍物。只要有一轮碰到，认为转圈失败，结束RotateRecovery。while是如何把current_angle逐渐增大到current_angle + dist_left？方法是在[current_angle, current_angle + dist_left)以间隔sim_granularity_取N个点。举个例子，current_angle是10，dist_left是100，sim_granularity_是2，即要从10度转到110度，取点间隔是2度，那这个while会执行50轮。theta依次是10、12、14、...、108。
- 2.4、根据加速度、速度和路程的公式，以dist_left作为路程，计算出机器人要用的速度vel。另外，vel必须处于范围[min_rotational_vel_, max_rotational_vel_]之内。
- 2.5、以(0, 0, vel)发向机器人。
- 2.6、为保证机器人有时间按这速度移动，须要留出点时间。等待，让这轮while的时间不小于(1/frequency_)秒。
<libros>/rotate_recovery/src/rotate_recovery.cpp
------
void RotateRecovery::runBehavior()
{
...
ros::Rate r(frequency_);以frequency_频率向机器发布速度指令。frequency_值是20时，以间隔1000/20=50毫秒发布向面机器。最后的r.sleep()会保证发布间隔至少是20毫秒。
ros::NodeHandle n;
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
geometry_msgs::PoseStamped global_pose;
local_costmap_->getRobotPose(global_pose);
double current_angle = tf2::getYaw(global_pose.pose.orientation);
double start_angle = current_angle;
bool got_180 = false;
while (n.ok() &&
(!got_180 ||
std::fabs(angles::shortest_angular_distance(current_angle, start_angle)) > tolerance_))
{- start_angle：机器人初始时刻的角度。如果真能转完一圈，机器人应该还大致这是角度。
- got_180：机器人已转过的角度是否已超过180度。
结束while条件：机器人至少转过180度，并且和初始角度差值<=tolerance_。由于tolerance_很小，换句话说，角度转换一圈。
// Update Current Angle
local_costmap_->getRobotPose(global_pose);
current_angle = tf2::getYaw(global_pose.pose.orientation);
// compute the distance left to rotate
double dist_left;
if (!got_180)
{
// If we haven't hit 180 yet, we need to rotate a half circle plus the distance to the 180 point
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
{
// If we have hit the 180, we just have the distance back to the start
dist_left = std::fabs(angles::shortest_angular_distance(current_angle, start_angle));
}dist_left是离360度还剩多少度。对第一轮while，这值就是360度。
double x = global_pose.pose.position.x, y = global_pose.pose.position.y;
// check if that velocity is legal by forward simulating
double sim_angle = 0.0;
while (sim_angle < dist_left)
{
double theta = current_angle + sim_angle;
// make sure that the point is legal, if it isn't... we'll abort
double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
if (footprint_cost < 0.0)
{通过一个对累加角度(sim_angle)的while循环，判断机器人是否能转完剩下的dist_left度。判断是否能转，用的是模拟机器人将走路径中的N个位置。这里是在(x, y, theta)位置位置碰到，认为不能转，结束RotateRecovery。
return; } sim_angle += sim_granularity_; } // compute the velocity that will let us stop by the time we reach the goal double vel = sqrt(2 * acc_lim_th_ * dist_left); // make sure that this velocity falls within the specified limits vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
根据加速度、速度和路程的公式，以dist_left作为路程，计算机器人旋转时的速度vel。另外，vel必须处于范围[min_rotational_vel_, max_rotational_vel_]之内。
geometry_msgs::Twist cmd_vel; cmd_vel.linear.x = 0.0; cmd_vel.linear.y = 0.0; cmd_vel.angular.z = vel; vel_pub.publish(cmd_vel); r.sleep(); } }
我在想，转一圈后，能有什么作用？想像这么个场景，雷达用的是视角只有180度深度摄像头，先用ClearCostmapRecovery清除了以机器人中心的R矩形，转过360后，确实能扫出周围障碍物，从而填满R矩形。但用视角是360度的激光雷达，这转一圈就纯属多余。
用的是360度激光雷达，不知这RotateRecovery有啥用。