DWA
在move_base下的dwa_local_planner文件夹下，有两个类：DWAPlannerROS和DWAPlanner。一个是供ROS调用的类，后者是DWA的核心实现类。
ROS调用
参考：ROS源码阅读
在movebase的局部规划函数executeCycle中，使用规划器tc_的函数setPlan, isGoalReached, computeVelocityCommands tc_由插件对象 tc_ = blp_loader_.createInstance(local_planner);创建
DWAPlannerROS
继承自接口类：nav_core::BaseLocalPlanner。
主要成员
1
2
3
4
5
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<DWAPlanner> dp_; // 正常的dwa运动控制类
base_local_planner::LatchedStopRotateController latchedStopRotateController_; // 到达目标点后的停止旋转运动控制类
按照ROS里面调用的顺序来看源码。
setPlan 设置全局路径
1
2
3
4
5
6
7
8
9
10
11
bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if (! isInitialized()) {
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
//when we get a new plan, we also want to clear any latch we may have on goal tolerances
latchedStopRotateController_.resetLatching();
ROS_INFO("Got new plan");
return dp_->setPlan(orig_global_plan);
}
重置了公差。传给dp的setPlan函数。又传给了planner_util_的setPlan函数：
1
return planner_util_->setPlan(orig_global_plan);
在LocalPlannerUtil类中，赋给了global_plan_：
1
2
3
//reset the global plan
global_plan_.clear();
global_plan_ = orig_global_plan;
LocalPlannerUtil类储存局部规划器的配置参数。
isGoalReached 判断是否到达目标点
1
2
3
4
5
6
if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) {
ROS_INFO("Goal reached");
return true;
} else {
return false;
}
调用LatchedStopRotateController类的isGoalReached函数：
判断直线距离是否小于误差xy_goal_tolerance：
1
base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance
判断偏航是否小于误差yaw_goal_tolerance：
1
fabs(angle) <= limits.yaw_goal_tolerance
判断速度小于trans_stopped_vel：
1
base_local_planner::stopped(base_odom, theta_stopped_vel, trans_stopped_vel)
这三个条件都满足，返回真，move_base发送零速度和重置状态。否则，move_base调用函数computeVelocityCommands计算速度指令
computeVelocityCommands 计算控制速度
将全局路径中的部分路径点映射到局部地图中，（半个local costmap的、不能离机器人太近的）。并更新对应的打分项。映射大致示意图：
1
2
3
4
5
6
7
// costs for going away from path
path_costs_.setTargetPoses(global_plan_);
// costs for not going towards the local goal as much as possible
goal_costs_.setTargetPoses(global_plan_);
// alignment costs
然后，利用LatchedStopRotateController::isPostionReached函数判断是否已经到达目标位置，如果是，则利用LatchedStopRotateController::computeVelocityCommandsStopRotate函数计算对应的减速停止或者旋转至目标朝向的速度指令（取决于是否机器人已经停稳，进而决定实现减速停止或者旋转至目标朝向），否则利用DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel)计算DWA局部路径速度。
dwaComputeVelocityCommands
这个函数调用dwa的findBestPath：
1
base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);
DWAPlanner::findBestPath
简介：给定当前机器人的状态：位姿，速度，得到一个好的轨迹。
调用SimpleTrajectoryGenerator类的函数initialise，该函数内使用了三个VelocityIterator类，来迭代xy速度和theta角。
1
2
3
4
5
6
7
8
9
10
11
12
13
for(; !x_it.isFinished(); x_it++) {
vel_samp[0] = x_it.getVelocity();
for(; !y_it.isFinished(); y_it++) {
vel_samp[1] = y_it.getVelocity();
for(; !th_it.isFinished(); th_it++) {
vel_samp[2] = th_it.getVelocity();
//ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
sample_params_.push_back(vel_samp);
}
th_it.reset();
}
y_it.reset();
}
VelocityIterator类在构造时，就会根据每个量的采样个数和最大最小值，计算出每个值，存放在vector类型的samples_中。重载了运算符++，给内部维护的变量current_index加1。
例如，x方向的速度最大最小值为0.5，0.1，采样个数为5。，则samples_数组中应为：0.1,0.2,0.3,0.4,0.5。
将这些三个维度的遍历量添加到数组sample_params_中。如果xytheta的采样个数为5，1，20，则这个数组中有5x1x20=100个。
调用SimpleScoredSamplingPlanner类的函数findBestTrajectory，传入轨迹类和轨迹类数组指针。
该类中维护了一个数组：gen_list_
1
std::vector<TrajectorySampleGenerator*> gen_list_;
存放了指向TrajectorySampleGenerator的指针。查看该类的构造函数，可以看出是在构造的时候传入的。在DWAPlanner类中，有如下调用：
1
2
3
4
5
// trajectory generators
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
generator_list.push_back(&generator_);
// 把轨迹生成器的指针给加进去，传给了SimpleScoredSamplingPlanner类
scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
可见，通过上述这种类之间传递指针的方法，使得SimpleScoredSamplingPlanner能够使用TrajectorySampleGenerator类的数据，而后者恰是上面生成的轨迹空间。
有一句：gen_success = gen_->nextTrajectory(loop_traj);根据速度空间生成轨迹。这个函数里面调用：
1
2
3
4
5
6
7
if (generateTrajectory(
pos_,
vel_,
sample_params_[next_sample_index_],
comp_traj)) {
result = true;
}
而函数：SimpleTrajectoryGenerator::generateTrajectory，输入机器人位姿、当前速度、速度空间的速度和轨迹指针，这个函数是生成轨迹的核心函数。怎么计算轨迹的，继续看： 计算了步长：num_steps = ceil(sim_time_ / sim_granularity_); 通过一个循环计算轨迹：
1
2
3
4
5
6
7
8
9
10
for (int i = 0; i < num_steps; ++i) {
// 将点加入轨迹数组
traj.addPoint(pos[0], pos[1], pos[2]);
// 如果加速度连续，即引入加速度。根据加速度计算下一时刻的速度
if (continued_acceleration_) {
loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
}
// 根据速度loop_vel计算下一刻的位姿pos
pos = computeNewPositions(pos, loop_vel, dt);
}
计算速度的公式： \(\begin{matrix} max(sample,v-a*\delta t)\\ or \\ min(sample,v+a*\delta t) \end{matrix}\) 计算位姿的公式： \(\begin{matrix} x_{i+1}=x_i+(vx*cos(\theta)+vy*cos(\pi/2+\theta))*\delta t \\ y_{i+1}=y_i+(vx*sin(\theta)+vy*sin(\pi/2+\theta))*\delta t \\ \theta_{i+1} = \theta_i + v\theta*\delta t \end{matrix}\)
轨迹计算完以后，返回到findBestTrajectory函数中，下一步计算评分：loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);遍历每个评价器：评价器的定义在这里
1
2
3
4
5
6
7
8
std::vector<base_local_planner::TrajectoryCostFunction*> critics;
critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1) 放弃振荡运动(赋值cost -1)
critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles 丢弃移动到障碍物的轨迹
critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal 更倾向于使机头朝向(局部)机头目标的轨迹
critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path 更喜欢保持机器人鼻子在鼻子路径上的轨迹
critics.push_back(&path_costs_); // prefers trajectories on global path 更喜欢全球路径上的轨迹
critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation 基于波的传播，倾向于走向(局部)目标的轨迹
critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin 可以选择不旋转的轨迹
评价器的权重：
| 评价器 | 解释 | 权重 | rqt中的数值 | |
|---|---|---|---|---|
path_costs_ | path_distance_bias_ | 0.6 | ||
oscillation_costs_ | oscillation_reset_dist 和oscillation_reset_angle | 0.05，0.2 | ||
obstacle_costs_ | occdist_scale_ | 0.01 | ||
goal_front_costs_ | goal_distance_bias_ | 0.8 | ||
alignment_costs_ | path_distance_bias_ | 0.6 | ||
goal_costs_ | goal_distance_bias_ | 0.8 | ||
twirling_costs_ | twirling_scale | 0.0 |
然后调用每个评价器中的函数scoreTrajectory计算得分。 double cost = score_function_p->scoreTrajectory(traj); 具体怎么实现的就先不看了，每个评价器的原理大概是：
- Obstacle_costs 轨迹上是否存在障碍物以及距离障碍物的距离
- Path_costs 轨迹上点距离局部参考路径最近距离
- Goal_costs 轨迹上点距离局部参考路径终点最近距离
返回所有评价器的总和，loop_traj_cost， 遍历所有的轨迹，直到遍历完或超过指定的最大采样值max_samples_。 将最优轨迹赋给traj，findBestTrajectory函数到这里即结束了。
1
2
3
4
5
6
7
8
9
10
11
12
if (best_traj_cost >= 0) {
traj.xv_ = best_traj.xv_;
traj.yv_ = best_traj.yv_;
traj.thetav_ = best_traj.thetav_;
traj.cost_ = best_traj_cost;
traj.resetPoints();
double px, py, pth;
for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
best_traj.getPoint(i, px, py, pth);
traj.addPoint(px, py, pth);
}
}
可见，轨迹包括，xv，yv，thetav，cost，和路径点数组px,py,pth
到这里。DWAPlanner::findBestPath再将最优轨迹的xv和yv赋给期望速度，就结束了。
1
2
3
4
5
6
drive_velocities.pose.position.x = result_traj_.xv_;
drive_velocities.pose.position.y = result_traj_.yv_;
drive_velocities.pose.position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, result_traj_.thetav_);
tf2::convert(q, drive_velocities.pose.orientation);
回到DWAPlannerROS::dwaComputeVelocityCommands函数中，判断得到的最优轨迹代价值，如果小于0，则该函数会返回false，从而引起move_base进行进一步的处理，如重新规划、recovery等。
如果大于0， 将速度赋给cmd_vel，速度指令一直回传给move_base再发布。将路径点赋给local_plan，然后发布路径。然后回到DWAPlannerROS::computeVelocityCommands函数中，再发布一次全局的 publishGlobalPlan(transformed_plan);
评价器
dwa里面使用了好几个评价器，比较重要的有三个path_costs_,goal_costs_,obstacle_costs_。前两个是base_local_planner::MapGridCostFunction类，障碍物的是base_local_planner::ObstacleCostFunction。这三个类都继承自接口类：
1
2
class MapGridCostFunction: public base_local_planner::TrajectoryCostFunction
class ObstacleCostFunction : public TrajectoryCostFunction // 这里已经定义过命名空间了
主要目的是返回一个轨迹的代价（得分）。 类中主要的成员变量：
1
2
3
4
5
6
// 接收全局路径点
std::vector<geometry_msgs::PoseStamped> target_poses_;
// 局部代价地图指针
costmap_2d::Costmap2D* costmap_;
// 栅格地图类
base_local_planner::MapGrid map_;
MapGrid类是对栅格地图进行具体操作的类，里面的成员变量是，存放了整张代价地图的栅格。注意这里的代价地图是局部的，因此数量也不会很夸张。例如局部代价地图是2mx2m，分辨率为0.05m，则尺寸为40x40=1600个栅格。
1
std::vector<MapCell> map_;
按照评价器的调用顺序来看：
- 构造：路径评价器传入代价地图指针，其余为默认值。
1 2
path_costs_(planner_util->getCostmap()), goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
- 在函数
updatePlanAndLocalCosts中，对评价器设置局部路径(transformed_plan)。1
path_costs_.setTargetPoses(global_plan_);
- 在dwaplanner.cpp中就没有调用了，进入
findBestTrajectory函数，在评价器的迭代器中继续调用，prepare()函数1 2 3 4
// 截取部分 std::vector<TrajectoryCostFunction*>::iterator loop_critic; TrajectoryCostFunction* loop_critic_p = *loop_critic; loop_critic_p->prepare();
对于
path_cost_，使用map_.setTargetCells，对于goal_costs_，使用map_.setLocalGoal。setTargetCells我看懂了，就是从第一个路径点开始，更新地图上的栅格，每个栅格中的距离信息。个人理解：绿色为第一个路径点，蓝色为障碍物，max=size^2
- 然后在
SimpleScoredSamplingPlanner::scoreTrajectory中计算得分。调用评价器的scoreTrajectory函数。1
double cost = score_function_p->scoreTrajectory(traj);
- 进入评价器的计算得分函数，aggregationType默认为Last。
计算出来最后一个点的距离信息。当根据mapgrid中的值对轨迹进行评分时，我们可以返回最后一个点的值(如果之前的点没有返回碰撞)，所有点的和，或者所有(非零)点的乘积。如果跳出地图，返回-4，如果有碰撞，返回-3，如果不可达，返回-2。
ObstacleCostFunction
06-20 16:45： dwa ObstacleCostFunction:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
traj.getPoint(i, px, py, pth);
double f_cost = footprintCost(px, py, pth,
scale, footprint_spec_,
costmap_, world_model_);
if(f_cost < 0){
return f_cost;
}
if(sum_scores_)
cost += f_cost;
else
cost = std::max(cost, f_cost);
}
对路径上的每一个计算一次footprintCost，这个函数输入当前的位姿、比例因子、机器人轮廓、costmap模型。在进去就是带有比例因子的点：
1
double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint);
需要注意这里是基类指针，实际调用的是CostmapModel对象：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
double
CostmapModel::footprintCost(const geometry_msgs::Point &position,
const std::vector<geometry_msgs::Point> &footprint,
double inscribed_radius,
double circumscribed_radius) {
// returns:
// -1 if footprint covers at least a lethal obstacle cell, or
// -2 if footprint covers at least a no-information cell, or
// -3 if footprint is [partially] outside of the map, or
// a positive value for traversable space
...
// we need to rasterize each line in the footprint
for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
// get the cell coord of the first point
if (!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
return -3.0;
// get the cell coord of the second point
if (!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1,
y1))
return -3.0;
line_cost = lineCost(x0, x1, y0, y1);
footprint_cost = std::max(line_cost, footprint_cost);
// if there is an obstacle that hits the line... we know that we can
// return false right away
if (line_cost < 0)
return line_cost;
}
先判断每个点是否在地图上。再进入一个循环，两两之间进行连线。这里使用了一个直线迭代器LineIterator，输入直线两个端点的坐标(int)，遍历这个直线上的所有点，遍历方法封装的比较好：
1
for (LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance())
advance这个方法里面每次运行会使x和y按照直线方向加1。算法这里不关心，主要看ROS的costmap层面。
1
2
3
4
5
6
7
8
9
10
double CostmapModel::pointCost(int x, int y) const {
unsigned char cost = costmap_.getCost(x, y);
// if the cell is in an obstacle the path is invalid
if (cost == NO_INFORMATION)
return -2;
if (cost == LETHAL_OBSTACLE)
return -1;
return cost;
}
对直线上的每个点都计算一次，如果是未知则返回-2，致命障碍物则返回-1。所以也就完成了注释中强调的：
1
2
3
4
5
// returns:
// -1 if footprint covers at least a lethal obstacle cell, or
// -2 if footprint covers at least a no-information cell, or
// -3 if footprint is [partially] outside of the map, or
// a positive value for traversable space
a是所有连线里面costmap最大的值。 所有double ObstacleCostFunction::scoreTrajectory这个函数，返回和上面一样，如果不是负的，返回的是最大值。