dwa_local_planner
包dwa_local_planner中的DWAPlannerROS是amcl_demo默认的局部规划器。 它是对nav_core中用虚类BaseLobalPlanner所定义的接口的实现。DWAPlannerROS只是一个针对move_base框架的封装，具体的实现由DWAPlanner完成。 DWAPlanner使用一种称为动态窗口路径(Dynamic Window Approach)的方法进行局部的路径规划， 跟踪全局规划器输出的全局轨迹，依据机器人的动态特性，和周围障碍物特征，生成实际控制机器人运动的速度指令。
DWA算法的基本思想是，在机器人的控制空间(线速度、角速度指令)中离散地采样，对于每个样本指令，在机器人当前状态和地图信息的基础上，推演未来很短的一段时间内的运动轨迹。 根据发生碰撞的可能性、目标点的接近程度、全局轨迹的跟踪近似度、速度限制等多方面的指标评价这些轨迹并打分，选取得分最高的轨迹，将其对应的指令下发给底盘，控制机器人运动。
实际上DWAPlanner也不是DWA算法的具体实现，它只是包base_local_planner一个封装而已。 本文分析DWAPlannerROS和DWAPlanner相关的源码，旨在了解DWA算法运行的基本过程，以及融合到move_base框架下的必要操作。在详细分析这两个类之前，让我们先来看一下BaseLobalPlanner的定义。
1. 接口——BaseLocalPlanner
BaseLobalPlanner是move_base获取局部规划能力的接口， 它是定义在包nav_core的头文件base_lobal_planner.h中的一个虚类， 其类定义如下:
class BaseLobalPlanner {
public:
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
virtual bool isGoalReached() = 0;
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
virtual void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
virtual ~BaseLocalPlanner(){}
protected:
BaseLocalPlanner(){}
};
从类定义可以看到，BaseLocalPlanner中定义了四个接口函数：
- computeVelocityCommands: move_base通过该接口获取局部规划器输出的机器人控制速度指令。
- isGoalReached: 用于判定是否到达目标点。
- setPlan: 用于设定全局规划的路径。
- initialize: 用于初始化局部规划器。
2. 封装——DWAPlannerROS
DWAPlannerROS是接口BaseLobalPlanner的一种实现，是对局部规划器DWAPlanner的封装。下面是DWAPlannerROS的类定义片段，可以看到它继承自BaseLobalPlanner，并实现了上述的四个接口函数。
class DWAPlannerROS : public nav_core::BaseLocalPlanner {
public:
void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
bool isGoalReached();
// 省略其它成员
};
2.1 初始化
下面是DWAPlannerROS的初始化函数，它有三个参数。其中name是规划器的名称，tf是系统坐标变换器， costmap_ros是进行局部路径规划需要参考的代价地图对象。tf有tf和tf2两个库，两者的API有比较大的差异，ROS官方希望通过tf2替代tf。在18年7月份发布的1.16版的navigation之后就改用tf2了。
void DWAPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) {
if (!isInitialized()) {
// 初始化
initialized_ = true;
} else
ROS_WARN("This planner has already been initialized, doing nothing.");
}
在初始化时，首先构建一个ROS句柄，在根据此句柄发布主题"global_plan"和"local_plan"。
ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise("global_plan", 1);
l_plan_pub_ = private_nh.advertise("local_plan", 1);
接下来，用DWAPlannerROS的成员变量记录下坐标变换对象tf，代价地图costmap_ros_。并据此构建了规划器辅助对象planner_util_， 这是一个定义在包base_local_planner中的类LocalPlannerUtil，主要用于维护局部规划器的各种限制参数。 dp_则是实际的DWA规划器对象，它提供了局部规划算法。
tf_ = tf;
costmap_ros_ = costmap_ros;
costmap_ros_->getRobotPose(current_pose_);
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());
dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));
下面，通过ROS句柄对象，获取里程计主题，并设置里程计辅助类对象odom_helper_。 它是一个在包base_local_planner中定义的辅助类OdometryHelperRos。
if (private_nh.getParam( "odom_topic", odom_topic_))
odom_helper_.setOdomTopic(odom_topic_);
然后，标记已经完成初始化。源码中还有一些关于参数重命名和参数动态配置的Waring和初始化操作，它们与路径规划的业务流程无关，这里略去。
initialized_ = true;
2.2 业务逻辑
move_bae通过接口computeVelocityCommands进行局部路径规划，下面代码片段是DWAPlannerROS的实现。它有一个参数cmd_vel，这是一个输出参数，用于输出局部规划器计算的机器人控制指令。
bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
在函数一开始先通过代价地图对象获取机器人的位姿。
if (!costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
然后通过planner_util_对象的getLocalPlan接口，在全局轨迹中截取从当前位置到终点之间的路径，用局部变量transformed_plan记录。getLocalPlan除了截取剩余轨迹之外， 还会将全局轨迹点转换到代价地图的坐标系下。所以我猜这个局部变量的前缀transformed就是用来描述这一特性的。
std::vector<geometry_msgs::PoseStamped> transformed_plan;
if (!planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
ROS_ERROR("Could not get local plan");
return false;
}
如果剩余的路径是空的，就没有继续规划的必要了。
if(transformed_plan.empty()) {
ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
return false;
}
更新规划器的轨迹和局部代价。
dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());
然后通过辅助对象latchedStopRotateController_判定是否到达了终点。 这个辅助对象是包base_local_planner中定义的辅助类LatchedStopRotateController。 如果我们到达了终点，就发布空路径，同时通过辅助对象控制机器人停下来。
if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
std::vector<geometry_msgs::PoseStamped> local_plan;
std::vector<geometry_msgs::PoseStamped> transformed_plan;
publishGlobalPlan(transformed_plan);
publishLocalPlan(local_plan);
base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
return latchedStopRotateController_.computeVelocityCommandsStopRotate(cmd_vel, limits.getAccLimits(), dp_->getSimPeriod(),
&planner_util_, odom_helper_, current_pose_, boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
}
没有到达终点，我们就通过成员函数dwaComputeVelocityCommands计算速度控制指令，并发布路径。
else {
bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
if (isOk) {
publishGlobalPlan(transformed_plan);
} else {
ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
std::vector empty_plan;
publishGlobalPlan(empty_plan);
}
return isOk;
}
}
2.3 局部路径规划
DWAPlannerROS通过成员函数dwaComputeVelocityCommands完成局部的路径规划。该函数有两个参数，通过参数global_pose输入机器人当前位姿，通过cmd_vel是输出机器人速度控制指令。
bool DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped &global_pose, geometry_msgs::Twist& cmd_vel) {
首先确认局部规划器已经初始化了，若否则报错退出。
if(!isInitialized()){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
然后根据里程计获取机器人当前的运动速度。
geometry_msgs::PoseStamped robot_vel;
odom_helper_.getRobotVel(robot_vel);
接着，通过规划器dp_进行仿真并筛选出最优的局部路径。findBestPath调用结束之后，drive_cmds中将记录机器人的速度控制指令。该函数还将返回局部路径，这里将之记录在局部变量path中。
geometry_msgs::PoseStamped drive_cmds;
drive_cmds.header.frame_id = costmap_ros_->getBaseFrameID();
base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);
最后更新控制指令drive_cmds到输出参数cmd_vel中。
cmd_vel.linear.x = drive_cmds.pose.position.x;
cmd_vel.linear.y = drive_cmds.pose.position.y;
cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);
仅就机器人的控制而言，更新了cmd_vel，故事就已经结束了。下面的这些操作，只是在ROS的环境下发布局部的运动轨迹。局部变量path的成员变量cost_记录了局部路径的代价，如果它是一个负值， 表示没有规划出一条合理的局部轨迹，我们将发布一条空路径。
std::vector<geometry_msgs::PoseStamped> local_plan;
if(path.cost_ < 0) {
local_plan.clear();
publishLocalPlan(local_plan);
return false;
}
下面根据path的数据填充局部轨迹消息并发布。
for(unsigned int i = 0; i < path.getPointsSize(); ++i) {
double p_x, p_y, p_th;
path.getPoint(i, p_x, p_y, p_th);
geometry_msgs::PoseStamped p;
p.header.frame_id = costmap_ros_->getGlobalFrameID();
p.header.stamp = ros::Time::now();
p.pose.position.x = p_x;
p.pose.position.y = p_y;
p.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, p_th);
tf2::convert(q, p.pose.orientation);
local_plan.push_back(p);
}
publishLocalPlan(local_plan);
return true;
}
| 对象名称 | 数据类型 | 说明 |
|---|---|---|
| oscillation_costs_ | OscillationCostFunction | 尽量降低机器人在原地晃动的情况。 |
| obstacle_costs_ | ObstacleCostFunction | 防止机器人撞到障碍物上。 |
| path_costs_ | MapGridCostFunction | 使机器人尽可能的贴近全局轨迹。 |
| goal_costs_ | MapGridCostFunction | 更倾向于选择接近目标点的轨迹。 |
| goal_front_costs_ | MapGridCostFunction | 尽可能的让机器人朝向局部的nose goal。 |
| alignment_costs_ | MapGridCostFunction | 尽可能的让机器人保持在nose path上。 |
| twirling_costs_ | TwirlingCostFunction | 尽量不让机器人原地打转。 |
3. 实现——DWAPlanner
DWAPlanner并不具体实现局部路径规划算法。它使用包base_local_planner中的SimpleTrajectoryGenerator来生成预测轨迹， 并为预测轨迹定义了一系列的评价标准。实际的规划则是由SimpleScoredSamplingPlanner进行的。
右边是它所用的各个评价对象，它们都是在base_local_planner中实现的，具有相同的基类TrajectoryCostFunction。
3.1 构造函数和初始化
下面是DWAPlanner的构造函数，它有两个参数。其中name是规划器的名称，根据它我们可以从ROS环境中获取一些关于规划器的配置。planner_util则是规划器辅助对象， 它为规划器提供了代价地图、坐标变换、全局规划等算法输入。并为一些成员变量赋予了初值。
DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :
planner_util_(planner_util),
obstacle_costs_(planner_util->getCostmap()),
path_costs_(planner_util->getCostmap()),
goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
alignment_costs_(planner_util->getCostmap()) {
首先，我们构建ROS句柄，并据此获取相关的运行参数。
ros::NodeHandle private_nh("~/" + name);
private_nh.param("global_frame_id", frame_id_, std::string("odom"));
// 省略其它参数获取的语句
然后构建一些可视化的消息发布器和对象，它们主要用于在rviz工具中显示轨迹信息和潜在的代价，不必深究。
traj_cloud_pub_ = private_nh.advertise<sensor_msgs::PointCloud2>("trajectory_cloud", 1);
map_viz_.initialize(name, planner_util->getGlobalFrame(),
boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6));
接下来设置一些必要的准则对象属性:
goal_front_costs_.setStopOnFailure( false );
alignment_costs_.setStopOnFailure( false );
oscillation_costs_.resetOscillationFlags();
obstacle_costs_.setSumScores(sum_scores);
并一个vector容器装载所有的准则对象:
std::vector<base_local_planner::TrajectoryCostFunction*> critics;
critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns 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
构建轨迹生成器列表:
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
generator_list.push_back(&generator_);
最后，根据准则对象列表和轨迹生成器列表，构建局部规划器:
scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
}
3.2 更新轨迹和局部代价
在分析DWAPlannerROS的源码时，我们看到在生成速度控制指令，发布局部轨迹之前，先调用了DWAPlanner的成员函数updatePlanAndLocalCosts更新轨迹和局部代价。 下面是该函数的代码片段，它有三个参数。其中global_pose是当前机器人在全局坐标系下的位姿，new_plan则是在DWAPlannerROS中截断后的全局剩余路径， footprint_spec则是由代价地图提供的机器人脚印。
void DWAPlanner::updatePlanAndLocalCosts(const geometry_msgs::PoseStamped& global_pose,
const std::vector<geometry_msgs::PoseStamped>& new_plan,
const std::vector<geometry_msgs::Point>& footprint_spec) {
首先更新全局轨迹。
global_plan_.resize(new_plan.size());
for (unsigned int i = 0; i < new_plan.size(); ++i)
global_plan_[i] = new_plan[i];
然后更新障碍物、路径、和目标相关的准则对象。
obstacle_costs_.setFootprint(footprint_spec);
path_costs_.setTargetPoses(global_plan_);
goal_costs_.setTargetPoses(global_plan_);
接着计算当前机器人到终点的距离，和方向角偏差。
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
double sq_dist = (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
(pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);
double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
沿着朝向终点的方向延长forward_point_distance_，更新准则对象goal_front_costs_。forward_point_distance_是DWAPlanner的一个配置参数。
std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x
+ forward_point_distance_ * cos(angle_to_goal);
front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y
+ forward_point_distance_ * sin(angle_to_goal);
goal_front_costs_.setTargetPoses(front_global_plan);
最后更新alignment_costs_。
if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {
alignment_costs_.setScale(pdist_scale_);
// costs for robot being aligned with path (nose on path, not ju
alignment_costs_.setTargetPoses(global_plan_);
} else {
// once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
alignment_costs_.setScale(0.0);
}
}
}
3.3 获取最优路径
更新了剩余路径和准则对象之后，我们就可以通过findBestPath获取一条较优的局部路径，并输出速度控制指令。下面是该函数的代码片段，它有三个参数。 其中global_pose和global_vel是两个输入量，分别描述了当前机器人的全局位姿和速度。drive_velocities则是一个输出参数，在函数退出之后，它将记录速度控制指令。 该函数的返回值是一条在各个准则对象综合评价下的最优路径。
在函数的一开始，对信号量configuration_mutex_加锁。这是为了防止move_base的动态配置参数的特性，与规划过程冲突。
base_local_planner::Trajectory DWAPlanner::findBestPath(const geometry_msgs::PoseStamped& global_pose,
const geometry_msgs::PoseStamped& global_vel, geometry_msgs::PoseStamped& drive_velocities) {
boost::mutex::scoped_lock l(configuration_mutex_);
下面构建三个Eigen的向量对象pos, vel, goal，分别记录了机器人当前的位姿、速度和目标点。 同时用一个局部变量limits记录机器人的各种运动学限制。
Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation));
Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y, tf2::getYaw(global_vel.pose.orientation));
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf2::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
根据这些临时变量初始化轨迹生成器generator_。其中参数vsamples_是DWAPlanner的一个成员变量Eigen::Vector3f vsamples_，
它的三个元素分别记录了x,y方向上和转角的指令采样数量。
generator_.initialise(pos, vel, goal, &limits, vsamples_);
下面通过实际的规划器scored_sampling_planner_对控制指令采样，并从中找出最优的局部路径。result_traj_是DWAPlanner的一个成员变量，它将在如下的操作之后记录最优的局部路径。 而临时vector对象all_explored则记录了所有的样本路径。
result_traj_.cost_ = -7;
std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
如果配置可视化轨迹样本和代价地图，则通过traj_cloud_pub_和map_viz_发布相关主题的消息，我们可以在rviz中订阅对应主题看到可视化的结果。
if (publish_traj_pc_) {
// 填充轨迹云消息
traj_cloud_pub_.publish(traj_cloud);
}
if (publish_cost_grid_pc_) {
map_viz_.publishCostCloud(planner_util_->getCostmap());
}
接着更新碰撞准则对象。
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_vel_trans);
如果result_traj_的字段cost_是一个负数，意味着我们没有成功规划出一条局部路径，此时需要控制机器人停下来。否则根据最优样本的线速度和角速度控制量输出控制指令。
if (result_traj_.cost_ < 0) {
drive_velocities.pose.position.x = 0;
drive_velocities.pose.position.y = 0;
drive_velocities.pose.position.z = 0;
drive_velocities.pose.orientation.w = 1;
drive_velocities.pose.orientation.x = 0;
drive_velocities.pose.orientation.y = 0;
drive_velocities.pose.orientation.z = 0;
} else {
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);
}
return result_traj_;
}
4. 完
本文中，我们分析了局部规划器DWAPlannerROS和DWAPlanner的源码，大体了解了其业务基本流程。真正的DWA算法在包base_local_planner中实现。 DWAPlannerROS提供了接入move_base框架的接口，DWAPlanner定义了评价轨迹优劣的准则对象。