ROS源码阅读---局部路径规划之DWAPlannerROS分析

1 体系结构

（1）主要成员
base_local_planner::LocalPlannerUtil planner_util_; 用来存储运动控制参数以及costmap2d、tf等，会被传入dp_
costmap_2d::Costmap2DROS* costmap_ros_;
base_local_planner::OdometryHelperRos odom_helper_; 用来辅助获取odom信息，会被传入dp_
boost::shared_ptr dp_; 正常的dwa运动控制类
base_local_planner::LatchedStopRotateController latchedStopRotateController_; 到达目标点后的停止旋转运动控制类

（2）主要接口
void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)—初始化
bool setPlan(const std::vector& orig_global_plan)—设置全局路径
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel)—计算控制速度
bool isGoalReached()—判断是否达到目标点

2 总体流程

在move base控制循环中会在规划出新的路径时，将新的全局路径利用setPlan传给DWAPlannerROS，然后调用DWAPlanner::setPlan将路径传递给DWAPlanner（同时复位latchedStopRotateController_中的锁存相关的标志位）。

在运动规划之前，move_base先利用DWAPlannerROS::isGoalReached（内部利用LatchedStopRotateController::isGoalReached函数实现）判断是否已经到达了目标点（即位置在xy_goal_tolerance以内，朝向在yaw_goal_tolerance以内，且速度小于trans_stopped_velocity，rot_stopped_velocity），如果是则控制结束，即发送0速，且复位move_base的相关控制标记，并返回action成功的结果。否则，利用DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)计算局部规划速度。

在DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)中，会先将全局路径映射到局部地图中，并更新对应的打分项（参考DWAPlanner::updatePlanAndLocalCosts函数，和具体的打分项更新）。然后，利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置，如果是，则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令（取决于是否机器人已经停稳，进而决定实现减速停止或者旋转至目标朝向），否则利用DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped &global_pose, geometry_msgs::Twist& cmd_vel)计算DWA局部路径速度。

对于停止时的处理逻辑，即LatchedStopRotateController的所有逻辑，参考“停止”章节。

对于DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped &global_pose, geometry_msgs::Twist& cmd_vel)函数，直接利用DWAPlanner::findBestPath(
tf::Stamped global_pose,
tf::Stamped global_vel,
tf::Stamped& drive_velocities,
std::vector footprint_spec)获取最优局部路径对应的速度指令。

在DWAPlanner::findBestPath函数中，先利用SimpleTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
bool discretize_by_time = false)初始化轨迹产生器，即产生速度空间。
然后，利用SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector* all_explored = 0)查找最优的局部路径。在该函数中，先调用每个打分项的prepare函数进行准备，参考“打分”章节。然后，利用SimpleScoredSamplingPlanner里面存储的TrajectorySampleGenerator轨迹产生器列表（目前，只有SimpleTrajectoryGenerator一个产生器）分别进行轨迹产生和打分，并根据打分选出最优轨迹。对于每个轨迹的打分，调用SimpleScoredSamplingPlanner::scoreTrajectory执行，在该函数中，遍历各个打分项进行打分并进行求和，如果某个打分项打分小于0，则将该负值作为路径打分项进行立刻返回。在对每个打分项打分之前，先获取对应的scale参数，如果scale为0证明该打分项无效，则跳过。如果获取到正常的打分项打分后（利用打分项的scoreTrajectory函数），将打分项乘以对应的scale作为最终的打分项打分。

具体的速度采样和轨迹规划参考“运动规划”章节。

接着在DWAPlanner::findBestPath中，根据使能参数，选择发布轨迹相关的可视化数据。
然后，如果最优轨迹有效，则利用最优轨迹更新摆动打分项的标志位。
最后，在返回最优规划轨迹之前，判断最优轨迹代价，如果代价小于0，则代表最优轨迹无效，即没有有效轨迹，则将返回速度指令，设置为0。

最后，如果DWAPlannerROS::dwaComputeVelocityCommands会判断得到的最优轨迹代价值，如果小于0，则该函数会返回false，从而引起move_base进行进一步的处理，如重新规划、recovery等。

至此，完成了整个dwa local planner的流程。

3 运动规划

在每个控制周期内，DWAPlanner::findBestPath会查找最优局部轨迹。

