4.2.4 costmap_2d工作过程
http://wiki.ros.org/costmap_2d/
4.2.0节描述了MoveBase启动过程中创建和初始化global planner的costmap的过程。
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause(); //先暂定运行
costmap的动态库位于 /opt/ros/kinetic/lib/libcostmap_2d.so
costmap会启动一个线程处理工作流程:costmap_2d::Costmap2DROS::mapUpdateLoop(double)
4.2.4.1 原理
costmap就是一张代价地图,地图数据是一个unsigned char数组,每一个点的值表示该点对应的实际坐标的通行性。 costmap坐标系的坐标是整数,世界坐标系的坐标是双浮点数,它们之间通过分辨率以及costmap左下角坐标进行转换。costmap对应的类为costmap_2d::Costmap2D。
ros利用插件机制实现了分层的costmap,每一层保存一张由该层对应的数据源生成的costmap,由各自的数据源实时更新,最后,各层的数据上下叠加在一起,就生成了最终的costmap。每一层的costmap为Costmap2D类型,最后叠加在一起的为LayeredCostmap类型,LayeredCostmap类里包含一个Costmap2D类型的成员变量。
4.2.4.2 类图
costmap_2d\include\costmap_2d\costmap_2d_ros.h
class Costmap2DROS
{
LayeredCostmap* layered_costmap_;
}
class LayeredCostmap
{
Costmap2D costmap_;
std::vector<boost::shared_ptr<Layer> > plugins_;
}
class Costmap2D
{
unsigned char* costmap_
}
class Layer
{
LayeredCostmap* layered_costmap_
}
class VoxelLayer : public ObstacleLayer{}
class ObstacleLayer : public CostmapLayer{}
class StaticLayer : public CostmapLayer{}
class CostmapLayer : public Layer, public Costmap2D{}
class InflationLayer : public Layer{}
4.2.4.3 Costmap2DROS初始化过程
初始化过程在构造函数中完成:costmap_2d\src\costmap_2d_ros.cpp
Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf)
主要工作有:
1) 初始化old_pose_
tf::Stamped<tf::Pose> old_pose_
old_pose_.setIdentity();
old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));
2) 读取frame参数global_frame和robot_base_frame
global_frame_=/map
robot_base_frame_=/base_footprint
变量名 | 变量类型 | 参数名 | 值 |
global_frame_ | std::string | global_frame | /map |
robot_base_frame_ | std::string | robot_base_frame | /base_footprint |
rolling_window | bool | rolling_window | false |
track_unknown_space | bool | track_unknown_space | false |
always_send_full_costmap | bool | always_send_full_costmap | false |
|
|
|
|
|
|
|
|
3)等待,直到有global_frame_到robot_base_frame_的TF变换
tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), ros::Duration(0.01), &tf_error)
4) 创建LayeredCostmap对象
LayeredCostmap* layered_costmap_;
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);//参数都为false
5) 加载plugins
读取plugins参数中指定的costmap plugin, 加载并初始化
这里配置的为:
Costmap_2d::StaticLayer
Costmap_2d::VoxelLayer
Costmap_2d::InflationLayer
- addPluin()会把layer plugin加到vector plugins_中。
std::vector<boost::shared_ptr<Layer> > plugins_
<class StaticLayer>
<class VoxelLayer>
<class InflationLayer >
- initialize()会调用相应layer的onInitialize()函数
每一个layer对象也有一个LayeredCostmap* layered_costmap_指针。会把此指针赋值为
第4)步Costmap2DROS中的LayeredCostmap* layered_costmap_对象
StaticLayer和VoxelLayer有自己的costmap。 InflationLayer没有自己的costmap,
它直接访问LayeredCostmap中的costmap
具体的每一个layer的初始化过程见4.2.4.4节
6) 订阅和发布topic /move_base/global_costmap/footprint
footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);
footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);
7) 设置robot的footprint
setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
从配置参数footprint或者robot_radius中读取
8) 创建Costmap2DPublisher
publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_,
"costmap",always_send_full_costmap);
此对象用于发布/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
9)创建定时器,每隔0.1秒判断robot是否处于运动中
// Create a time r to check if the robot is moving
robot_stopped_ = false;
timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
10)创建mapUpdateLoop线程
在Costmap2DROS::reconfigureCB()会创建线程。
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this,
map_update_frequency));
4.2.4.4 每层layer的初始化
Costmap2DROS调用父类Layer的initialize()进行初始化。Initialize()最终调用每一个子类的onInitialzie() 函数完成每一个layer自己的初始化。
plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
void Layer::initialize(LayeredCostmap* parent, std::string name, tf::TransformListener *tf)
{
layered_costmap_ = parent;
name_ = name;
tf_ = tf;
onInitialize();
}
4.2.4.4.1 StaticLayer
StaticLayer的初始化是在函数void StaticLayer::onInitialize()中
costmap_2d\plugins\static_layer.cpp
初始化过程为:
1) 读取配置参数
变量名字 | 类型 | 参数名字 | 值 |
map_topic | std::string | map_topic | /map |
first_map_only_ | bool | first_map_only | false |
subscribe_to_updates_ | bool | subscribe_to_updates | false |
track_unknown_space_ | bool | track_unknown_space | true |
use_maximum_ | bool | use_maximum | false |
temp_lethal_threshold | int | lethal_cost_threshold | 100 |
temp_unknown_cost_value | int | unknown_cost_value | -1 |
trinary_costmap_ | bool | trinary_costmap | true |
2) 订阅topic /map
/map topic是map_server发布。会一直在等待,直到有topic数据才继续后面的初始化工作。
map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
> 回调函数 StaticLayer::incomingMap()的工作:
/map topic的消息数据格式如下所示:
rosmsg show nav_msgs/OccupancyGrid
std_msgs/Header header
uint32 seq
time stamp
string frame_id
nav_msgs/MapMetaData info
time map_load_time
float32 resolution
uint32 width
uint32 height
geometry_msgs/Pose origin
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
int8[] data
>首先读取地图的宽度,高度和分辨率。
>与layered_costmap_的宽,高,分辨率,初始位置(x,y)比较,是否一致,如果不一致,则:
调用 layered_costmap_->resizeMap()重新初始化地图数据(根据size重新分配内存,并初始化)
调用各个layer的(*plugin)->matchSize():
(staticlayer:根据master map的尺寸重新分配内存并初始化
VoxelLayer:根据master map的尺寸重新分配内存并初始化,voxelgrid 初始化
InflationLayer:重新分配cache cost和distance内存,并计算值
)
如果一致,则只resizeMap() static layer自己的地图.
> 根据配置参数,转化每个像素的值到规定值
具体的转换规则定义在函数StaticLayer::interpretValue(unsigned char value)中。
入参value是map_server发布的地图数据值。
如果value等于unknown_cost_value_(-1),则返回255(因为track_unknown_space为true)
如果value大于等于lethal_cost_threshold(100), 则返回254
如果value小于lethal_cost_threshold(100),则返回0
4.2.4.4.2 VoxelLayer
Voxel layer继承自Obstacle layer. 他们从传感器接收PointClouds or LaserScans格式的数据。区别在于Obstacle layer处理二维数据, Voxel layer处理三维数据。
VoxelLayer 的初始化过程是在函数VoxelLayer::onInitialize()中完成的。它首先会先调用ObstacleLayer::onInitialize()来完成二维层面的初始化。
costmap_2d\plugins\obstacle_layer.cpp
costmap_2d\plugins\voxel_layer.cpp
ObstacleLayer::onInitialize()初始化过程:
1)读取track_unknown_space参数,初始化默认值
track_unknown_space参数决定了地图初始化的初始值,如果为true,则初始化默认值为NO_INFORMATION(255), 如果为false,则初始化默认值为FREE(0)。
2)分配costmap地图内存并初始化。参数来自master(layered_costmap_) 的costmap。master的costmap在staticlayer初始化时完成的。
ObstacleLayer::matchSize();
3)读取参数transform_tolerance,置为0.2
4)读取参数observation_sources,这里配置为:laser_scan_sensor sonar_scan_sensor camera_depth
指定了使用的传感器数据
下面对每一项都有详细配置。会依次解析配置参数,下面是laser scan的配置:
变量名 | 变量类型 | 参数名 | 值 |
topic | std::string | topic | /scan |
sensor_frame | std::string | sensor_frame | “” |
observation_keep_time | double | observation_persistence | 0.0 |
expected_update_rate | double | expected_update_rate | 0 |
data_type | std::string | data_type | LaserScan |
min_obstacle_height | double | min_obstacle_height | 0.21 |
max_obstacle_height | double | max_obstacle_height | 0.30 |
inf_is_valid | bool | inf_is_valid | false |
clearing | bool | clearing | true |
marking | bool | marking | true |
| |||
obstacle_range | double | obstacle_range | 2.0 |
raytrace_range | double | raytrace_range | 5.0 |
5) 根据配置依次分配observations
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_;
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;
订阅相应的topic,并注册callback
filter->registerCallback(boost::bind(&ObstacleLayer::laserScanCallback, this, _1, observation_buffers_.back()));
雷达数据使用ObstacleLayer::laserScanCallback回调函数。将/map发布的数据转换为pointcloud格式,放入ObservationBuffer的list std::list<Observation> observation_list_中的成员 pcl::PointCloud<pcl::PointXYZ>* cloud_
最终数据时点云格式。
6)设置参数动态配置的回调函数
setupDynamicReconfigure(nh);
至此,ObstacleLayer::onInitialize()部分初始化完成。
下面是VoxelLayer::onInitialize()的剩余部分。
7)发布topic
如果publish_voxel_map为true,则发布topic /move_base/global_costmap/obstacle_layer/voxel_grid
发布topic /move_base/global_costmap/obstacle_layer/clearing_endpoints
voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
4.2.4.4.3 InflationLayer
InflationLayer的初始化是在InflationLayer::onInitialize()函数中完成的。
costmap_2d\plugins\inflation_layer.cpp
具体过程为:
- 注册reconfigureCB函数
- 调用matchSize()初始化
为cached_distances_和cached_costs_分配内存并初始化
为seen_分配内存
4.2.4.5 mapUpdateLoop线程工作过程
Costmap2DROS初始化完成,启动mapUpdateLoop线程更新地图
Costmap2DROS::mapUpdateLoop(double frequency)
costmap_2d\src\costmap_2d_ros.cpp
具体过程如下:
1) updateMap
Costmap2DROS::updateMap()
>首先获取机器人的当前全局位姿(x, y, yaw)
>调用layered_costmap_->updateMap(x, y, yaw);
》遍历每一个layer plugin,调用updateBounds()
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
》遍历每一个layer plugin, 调用updateCosts()
(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
> 发布机器人的footprint
2) publisher_->updateBounds(x0, xn, y0, yn);
3) 发布costmap到/move_base/global_costmap/costmap和/move_base/global_costmap/costmap_updates
publisher_->publishCostmap();
此线程在一直循环上面的过程。