ROS源代码阅读（9）——DWA算法

2021SC@SDUSC

ROS源代码阅读（9）
后面主要看dwa算法
DWAPlannerROS是封装类，提供了与move_base的接口，而DWAPlanner是具体实现类，它非常依赖costmap，因此我们在使用时需要保证代价地图的膨胀度以及实时更新频率。

总体过程是，在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::Stampedtf::Pose &global_pose, geometry_msgs::Twist& cmd_vel)计算DWA局部路径速度。
对于DWAPlannerROS::dwaComputeVelocityCommands(tf::Stampedtf::Pose &global_pose, geometry_msgs::Twist& cmd_vel)函数，直接利用
`DWAPlanner::findBestPath

  tf::Stamped<tf::Pose> global_pose,

  tf::Stamped<tf::Pose> global_vel,

  tf::Stamped<tf::Pose>& drive_velocities,

  std::vector<geometry_msgs::Point> 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的流程。

下面看一下dwa的代码：
（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<geometry_msgs::PoseStamped>& orig_global_plan)—设置全局路径

bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel)—计算控制速度

bool isGoalReached()—判断是否达到目标点

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

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

运动规划
采样，速度采样在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三个采样个数进行插补，进而组合出整个速度采样空间。
获取最优轨迹
获取最优轨迹在SimpleScoredSamplingPlanner::findBestTrajectory中进行，在该函数中，首先调用各个打分项的prepare函数进行准备。然后遍历std::vector<TrajectorySampleGenerator*>轨迹产生器列表（目前只有一个）进行打分，并且一旦某个产生器发现了最优轨迹则返回（该做法较为粗略，即使有多个产生器也无法得到全局最优）。

对于每个打分器，由于上一步已经进行了速度采样，因此可以逐个速度产生轨迹，并进行打分，同时记录最优打分和最优轨迹。

其中，由速度产生轨迹由SimpleTrajectoryGenerator::generateTrajectory(

  Eigen::Vector3f pos,

  Eigen::Vector3f vel,

  Eigen::Vector3f sample_target_vel,

  base_local_planner::Trajectory& traj)函数实现。

在该函数中，先判断速度是否满足“平移速度不小于min_trans_vel且旋转速度不小于min_rot_vel”和“平移速度不大于max_trans_vel”两个条件，如果满足，则直接返回false，此时，traj为空路径。否则，继续正常的轨迹产生流程。

为了产生轨迹，需要由pos，vel，sample_target_vel三个参数离散仿真出轨迹中的所有点位姿。首先，需要确定仿真步数（即多少个点）（虽然由参数discretize_by_time_确定是由sim_time/sim_granularity计算仿真步数，还是由std::max(sim_time_distance / sim_granularity_, sim_time_angle / angular_sim_granularity_)计算，但该参数目前保持为false，即由后者计算仿真步数）。有了仿真步数num_steps，可以由sim_time/num_steps计算出每步的时间间隔dt。然后，就可以根据具体策略计算轨迹点。

如果use_dwa==false，则采用连续加速的策略，即仿真出的轨迹中不同点对应的速度是变化的，此时将轨迹中保存的对应速度设为基于当前速度第一次加速出的速度。否则，轨迹中的各个点为同样的速度，即sample_target_vel，此时轨迹中保存的速度也是该速度。

然后，开始计算各个轨迹点，如果use_dwa==false，则边计算轨迹点位姿，边变化速度，否则用恒速计算轨迹点位姿。

这样，就完成了轨迹的生成。

每个速度对应轨迹产生后，调用SimpleScoredSamplingPlanner::scoreTrajectory进行打分，在该函数中，遍历各个打分项进行打分并进行求和，如果某个打分项打分小于0，则将该负值作为路径打分项进行立刻返回。在对每个打分项打分之前，先获取对应的scale参数，如果scale为0证明该打分项无效，则跳过。如果获取到正常的打分项打分后（利用打分项的scoreTrajectory函数），将打分项乘以对应的scale作为最终的打分项打分。

至此，就完成了速度采样、轨迹生成、轨迹打分和选优。

路径
（1）存储全局路径
路径类型为std::vector<geometry_msgs::PoseStamped>。
在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::Stampedtf::Pose& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan)实现。
在getLocalPlan中，先将较长的全局路径映射并截断到局部地图内（即坐标系转换为局部地图，且范围完全在局部地图内，超出地图的则抛弃）。然后，裁减全局路径和局部路径（与机器人当前位置距离超过1m的旧的路径会被裁减掉）。
裁减过的全局路径还保存在planner_util_中，映射并经过裁减后的局部路径会在computeVelocityCommands函数中传入dp_中（利用DWAPlanner::updatePlanAndLocalCosts函数）。
在updatePlanAndLocalCosts中，会将局部路径保存到dp_的global_plan_中（对于dp_来说，局部地图中映射的全局路径就是全局的，而dp_自己规划的路径是局部的）。

打分

打分对象一共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。
打分对象初始化及更新
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<base_local_planner::TrajectoryCostFunction*>和采样规划器列表std::vector<base_local_planner::TrajectorySampleGenerator*>传入打分采样规划器base_local_planner::SimpleScoredSamplingPlanner。
在DWAPlannerROS的动态配置回调函数中，会将动态配置参数再传入DWAPlanner::reconfigure函数，在该函数中利用oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle)初始化该打分项。
在正常行走过程中（没有到达目标点范围），DWAPlannerROS会调用dwaComputeVelocityCommands实现computeVelocityCommands函数，在dwaComputeVelocityCommands中，会调用DWAPlanner::findBestPath方法，在该方法中会先采样计算出最优路径，然后利用oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel)更新摆动打分项的标志位。在打分项更新标志位的函数中，如果判断最优路径的代价也小于0（即无效），则不更新，否则，则根据最优路径和最小平移速度限制设置打分标志setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans)。然后，如果在某些方向上有了限制，则检查是否能够复位标志位resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev)。
对于setOscillationFlags函数，先设置x方向的摆动：一旦规划的速度与当前的方向不同，则设置forward_xxx_only标志位，表示后续只能沿规划的速度方向运行。对于侧向摆动和旋转摆动，必须在x方向速度小于min_vel_trans时，才会判断，判断逻辑和x方向一致。
由于前面可能变过方向，即设置过xxx_xxx_only标志位，短时间内不能变向。此时，需要利用resetOscillationFlagsIfPossible判断是否可以取消标志位，即是否可以开始变向。通过判断当前位置相对于上次换向的位置的平移和旋转位置是否超过oscillation_reset_dist_和oscillation_reset_angle_，如果是则复位所有标志位。
上面两个函数实现了_only标志位的置位和复位。
obstacle_costs_
在DWAPlanner的构造函数中，利用local costmap初始化避障打分项，然后通过sum_scores参数（默认为false）设置避障打分项的打分汇总方式。
然后，将obstacle_costs_传入打分项列表，同上。
在DWAPlanner::reconfigure函数中，利用obstacle_costs_.setScale函数设置比例参数为resolution * occdist_scale（碰撞参数），并在obstacle_costs中记录为scale_。然后利用obstacle_costs_.setParams函数设置max_trans_vel, max_scaling_factor, scaling_speed（这些参数可以引起避障打分项中的footprint变化，从而根据不同的速度膨胀不同的大小）。
在每一次通过DWAPlanner的findBestPath函数计算最优局部路径时，都会利用obstacle_costs_.setFootprint为obstacle_costs_设置一次footprint参数（带padding），会被obstacle_costs_的footprintr_spec_（std::vector<geometry_msgs::Point>类型）参数记录。
至此，就记录了所有参数，并且更新了碰撞打分相关的footprint。