在DWAPlanner::findBestPath中，首先调用SimpleTrajectoryGenerator::initialise进行速度采样，然后利用SimpleScoredSamplingPlanner::findBestTrajectory根据速度采样空间进行轨迹产生、打分、筛选，从而得到最优局部轨迹。

（1）采样
速度采样在SimpleTrajectoryGenerator::initialise中进行，总体思路是先获取速度空间边界，然后根据采样个数参数在空间内进行采样。

在获取空间边界时，根据use_dwa参数，采用两套策略。
如果use_dwa==false，首先用当前位置与目标位置的距离处理仿真时间sim_time（模拟仿真时间内匀减速到0，刚好到达目标点的情景）得到max_vel_x和max_vel_y边界，然后基于acc_lim * sim_time得到一种边界（上边界），还有设置的速度极限参数（max_vel_xxx，min_vel_xxx）作为一种边界，然后选取三种边界中空间较小的边界。这种策略，能够获得较大的采样空间（因为用了sim_time）。
如果use_dwa==true，则直接用acc_lim * sim_period得到边界，然后还有设置的速度极限参数作为边界，然后选取两种边界中空间较小的边界。

得到速度空间边界后，根据x，y，theta三个采样个数进行插补，进而组合出整个速度采样空间。

（2）获取最优轨迹
获取最优轨迹在SimpleScoredSamplingPlanner::findBestTrajectory中进行，在该函数中，首先调用各个打分项的prepare函数进行准备。然后遍历std::vector

4 路径

（1）存储全局路径
路径类型为std::vector。
在move base控制循环中会在规划出新的路径时，将新的全局路径利用setPlan传给DWAPlannerROS，然后调用DWAPlanner::setPlann将路径传递给DWAPlanner（同时复位latchedStopRotateController_中的锁存相关的标志位）。
在DWAPlanner::setPlann中，会先将OscillationCostFunction中的摆动标志位复位，然后利用planner_util_->setPlan将路径传入planner_util_，直接保存为global_plan_。
此时的路径时相对于全局地图的全局坐标系的（通常为”map”)。

（2）局部路径映射及存储
在computeVelocityCommands计算速度前，会先将全局路径映射到局部地图坐标系下（通常为“odom”），通过LocalPlannerUtil::getLocalPlan(tf::Stamped& global_pose, std::vector& transformed_plan)实现。
在getLocalPlan中，先将较长的全局路径映射并截断到局部地图内（即坐标系转换为局部地图，且范围完全在局部地图内，超出地图的则抛弃）。然后，裁减全局路径和局部路径（与机器人当前位置距离超过1m的旧的路径会被裁减掉）。

裁减过的全局路径还保存在planner_util_中，映射并经过裁减后的局部路径会在computeVelocityCommands函数中传入dp_中（利用DWAPlanner::updatePlanAndLocalCosts函数）。
在updatePlanAndLocalCosts中，会将局部路径保存到dp_的global_plan_中（对于dp_来说，局部地图中映射的全局路径就是全局的，而dp_自己规划的路径是局部的）。然后，将该路径信息传入各个打分对象，具体参考打分章节。

5 打分

打分对象一共6个，分别为：
base_local_planner::OscillationCostFunction oscillation_costs_（摆动打分）
base_local_planner::ObstacleCostFunction obstacle_costs_（避障打分）
base_local_planner::MapGridCostFunction path_costs_（路径跟随打分）
base_local_planner::MapGridCostFunction goal_costs_（指向目标打分）
base_local_planner::MapGridCostFunction goal_front_costs_（前向点指向目标打分）
base_local_planner::MapGridCostFunction alignment_costs_（对齐打分）

其中，摆动打分和避障打分项是独立的类型，后四个打分项是同一类型。

打分计算过程中出现的负的值可以认为是错误代码（用于指示具体的出错原因），而不是得分，该说明对下面所有的打分描述有效。
如果轨迹为空（在产生轨迹时出错，例如不满足最大最小速度要去），则各个打分项对应的得分都为0。
（1）打分对象初始化及更新
oscillation_costs_
在DWAPlanner的构造函数中，利用oscillation_costs_.resetOscillationFlags()复位摆动标志位（侧移、旋转、前进方向的相关参数strafe_pos_only_，strafe_neg_only_，strafing_pos_，strafing_neg_，rot_pos_only_，rot_neg_only_，rotating_pos_，rotating_neg_，forward_pos_only_，forward_neg_only_，forward_pos_，forward_neg_）。
然后将oscillation_costs传入打分项列表（也会加入其它打分项），并将打分项列表std::vector

