参考：【ROS-Navigation】Costmap2D代价地图源码解读-障碍层ObstacleLayer[1]
要没意外，全局代价地图和局部代价地图都会包含障碍层，在不同地图，表示全局坐标系标识的参数global_frame_往往不一样，全局代价地图是“map”，局部代价地图是“odom”。文中注释是以局部代价地图作为示例，即global_frame_ = "odom"。
rose使用时，observation_keep_time_总是0，即observation_list_只会保留最新帧的观测数据。
障碍层地图通过订阅传感器话题，将传感器输出的障碍物信息存进buffer（剔除过高、过远的点），在本层地图上将观测到的点云标记为障碍物，将传感器到点云点连线上的点标记为FREE_SPACE。最后，在bound范围内，将本层地图合并到主地图上，方法是覆盖。
一、初始化 StaticLayer::onInitialize
首先从参数服务器加载参数，确定rolling_window_、track_unknown_space的值，并调用matchSize函数，根据主地图的参数来设置障碍层地图。
void ObstacleLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_), g_nh;
nh.setCallbackQueue(&cbqueue_);
g_nh.setCallbackQueue(&cbqueue_);
rolling_window_ = layered_costmap_->isRolling();
bool track_unknown_space;
nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
ObstacleLayer::matchSize();
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
double transform_tolerance;
nh.param("transform_tolerance", transform_tolerance, 0.2);首先从参数服务器加载参数，确定rolling_window_、track_unknown_space的值，并调用matchSize函数，根据主地图的参数来设置障碍层地图。
std::string topics_string;
// get the topics that we'll subscribe to from the parameter server
nh.param("observation_sources", topics_string, std::string(""));
ROS_INFO(" Subscribed to Topics: %s", topics_string.c_str());
// now we need to split the topics based on whitespace which we can use a stringstream for
std::stringstream ss(topics_string);从参数服务器加载订阅的话题名称，即障碍物信息的来源。
std::string source;
while (ss >> source)
{
ros::NodeHandle source_node(nh, source);
// get the parameters for the specific topic
double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
std::string topic, sensor_frame, data_type;
bool inf_is_valid, clearing, marking;
source_node.param("topic", topic, source);
source_node.param("sensor_frame", sensor_frame, std::string(""));
source_node.param("observation_persistence", observation_keep_time, 0.0);
source_node.param("expected_update_rate", expected_update_rate, 0.0);
source_node.param("data_type", data_type, std::string("PointCloud"));
source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
source_node.param("inf_is_valid", inf_is_valid, false);
source_node.param("clearing", clearing, false);
source_node.param("marking", marking, true);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{
ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
}
std::string raytrace_range_param_name, obstacle_range_param_name;
// get the obstacle range for the sensor
double obstacle_range = 2.5;
if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
{
source_node.getParam(obstacle_range_param_name, obstacle_range);
}
// get the raytrace range for the sensor
double raytrace_range = 3.0;
if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
{
source_node.getParam(raytrace_range_param_name, raytrace_range);
}
ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(),
sensor_frame.c_str());进入循环，将topics_string记录的观测源逐个输出到source字符串，并找到对应每个观测源的参数。
// create an observation buffer observation_buffers_.push_back( boost::shared_ptr < ObservationBuffer > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, sensor_frame, transform_tolerance))); // check if we'll add this buffer to our marking observation buffers if (marking) marking_buffers_.push_back(observation_buffers_.back()); // check if we'll also add this buffer to our clearing observation buffers if (clearing) clearing_buffers_.push_back(observation_buffers_.back()); ROS_DEBUG( "Created an observation buffer for source %s, topic %s, global frame: %s, " "expected update rate: %.2f, observation persistence: %.2f", source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
为每个观测源创建一个buffer，并将其指针存放进observation_buffers_中进行管理。并根据标志位确定是否将其添加到marking_buffers与clearing_buffers中。
- marking。是否要用收到的点云更新私有地图上的相关栅格，代价值设置为LETHAL_OBSTACLE。即根据点云标记障碍栅格。
- clearing。是否要用收到的点云更新私有地图上的相关栅格，代价值设置为FREE_SPACE。即根据点云标记空闲栅格。
rose_ros把这两值都设置为true，也就说，收到点云既会标记为障碍栅格，也会标记为空闲栅格。
// create a callback for the topic
if (data_type == "LaserScan")
{
...
}
else if (data_type == "PointCloud")
{
...
}
else
{
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
if (inf_is_valid)
{
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
filter->registerCallback(
boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back()));
observation_subscribers_.push_back(sub);
observation_notifiers_.push_back(filter);
}针对不同的sensor类型（LaserScan、PointCloud、PointCloud2），注册不同的回调函数。在有运行cartographer时，cartographer会发布sensor_msgs::PointCloud2，此种场合最好让data_type值是“PointCloud2”，对应回调函数为pointCloud2Callback。
if (sensor_frame != "")
{
std::vector < std::string > target_frames;
target_frames.push_back(global_frame_);
target_frames.push_back(sensor_frame);
observation_notifiers_.back()->setTargetFrames(target_frames);
}
}
dsrv_ = NULL;
setupDynamicReconfigure(nh);
}
二、回调函数 ObstacleLayer::pointCloud2Callback
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// buffer the point cloud
buffer->lock();
buffer->bufferCloud(*message);
buffer->unlock();
}收到sensor_msgs::PointCloud2格式的点云后，调用ObservationBuffer类的bufferCloud函数，将点云数据存到buffer中。
ObservationBuffer是专门用于存储观测数据的工具类，它是ObstacleLayer的类成员。这里关注一下它的bufferCloud函数：当接收到sensor_msgs::PointCloud2格式的点云后，设置好传感器坐标系origin_frame，并将origin_frame下的传感器原点（0，0，0）转换到global系下，得到传感器的坐标，并放进buffer的observation_list_.front().origin_中。
然后将传入的点云cloud转换到global系下，得到global_frame_cloud，接下来，开始遍历点云中的点，剔除z坐标过小或过大的点，将合乎要求的点放进buffer的observation_list_.front().cloud_。这样，便得到了经过筛选后的点云及传感器原点。
void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
{
geometry_msgs::PointStamped global_origin;
// create a new observation on the list to be populated
observation_list_.push_front(Observation());
// check whether the origin frame has been set explicitly or whether we should get it from the cloud
string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
try
{
// given these observations come from sensors... we'll need to store the origin pt of the sensor
geometry_msgs::PointStamped local_origin;
local_origin.header.stamp = cloud.header.stamp;
local_origin.header.frame_id = origin_frame;
local_origin.point.x = 0;
local_origin.point.y = 0;
local_origin.point.z = 0;
tf2_buffer_.transform(local_origin, global_origin, global_frame_);
// 将传感器原点从传感器坐标系转换到odom坐标系
tf2::convert(global_origin.point, observation_list_.front().origin_);
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
observation_list_.front().raytrace_range_ = raytrace_range_;
observation_list_.front().obstacle_range_ = obstacle_range_;
sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud
// 将点云从map坐标系转换到odom坐标系
tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
global_frame_cloud.header.stamp = cloud.header.stamp;
tf2_buffer_.transform(...)除填充global_frame_cloud中的data字段外，即点云数据，还会修改global_frame_cloud的其它字段，像header、fields等。至此global_frame_cloud已存储着odom坐标下的PointCloud2点云。接下要做的是由global_frame_cloud生成observation_list_.front().cloud_。由于点云已在odom坐标系，生成过程不再含有点云坐标值变换。
// now we need to remove observations from the cloud that are below or above our height thresholds sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_); observation_cloud.height = global_frame_cloud.height; observation_cloud.width = global_frame_cloud.width; observation_cloud.fields = global_frame_cloud.fields; observation_cloud.is_bigendian = global_frame_cloud.is_bigendian; observation_cloud.point_step = global_frame_cloud.point_step; observation_cloud.row_step = global_frame_cloud.row_step; observation_cloud.is_dense = global_frame_cloud.is_dense;
除header、data外的其它字段赋值。
unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
modifier.resize(cloud_size);
unsigned int point_count = 0;
// copy over the points that are within our height bounds
sensor_msgs::PointCloud2Iterator iter_z(global_frame_cloud, "z");
std::vector::const_iterator iter_global = global_frame_cloud.data.begin(),
iter_global_end = global_frame_cloud.data.end();
std::vector::iterator iter_obs = observation_cloud.data.begin();
for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
{
if ((*iter_z) <= max_obstacle_height_
&& (*iter_z) >= min_obstacle_height_)
{
//复制点云，去除掉z坐标太高的点
std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);复制一个点。iter_global是src，iter_obs是dest。iter_global已是odom坐标系，原样复制就行，不必转换变换。global_frame_cloud.point_step是表示一个点要占用的字节数，一般是16。具体是x、y、z、强度4个分量，每个分量值是4个字节的FLOAT32。
iter_obs += global_frame_cloud.point_step;
++point_count;
}
}
// resize the cloud for the number of legal points
modifier.resize(point_count);
observation_cloud.header.stamp = cloud.header.stamp;
observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
}
catch (TransformException& ex)
{
// if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front();
ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
// if the update was successful, we want to update the last updated time
last_updated_ = ros::Time::now();
// we'll also remove any stale observations from the list
purgeStaleObservations();
}
三、更新障碍地图边界ObstacleLayer::updateBounds
这个函数主要完成：clearing、marking以及确定bound。
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (rolling_window_)
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
useExtraBounds(min_x, min_y, max_x, max_y);和静态地图类似，同样也是先判断是否是rolling地图，若是则更新地图原点。
bool current = true; std::vector<Observation> observations, clearing_observations; // get the marking observations current = current && getMarkingObservations(observations); // get the clearing observations current = current && getClearingObservations(clearing_observations); // update the global current status current_ = current;
在接收到传感器数据后，buffer将被更新，同样，marking_buffers_和clearing_buffers_也更新（即buffer中的内容），将二者分别提取到observations和clearing_observations中存储。
// raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
// 首先清理出传感器与被测物之间的距离，标记为FREE_SPACE
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}接下来是第一步，对clearing_observations中的点云点执行clearing操作，即将其与传感器的连线上的点标记为FREE_SPACE。这里调用的raytraceFreespace函数内容后述。
// place the new obstacles into a priority queue... each with a priority of zero to begin with
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const Observation& obs = *it;
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
//然后开始进入mark操作，对于每个测量到的点，标记为LETHAL_OBSTACLE
//即迭代点云中的点，本身太高太远的障碍和跟其他障碍点离得太远的点都不考虑
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
double px = *iter_x, py = *iter_y, pz = *iter_z;
//如果障碍太高，则不加入mark
// if the obstacle is too high or too far away from the robot we won't add it
if (pz > max_obstacle_height_)
{
ROS_DEBUG("The point is too high");
continue;
}
// compute the squared distance from the hitpoint to the pointcloud's origin
// 计算点与点之间的距离平方，如果距离太远，不考虑
double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
+ (pz - obs.origin_.z) * (pz - obs.origin_.z);
// if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
{
ROS_DEBUG("The point is too far away");
continue;
}
// now we need to compute the map coordinates for the observation
//获得障碍点在地图上的坐标
unsigned int mx, my;
if (!worldToMap(px, py, mx, my))
{
ROS_DEBUG("Computing map coords failed");
continue;
}
//设置该栅格cost为致命障碍
unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
touch(px, py, min_x, min_y, max_x, max_y);
}
}第二步是marking操作，即将点云中的点标记为障碍物。
在标记时通过二重循环，外层迭代各观测轮次，内层迭代一次观测得到的点云点，剔除本身太高（z坐标过大）、与传感器距离太远的点，将符合要求的障碍点坐标从global坐标系转换到区域相关的map坐标系，并在本层地图上标记致命障碍。
并调用touch函数，确保标记的障碍点包含在 bound内。
在以上两步完成后，调用updateFootprint函数，它的作用是基于机器人当前位置确定该位置下的足迹，并在内部调用touch函数保证足迹包含在bound范围内。
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); }
四、清理传感器到障碍物间的cell ObstacleLayer::raytraceFreespace
void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y)
{
// clearing_observation的origin_是传感器坐标，传入
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
// get the map coordinates of the origin of the sensor
unsigned int x0, y0;
//得到传感器原点在地图上的坐标
if (!worldToMap(ox, oy, x0, y0))
{
ROS_WARN_THROTTLE(
1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
ox, oy);
return;
}
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double origin_x = origin_x_, origin_y = origin_y_;
double map_end_x = origin_x + size_x_ * resolution_;
double map_end_y = origin_y + size_y_ * resolution_;
// 保证传感器原点在bound范围内
touch(ox, oy, min_x, min_y, max_x, max_y);先将传感器坐标转换到地图坐标系。
// for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y");
//对于点云中的每个点，我们要追踪原点和clear obstacles之间的一条线
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
{
//wx wy是当前点云中的点的坐标
double wx = *iter_x;
double wy = *iter_y;
// now we also need to make sure that the enpoint we're raytracing
// to isn't off the costmap and scale if necessary
//a、b是该点跟传感器原点的距离
double a = wx - ox;
double b = wy - oy;
// the minimum value to raytrace from is the origin
//如果当前点x方向比地图原点还小
if (wx < origin_x)
{
//t（比例）=（地图原点-传感器原点）/（点云中的该点-传感器原点）
double t = (origin_x - ox) / a;
//当前点x = 地图原点x
wx = origin_x;
//当前点y =
//实际上还是把点云点和传感器连线之间清空，只是通过相似三角形丢弃了超出地图原点范围外的部分，下面三个判断结构同理
wy = oy + b * t;
}
if (wy < origin_y)
{
double t = (origin_y - oy) / b;
wx = ox + a * t;
wy = origin_y;
}
// the maximum value to raytrace to is the end of the map
if (wx > map_end_x)
{
double t = (map_end_x - ox) / a;
wx = map_end_x - .001;
wy = oy + b * t;
}
if (wy > map_end_y)
{
double t = (map_end_y - oy) / b;
wx = ox + a * t;
wy = map_end_y - .001;
}
以上4个if判断，目的是确保(wx, wy)不落在代价地图外，如果落在外面，要把它修正到地图内。举个例子，图1中绿色表示地图区域，L是雷达原点，A是点云中某个点，它不在地图，属于四种例外情况的第四种。经过修正后，会使得(wx, wy)对应的是图1中B点。变量t指示地图内长度占总长度的比率。map_end_y是属于地图外的，“-.001”是为让属于地图内。
// now that the vector is scaled correctly... we'll get the map coordinates of its endpoint unsigned int x1, y1; // check for legality just in case // 转换到区域相关的map坐标系。如果worldToMap返回false，表示该坐标不在区域内。 if (!worldToMap(wx, wy, x1, y1)) continue; unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); MarkCell marker(costmap_, FREE_SPACE); // and finally... we can execute our trace to clear obstacles along that line //调用raytraceLine函数清理传感器原点和障碍物点之间的cell raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); //updateRaytraceBounds函数用来根据测量的距离，更新扩张 updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); }
接下来的几个判断结构实际上做的主要工作就是不断迭代传感器原点和点云中的点的连线，并对其调用raytraceLine函数，将连线上的点在本层地图上标记为FREE_SPACE。标记不是标记整条线，而是从传感器原点开始、raytrace_range_米内那些点。raytrace_range_是个param参数，当前值是3(米)。
图2显示了raytrace_range_作用。传感器正上方3.7米处有障碍物，由于raytrace_range_=3，只是把3米内的标记为FREE_SPACE，超过3米的就保持着NO_INFORMATION。传给raytraceLine的cell_raytrace_range是基于raytrace_range_算出的、转换到栅格数为单位的距离，此处是3/0.05=60。raytraceLine第6个参数max_length提供了这么个功能：max_length指示这个线段的最大期望长度，允许你不用一直走到终点。
这里获取连线时，要注意该点是否超出地图范围，如果超出，则通过相似三角形去除连线在地图外的部分。
updateRaytraceBounds函数的作用是保证连线上的一点（距离传感器特定范围内）被包含进bound。
}
五、更新障碍地图代价 ObstacleLayer::updateCosts
这个函数就是将机器人足迹范围内设置为FREE_SPACE，并且在bound范围内将本层障碍地图的内容合并到主地图上。
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (footprint_clearing_enabled_)
{
//设置机器人所在区域为FREE_SPACE
setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
}
switch (combination_method_)
{
case 0: // Overwrite
updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
break;
case 1: // Maximum
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
break;不论globalcostmap，还是localcostmap，combination_method_默认都是1，即取得层中最大的cost。
default: // Nothing break; } }
五、参数sensor_frame
sensor_frame的类型是std::string，用于指示点云最初来自哪个坐标系。rose_ros把它赋值为“laser”，即激光雷达坐标系。为什么要用sensor_frame，让结合图2。
图2根据点云情况，设置相关栅格为FREE_SPACE。这时我们有Observation。它当中有个类型是sensor_msgs::PointCloud2的字段cloud，由cloud，能知道各障碍点在odom坐标系下坐标。这里有个问题，生成这个点云时的原点在哪里？只有知道了这个原点，我们才能以原点为中心、raytrace_range_为半径生成一个圆，圆内栅格置为FREE_SPACE。很显然，这个原点得是激光雷达的中心，可PointCloud2没有存储原点的字段。
根据图2，有人会认为原点不就是机器人中心，也就是那个绿色矩形。这个中心是basefoot_print坐标系原点在odom坐标系下的坐标。如果雷达原点和机器人中心点重合，这没问题，如果不重合，就出错了。
sensor_frame的作用就是指示点云最初来自哪个坐标系，是激光雷达的，往往把这坐标系命名为“laser”。
因为PointCloud2没有存储原点的字段，Observation用了origin_字段专门存储激光雷达坐标系原点在odom坐标系下的坐标。至于怎么算这个坐标，只要(0, 0, 0)乘上laser到odom的就行。
除用于标记空闲栅格，同样原因，障碍层用sensor_frame标识障碍栅格。
参考
- 【ROS-Navigation】Costmap2D代价地图源码解读-障碍层ObstacleLayer https://blog.csdn.net/Neo11111/article/details/104852657