GlobalPlanner/plan。makePlan规划出的全局路径。
DWAPlannerROS/global_plan。有时写成global_plan。从makePlan截取出的一段落在本地代价地图内路径，并经过adjust_global_plan进行调整。
DWAPlannerROS/local_plan。有时写成local_plan。
一、param参数
1.1 DWAPlannerROS
max_vel_x = 0.25; min_vel_x = -0.025; max_vel_y = 0.0; // Notice! min_vel_y = 0.0; max_vel_trans = 0.25; min_vel_trans = 0.01; max_vel_theta = 1.00; // Notice! min_vel_theta = 0.01; // degree: 0.57 acc_lim_x = 1.25; acc_lim_y = 0.0; acc_lim_theta = 5; acc_lim_trans = 1.25; prune_plan = false; xy_goal_tolerance = 0.15; // Notice! desire is 0.25 yaw_goal_tolerance = 0.1; trans_stopped_vel = 0.1; rot_stopped_vel = 0.1;
max_vel_x、min_vel_x。限制线速度中的x分量范围。模拟一秒的话，每秒最大位移25cm。
max_vel_y、min_vel_y。不会给y设置速度。要拐弯时都用theta。
max_vel_trans、min_vel_trans。vel_trans表示x、y的合速度，即。因为vel_y总是0，因而max_vel_trans等于max_vel_x。
max_vel_theta。图1是max_vel_theta=0.7时生成的候选轨迹，对这种要过近似90的急转弯时，0.7拐出的弯还不够大，于是增大到1.0。
xy_goal_tolerance开始的4个参数和判断是否到达目标goal有关。只有位置、角度、当前速度，三个条件都满足的情况下，才算导航到达了目标位姿。
xy_goal_tolerance。判断是否到达目标的位置阈值。由于latch_xy_goal_tolerance=true，这个值要设置得比预想的大一些。举个例子，想设的xy_goal_tolerance值是0.25，那就设为0.15。
yaw_goal_tolerance。判断是否到达目标的角度阈值。
trans_stopped_vel、rot_stopped_vel。判断是否到达目标的当前速度阈值，这个速度包括线速度、角速度。rose_ros认为机器人反馈出的线速度、角速度都是0，这两个值只要大于0，就一定满足当前速度要求了。
1.2 LatchedStopRotateController
latch_xy_goal_tolerance = true;
只有位置、角度、当前速度，三个条件都满足的情况下，导航才算到达了目标goal。rose_ros当前速度总是满足的，因而只要考虑位置和角度。在次序上，算法是先让满足位置，然后角度。满足位置后，机器人为满足角度需要继续移动，这些移动可能导致机器人不再满足位置要求。这时会出现机器人这次移动是为满足角度，下一次又是为满足位置，拉拉扯扯很难到达目标goal。latch_xy_goal_tolerance=true可让一旦出现一次位置满足后，就不再检查位置，后面只要一心满足角度就行了。
正如上面说，满足角度时会导致不满足位置，这时要设置个比预想的更小的xy_goal_tolerance。举个例子，想设的xy_goal_tolerance值是0.25，那就设为0.15。满足角度时往往不会有太多位置变化，最后结果可能还是在0.25内。
latch_xy_goal_tolerance=true时，满足角度期间，不会有候选速度概念了，要注意设置的goal至少是较空旷的。
三、生成路径、cmd_vel
1.1 makePlan
生成全局路径使用的是GlobalPlanner功能包，makePlan会生成std::vector<geometry_msgs::PoseStamped>类型的路径，存储在LocalPlannerUtil::global_plan_。
使用DijkstraExpansion计算成本矩阵。
使用GradientPath从成本矩阵提取路径。
问题
在膨胀层，一旦按正确的inscribed_radius_生成膨胀半径，makePlan失败概率大幅增加。但是，减少膨胀半径，那会让全局路径错误选了一条其实是机器人过不去的窄路。
1.2 (LocalPlannerUtil)planner_util_.getLocalPlan
在全局路径中截取出在本地代价地图内的那一段，同时把这些路径点从/map坐标系转换到odom坐标系。函数变量transformed_plan表示生成的路径。
此时transformed_plan就是LocalPlannerUtil::global_plan_某一部分，用rviz看，它们就是重合的。
1.3 DWAPlannerROS::adjust_global_plan
rose_ros新增逻辑。功能是让仔细考虑此时代价地图路况调整路径。
以transformed_plan第一个点为起始，“最后”那个点终点，在充份考虑足迹情况下，重新生成一条路径route.steps。生成用A*算法。
生成route.steps后，还是截短，目的是让后面的goal_costs_有个更准确打分。
1.4 (DWAPlanner)dp_->updatePlanAndLocalCosts
就处理路径上说，updatePlanAndLocalCosts做的只是把transformed_plan复制到DWAPlanner::global_plan_。
1.5 (DWAPlanner)dp_->findBestPath
生成候选轨迹
只在距离目标30cm时才会出现负x速度。x采样点数16。
y速度总是0。y采样点数1。
theta采样点数16。
理论上，最多候选轨迹数目在256。
打分准则
只留3条准则：obstacle_costs_、path_costs_、goal_costs_。
为什么不用oscillation_costs_。findBestPath基本没有了负线数速，既然没有了后退，也就不存在摆动。注：rose_ros不是没有了负线速数，而是findBestPath生成的候选轨迹没有了而已，后面adjust_traj_backward会按需改出负的线速度。
为什么不用goal_front_costs_。认为adjust_global_plan已更好处理了此次本地代价地图内的global_plan，没必须增加“前向”最短到Goal评分。
为什么不用alignment_costs_。认为adjust_global_plan已更好处理了此次本地代价地图内的global_plan，没必须增加“前向”最短到global_plan评分。
1.6 DWAPlannerROS::adjust_traj_backward
rose_ros新增逻辑。实现需要的话改出负的线速度，即倒车。
什么情况会考虑要倒车？——选中轨迹走出的距离不超过9厘米。
倒车时，x_vel总是-0.1，theta_vel则可能是0.1、0或-0.1。根据机器人yaw（航偏角）和路径角度，选择要用哪个theta_vel。
选出(-0.1, 0, 0.1/0/-0.1)后，要进行打分判断，如果通不过打分，放弃此次修改，cmd_vel保持着dp_->findBestPath结果。
如果此次要倒车，而且机器人yaw和路径角度差160度以上，即基本在两个相反方向，会要求下次adjust_traj_backward不必考虑9厘米限制，也试着执行倒车。
三、恢复性操作：trose_recovery
rose_ros只使用一个恢复性操作：trose_recovery。
在图3，想让机器人从当前位置向上通过窄道回到map坐标系原点。由于在全局代价地图（较暗的代价地图）中，雷达检测到的历史障碍物没被即时清除，阻塞住了窄道，全局规划将失败。为让成功，需要清掉这些历史障碍物，一种方法就是把全局代替地图恢复到默认值。
<libros>/rose_ros/rose_recovery.cpp
------
void trose_recovery::runBehavior()
{
ROS_WARN("Rose clear recovery behavior started.");
global_costmap_->clearCostmaps();
local_costmap_->clearCostmaps();
}Costmap2DROS::clearCostmaps()，代替之前Costmap2DROS::resetLayers。用于把主代价地图，以及内中各层代价地图恢复到默认值。如何把各层代价地图恢复到默认值？靠的是Layer虚函数clearCostmap。
Layer::clearCostmap。一个新增的Layer虚函数。通知指定层，要让下一次plugin.updateBounds、plugin.updateCosts工作在一张干净的私有代价地图。特定层在实现该方法时，主要任务是将整张私有代价地图恢复到默认值。
去掉了Layer的虚函数reset，某种意义上说，可认为是用clearCostmap代替了reset。
四、Costmap2D::updateObstacleToInscribed
updateObstacleToInscribed会扩大足迹面积，扩出栅格中，代价是LETHAL_OBSTACLE的改为INSCRIBED_INFLATED_OBSTACLE。扩大方法是四个方向的半径变成内切圆半径加1，
图4显示了机器人正要过一条窄道。此时机器人左、右都挨靠障碍物。为让机器人至少能动，ros在障碍层会执行清除操作。
setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
setConvexPolygonCost用于把足迹所在栅格的代价设置为FREE_SPACE。
虽然做了清除操作，但在窄道场合，依旧会出现所有候选路径都通不过准则。updateObstacleToInscribed做的就是再把足迹+1的那一圈栅格“清除”掉，但“清除”不是简单置为FREE_SPACE，而只是把LETHAL_OBSTACLE栅格改为INSCRIBED_INFLATED_OBSTACLE。
updateObstacleToInscribed把让周围那一圈障碍消失，这极会导致不可预料后果。那什么时候执行updateObstacleToInscribed？
- 每次adjust_global_plan前。判断贴身障碍物情况，发现“正在过窄道”时，执行。这是个人最希望实现的方案，但找不到能好的判断“正在过窄道”方法。
- 在findBestPath后。变量path表示findBestPath得到的轨迹。如果path是个有效轨迹，或有着非常小的path.xv_、path.thetav_，执行。执行后，要再次findBestPath。这么处理原因：要updateObstacleToInscribed，是周围情况导致findBestPath没法得到一条有效轨迹，或得到的有效轨迹只能移动很短距离。既然如此，发现findBestPath后这是两种结果的，就执行。执行updateObstacleToInscribed后，可能findBestPath就能可选出更优轨迹了。
目前用的是第二种方案。这也产生了另外一个问题，如果不是过窄道时，findBestPath也产生了上述两结果，那会导致障碍物消失。
考虑到副作用，要限制内切圆半径>=3个栅格的才会进入updateObstacleToInscribed。
updateObstacleToInscribed在扩大半径时，只要长度不是超出宽度太大，只要考虑宽度。
疑问：ajdust_global_plan中的a_star_search，在起始点时就失败了。是不是此时要也进行updateObstacleToInscribed？
图5是updateObstacleToInscribed导致错误实例。当前yaw指向东南，adjust_global_plan判断出来“path_case_(path_only_backward) &&straight_backward_(true)”，即需要笔直后退。导致原因应该紧贴背后的障碍物被updateObstacleToInscribed清掉。或许吧，此时机器人正确操作：向前。
五、DWAPlannerROS::resolve_pressure
足迹(footprint)有说到“Costmap2D::setConvexPolygonCost(..., FREE_SPACE)会清除毗邻障碍物”。如果这障碍物又恰好在机器人前面，那会造成机器人一直顶着障碍物向前走。
resolve_pressure基于Costmap2D::setConvexPolygonCost得到的障碍栅格，判断前面(front_has_obs)、后面(back_has_obs)是否有障碍。一旦前面有、后面没有，触发执行后退(adjust_traj_backward)。
函数名resolve_pressure出现压力字眼，是一些机器人会用前面放个压力传感器来判断是不是有障碍物。是通过压力传感器，还是雷达，后绪处理往往是一样的，就是后退。
六、窄道时的findBestPath
举个例子，要过40厘米窄道。机器人宽30厘米。这时有可能发生findBestPath找不到一条可用候选路径。候选路径出错是因为无法满足以下某条准测。
- ObstacleCost。这占大多数（示例：272条占了186条）。因它出错时，该候选轨迹中至少存在这么个点，机器人站在那里时，会碰到障碍物。之前想过用updateObstacleToInscribed，考虑到副作用，要废弃。
- goal_costs。此时goal_costs返回-2(unreachableCellCosts)，因它出错时，该候选轨迹中至少存在这么个点，由该点无法到达global_plan的Goal点。
如何解决这问题。
- 提高pathfind::a_star_search成功率。一旦a_star_search，那会后退到使用makePlan出来的global_plan，以它得到的Goal点，往往是错的。
- findBestPath找不到候选路径时，用pathfind::a_star_search移动机器人。或许只要移动下机器人，下次候选路径就有了。
七、path_costs_
path_costs_用的还是Last。我认为得用Sum，到现在没用是有问题不好解决。用了Sum后，那些微小x速度的候选轨迹，因为整条都靠近起始点，路径点又可能不多，在这上有了巨大打分优势，如何平衡掉这个“错误”优势。
...... (max_x_vel: 0.25, max_theta_vel: 0.8), start_cell(30, 39), best_cost: 30.35000 [241/270]vel(0.23333, 0, -0.26666), points:20, cost: [(52.64999)379.00000 + 1.00000 + 8.00000 + 0 + 0], last_cell(34, 35) [242/270]vel(0.23333, 0, -0.15999), points:20, cost: [(44.64999)379.00000 + 1.00000 + 6.00000 + 0 + 0], last_cell(33, 36) [245/270]vel(0.23333, 0, 0.05333), points:20, cost: [(38.95000)379.00000 + 0 + 5.00000 + 0 + 0], last_cell(32, 36) [246/270]vel(0.23333, 0, 0.15999), points:20, cost: [(38.95000)379.00000 + 0 + 5.00000 + 0 + 0], last_cell(32, 36) [247/270]vel(0.23333, 0, 0.26666), points:20, cost: [(36.64999)379.00000 + 1.00000 + 4.00000 + 0 + 0], last_cell(31, 36) [253/270]vel(0.24999, 0, -0.80000), points:22, cost: [(70.34999)379.00000 + 2.00000 + 12.00000 + 0 + 0], last_cell(36, 33) [258/270]vel(0.24999, 0, -0.26666), points:22, cost: [(50.35000)379.00000 + 2.00000 + 7.00000 + 0 + 0], last_cell(34, 36) [261/270]vel(0.24999, 0, 0), points:22, cost: [(40.64999)379.00000 + 1.00000 + 5.00000 + 0 + 0], last_cell(33, 37) [262/270]vel(0.24999, 0, 0.05333), points:22, cost: [(34.95000)379.00000 + 0 + 4.00000 + 0 + 0], last_cell(32, 37) [263/270]vel(0.24999, 0, 0.15999), points:22, cost: [(34.95000)379.00000 + 0 + 4.00000 + 0 + 0], last_cell(32, 37) [265/270]vel(0.24999, 0, 0.37333), points:22, cost: [(30.35000)379.00000 + 2.00000 + 2.00000 + 0 + 0], last_cell(30, 37)
按规则，分数最好的是30.35000，对应速度vel(0.24999, 0, 0.37333)。相比vel(0.24999, 0, 0)，它顺时针拐了个弯，这极可能会碰上上面的障碍物。