6 停止

对于DWAPlannerROS的停止处理逻辑，由LatchedStopRotateController类完成，主要包括了停止判断、加速停止、旋转至目标朝向三个大的部分。

（1）停止判断
在move_base的每个控制周期，都会利用利用DWAPlannerROS::isGoalReached（内部利用LatchedStopRotateController::isGoalReached函数实现）判断是否已经到达了目标点（即位置在xy_goal_tolerance以内，朝向在yaw_goal_tolerance以内，且速度小于trans_stopped_velocity，rot_stopped_velocity）。

对于LatchedStopRotateController::isGoalReached函数，分为锁存目标和不锁存目标两种处理逻辑。锁存目标，即如果机器人在行驶过程中出现过位置满足xy_goal_tolerance的条件时，则设置xy_tolerance_latch_标志位，代表已经达到过目标，不用考虑最终是否超出目标点，直接进行停止、旋转至目标朝向即可。
如果不锁存，则必须始终满足机器人当前位置是否满足xy_goal_tolerance的条件，满足则代表到达了目标位置。

在到达目标位置的前提下，还要判断机器人朝向是否满足目标朝向yaw_goal_tolerance的需求，如果也满足，则判断当前速度是否满足停止条件，即x和y的速度小于trans_stopped_velocity，theta速度小于rot_stopped_velocity。

只有到达位置、达到朝向、速度满足停止三个条件都满足的情况下，才算机器人到达了目标位姿，isGoalReached函数才会返回true。

（2）加速停止
在DWAPlannerROS::computeVelocityCommands的每个计算周期内，都会先利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置，如果是，则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令，否则才会使用DWAPlanner计算最优轨迹对应的速度命令。

对于isPostionReached的判断，和isGoalReached中判断达到位置的逻辑一样，即如果启动了锁存，且xy_tolerance_latch_标志位被标记（即满足过xy_goal_tolerance条件），则认为已经到达；如果未启用锁存，则需要基于当前位置是否满足xy_goal_tolerance确定是否到达了位置。

在LatchedStopRotateController::computeVelocityCommandsStopRotate函数中，会假设用户想要进行旋转停止了，即已经到达位置了，因此会先判断当前朝向是否满足yaw_goal_tolerance要求，如果满足，则将速度指令设置为0（这种做法太过粗略）。
如果朝向没有满足要求，则会判断是要进行加速停止还是旋转至目标点。

如果机器人还未处于旋转停止状态（通过rotating_to_goal标记），并且速度没有满足停止要求，则会调用stopWithAccLimits进行加速停止处理，否则代表应进入了旋转停止状态，则会调用rotateToGoal进行旋转至目标朝向处理，同时设置rotating_to_goal标志位true。

对于stopWithAccLimits函数，对于利用acc_lim参数进行最快的减速命令计算，然后利用DWAPlanner::checkTrajectory函数计算出轨迹，并进行轨迹打分。在计算轨迹时利用了SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj)，如果速度不满足最小速度要求（min_rot_vel、min_trans_vel）则轨迹为空。
在对轨迹进行打分时，利用了SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost)，如果轨迹为空，则打分为0，此时虽然checkTrajectory返回true，但对应的速度可能过小（目前判断没有太大影响）。
如果checkTrajecotry返回true（轨迹打分大于等于0）则代表路径有效，stopWithAccLimits计算出的加速停止速度命令有效，否则设置速度命令为0。

（3）旋转至目标朝向
旋转至目标朝向有rotateToGoal函数计算，首先将x和y速度设置为0，对于theta速度，利用当前朝向与目标朝向的差的比例控制（系数为1）产生，然后用加速能力（acc_lim*sim_period）、速度限幅（max_rot_vel，min_rot_vel）、减速能力（v^2=2as公式）进行速度限幅。接着利用DWAPlanner::checkTrajectory进行轨迹打分检测，逻辑同前。

转自 http://www.cnblogs.com/sakabatou/p/8297479.html