costmap_2d
在move_base的框架下，costmap_2d为全局规划器和局部规划器提供了2D的代价地图。 costmap_2d使用的是占用删格地图，通过多个图层描述环境信息。每个图层描述了一种类型的信息，最终的代价是这些图层叠加的结果。比如说静态地图层(static map layer)描述的是导航的地图信息， 障碍物层(obstacle layer)则记录了环境中的障碍物，膨胀层(inflation layer)根据用户指定的参数和机器人的尺寸将障碍物的占用删格区域放大一部分，以防止碰撞。
Costmap2D实现了存储和访问代价地图的基本数据结构，Costmap2DROS则是对Costmap2D对象的ROS封装。它订阅了传感器数据和地图信息的主题，实时的更新各个图层，最终提供一个2D的代价地图。
本文中，我们分析Costmap2D及其封装Costmap2DROS的源码和业务流程。
1. 代价地图的ROS封装——Costmap2DROS
下面是Costmap2DROS的构造函数，它有两个参数，name是Costmap的命名，tf则是坐标变换对象。在进入构造函数之前，先对大量的成员变量进行了初始化。 一些主要的成员变量的数据类型和以及含义参见表 1。
Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf)
: layered_costmap_(NULL), name_(name), tf_(tf), transform_tolerance_(0.3), map_update_thread_shutdown_(false),
stop_updates_(false), initialized_(true), stopped_(false), robot_stopped_(false), map_update_thread_(NULL),
last_publish_(0), plugin_loader_("costmap_2d", "costmap_2d::Layer"), publisher_(NULL), dsrv_(NULL),
footprint_padding_(0.0) {
为了写文章方便，我们对构造函数的语句做一些调整。首先创建ROS的句柄，并获取必要的运行参数。
ros::NodeHandle private_nh("~/" + name);
ros::NodeHandle g_nh;
// 获取全局和机器人坐标系名称
private_nh.param("global_frame", global_frame_, std::string("map"));
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
// 关于滑动窗口
bool rolling_window, track_unknown_space, always_send_full_costmap;
private_nh.param("rolling_window", rolling_window, false);
private_nh.param("track_unknown_space", track_unknown_space, false);
private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
接下来，我们需要确认全局坐标系和机器人坐标系之间的变换是可用的。这里我们调用坐标变换对象tf_的canTransform来查询变换状态，用last_error计时，每隔5秒打印一个警告信息。
ros::Time last_error = ros::Time::now();
std::string tf_error;
while (ros::ok() && !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &tf_error)) {
ros::spinOnce();
if (last_error + ros::Duration(5.0) < ros::Time::now()) {
ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = ros::Time::now();
}
tf_error.clear();
}
然后根据参数plugins加载图层。默认情况下amcl_demo中加载的全局代价地图是由静态地图层、 障碍层和膨胀层构成，如下面右边的代码片段所示。而局部代价地图只有障碍层和膨胀层。在costmap的框架中，各种类型的图层都是以插件的形式加载的。 各个图层都将记录在图层管理器layered_costmap_中。
|
|
下面创建关于机器人脚印，或者说是机器人位姿，的主题订阅器和发布器。
std::string topic_param, topic;
if (!private_nh.searchParam("footprint_topic", topic_param))
topic_param = "footprint_topic";
private_nh.param(topic_param, topic, std::string("footprint"));
footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);
if (!private_nh.searchParam("published_footprint_topic", topic_param))
topic_param = "published_footprint";
private_nh.param(topic_param, topic, std::string("oriented_footprint"));
footprint_pub_ = private_nh.advertise("footprint", 1);
完成系统的一些状态变量的初始化。
tf2::toMsg(tf2::Transform::getIdentity(), old_pose_.pose);
setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
always_send_full_costmap);
stop_updates_ = false;
initialized_ = true;
stopped_ = false;
最后，我们创建一个计时器用于判断机器人是否在移动。并完成在线重配置参数系统的初始化操作。
robot_stopped_ = false;
timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name));
dynamic_reconfigure::Server::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,
dsrv_->setCallback(cb);
}
下表1中列举了Costmap2DROS的一些主要成员变量。
表 1. Costmap2DROS的主要成员变量
| 变量名称 | 数据类型 | 说明 |
|---|---|---|
| layered_costmap_ | costmap_2d::LayeredCostmap * | 图层管理器，记录了代价地图的各个图层对象，并提供融合各个图层数据的接口。 |
| name_ | std::string | Costmap2DROS对象的名称，用于获取ROS的接口。 |
| global_frame_ | std::string | 全局坐标系名称。 |
| robot_base_frame_ | std::string | 机器人坐标系名称。 |
| initialized_ | bool | 用于标记代价地图是否已经初始化，更新地图线程是否正常工作。 |
| stop_updates_ | bool | 用于标记是否停止更新代价地图，与图层的工作状态无关。 |
| stopped_ | bool | 用于标记是否关闭了各个图层。 |
| robot_stopped_ | bool | 机器人是否停止运动。 |
| map_update_thread_shutdown_ | bool | 是否终止更新地图的线程。 |
| tf_ | tf2_ros::Buffer & | 坐标变换系统tf2对象，提供获取系统中各个坐标系之间的变换关系的接口。 |
| timer_ | ros::Timer & | 用于判定机器人是否在移动的计时器。 |
| publish_cycle | ros::Duration | 发布代价地图的周期计数器。 |
| map_update_thread_ | boost::thread* | 用于更新代价地图的线程。 |
Costmap2DROS提供了start, pause, stop, resume接口，用于维护一个状态机，控制各个图层以及Costmap2DROS对象更新代价地图。下面是start的接口定义， 其中的3到7行中根据成员变量stopped_判定关闭了各个图层，若关闭了则打开之。然后重置stop_updates_的状态，开始更新代价地图。最后通过一个while循环等待initialized_置位。 initialized_在成员函数updateMap中被置位，它标志着更新地图的线程正常运行。
void Costmap2DROS::start() {
std::vector<boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
if (stopped_) {
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end(); ++plugin)
(*plugin)->activate();
stopped_ = false;
}
stop_updates_ = false;
ros::Rate r(100.0);
while (ros::ok() && !initialized_)
r.sleep();
}
下面是stop的函数实现，它与start是一对儿对称操作，用于关闭各个图层。
void Costmap2DROS::stop() {
stop_updates_ = true;
std::vector<boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
for (std::vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end(); ++plugin)
(*plugin)->deactivate();
initialized_ = false;
stopped_ = true;
}
下面左右两边的函数分别是用于暂停更新代价地图和恢复的接口，它们不操作图层对象。
|
|
Costmap2DROS使用一个线程来更新代价地图。该线程对象在重配置参数系统的回调函数reconfigureCB中以如下的语句得到构建。其中参数mapUpdateLoop是该线程的执行函数， 而map_update_frequency指示了更新地图的频率。
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
下面是更新地图线程执行函数mapUpdateLoop的代码片段，其输入参数就是更新地图的频率。我们首先创建ROS的句柄对象nh和频率计时器r，并在一个while循环中通过句柄nh检查节点是否正常运行， 以及状态变量map_update_thread_shutdown_是否指示退出更新进程。
void Costmap2DROS::mapUpdateLoop(double frequency) {
ros::NodeHandle nh;
ros::Rate r(frequency);
while (nh.ok() && !map_update_thread_shutdown_) {
在循环中，通过成员函数updateMap完成地图更新的工作。并填充地图发布器，同时发布代价地图。
updateMap();
if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized()) {
unsigned int x0, y0, xn, yn;
layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
publisher_->updateBounds(x0, xn, y0, yn);
ros::Time now = ros::Time::now();
if (last_publish_ + publish_cycle < now) {
publisher_->publishCostmap();
last_publish_ = now;
}
}
最后通过频率计数器r消耗掉一个更新周期中剩余的时间，如果发现时间偏差比较大就给出警告。
r.sleep();
if (r.cycleTime() > ros::Duration(1 / frequency))
ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency, r.cycleTime().toSec());
}
}
实际更新地图的过程是在成员函数updateMap中完成的，在该函数中根据状态变量stop_updates_决定是否更新地图。在更新过程中，首先获取机器人当前的位姿。 然后通过图层管理器layered_costmap_更新各个图层的代价地图。最后填充机器人脚印消息并发布，同时设置状态变量initialized_为true标志着至少完成了一次代价地图的更新。
void Costmap2DROS::updateMap() {
if (!stop_updates_) {
geometry_msgs::PoseStamped pose;
if (getRobotPose (pose)) {
double x = pose.pose.position.x;
double y = pose.pose.position.y;
double yaw = tf2::getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw);
geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
footprint.header.stamp = ros::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint);
footprint_pub_.publish(footprint);
initialized_ = true;
}
}
}
下面我们来分析一下图层管理器的源码。
2. 图层管理器——LayeredCostmap
在Costmap2DROS中我们以插件的形式加载图层，并使用LayeredCostmap类型的对象管理这些图层。下面的两行程序是LayeredCostmap的两个私有变量，plugins_记录了所有的图层， costmap_则是各个图层的数据叠加之后的代价地图对象。
std::vector<boost::shared_ptr<Layer> > plugins_;
Costmap2D costmap_;
下面是图层管理器更新地图的成员函数，它的三个参数分别是机器人的x,y位置和方向角。在函数的一开始，我们先对代价地图对象加锁，以保证地图数据更新的原子性。
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) {
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
如果配置需要随着机器人的移动滑动代价地图，我们需要重新计算地图的原点，并更新地图对象costmap_。
if (rolling_window_) {
double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
costmap_.updateOrigin(new_origin_x, new_origin_y);
}
如果没有图层，就不需要继续更新了。
if (plugins_.size() == 0)
return;
接下来更新地图边界。
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) {
double prev_minx = minx_;
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
}
获取更新了边界之后的地图栅格原点和尺寸。
int x0, xn, y0, yn;
costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
x0 = std::max(0, x0);
xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
y0 = std::max(0, y0);
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
最后，重置地图并重新计算各个栅格的代价，并标识地图更新状态。
costmap_.resetMap(x0, y0, xn, yn);
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) {
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
bx0_ = x0; bxn_ = xn; by0_ = y0; byn_ = yn;
initialized_ = true;
}
3. Costmap2D
下面的代码片段列举了类型Costmap2D的成员变量，它们记录了代价地图的原点、尺寸、分辨率、存储空间等信息。Costmap2D还提供了各种接口函数用于就获取这些信息。
class Costmap2D {
public:
typedef boost::recursive_mutex mutex_t;
private:
mutex_t* access_; // 信号量
protected:
unsigned int size_x_; // x轴尺寸
unsigned int size_y_; // y轴尺寸
double resolution_; // 分辨率
double origin_x_; // x轴原点坐标
double origin_y_; // y轴原点坐标
unsigned char* costmap_; // 存储空间
unsigned char default_value_; // 默认填充值
};
3. 完
本文中我们分析了Costmap2D及其封装Costmap2DROS的源码和业务流程。了解到Costmap2D使用图层的形式来描述不同的地图信息，包括静态的原始地图，动态的障碍物， 以及防止机器人发生碰撞的膨胀层。