博客转载自：https://blog.csdn.net/u013158492/article/details/50493676
构造函数
ObstacleLayer()
{
costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.这里指明了costmap_指针保存了Obstacle这一层的地图数据
}
对于ObstacleLater，首先分析其需要实现的Layer层的方法：
virtual void onInitialize(); virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,double* max_x, double* max_y); virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void activate(); virtual void deactivate(); virtual void reset();
函数 onInitialize();：
首先获取参数设定的值，然后新建observation buffer
// 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());
然后分别对不同的sensor类型如LaserScan PointCloud PointCloud2，注册不同的回调函数。这里选LaserScan 分析其回调函数：
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// project the laser into a point cloud
sensor_msgs::PointCloud2 cloud;
cloud.header = message->header;
// project the scan into a point cloud
try
{
projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
}
catch (tf::TransformException &ex)
{
ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
ex.what());
projector_.projectLaser(*message, cloud);
}
// buffer the point cloud
buffer->lock();
buffer->bufferCloud(cloud);
buffer->unlock();
}
其中buffer->bufferCloud(cloud)实际上是sensor_msgs::PointCloud2 >>>pcl::PCLPointCloud2 >>> pcl::PointCloud < pcl::PointXYZ > ; 然后才调用void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
{
Stamped < tf::Vector3 > 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
Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0),
pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);
tf_.waitForTransform(global_frame_, local_origin.frame_id_, local_origin.stamp_, ros::Duration(0.5));
tf_.transformPoint(global_frame_, local_origin, global_origin);
observation_list_.front().origin_.x = global_origin.getX();
observation_list_.front().origin_.y = global_origin.getY();
observation_list_.front().origin_.z = global_origin.getZ();
// 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_;
pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;
// transform the point cloud
pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
global_frame_cloud.header.stamp = cloud.header.stamp;
//上面的操作都是针对 observation_list_.front()的一些meta数据作赋值
下面的操作是对(observation_list_.front().cloud_)作赋值操作，
// now we need to remove observations from the cloud that are below or above our height thresholds
pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_);
unsigned int cloud_size = global_frame_cloud.points.size();
observation_cloud.points.resize(cloud_size);
unsigned int point_count = 0;
// copy over the points that are within our height bounds
for (unsigned int i = 0; i < cloud_size; ++i)
{
if (global_frame_cloud.points[i].z <= max_obstacle_height_
&& global_frame_cloud.points[i].z >= min_obstacle_height_)
{
observation_cloud.points[point_count++] = global_frame_cloud.points[i];
}
}
// resize the cloud for the number of legal points
observation_cloud.points.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
//这个操作会将timestamp较早的点都移除出observation_list_
purgeStaleObservations();
}
以下重点分析updateBounds：
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);
if (!enabled_)
return;
useExtraBounds(min_x, min_y, max_x, max_y);
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;
// raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);//首先清理出传感器到被测物之间的区域，标记为FREE_SPACE
}
// 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 pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
for (unsigned int i = 0; i < cloud.points.size(); ++i)
{
double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
// 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;
}
unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
touch(px, py, min_x, min_y, max_x, max_y);
}
}
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
函数raytraceFreespace：
会首先处理测量值越界的问题，然后调用
MarkCell marker(costmap_, FREE_SPACE);
// and finally... we can execute our trace to clear obstacles along that line
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
最终raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 会将所有在(x0,y0) -> (x1,y1)之间的所有cell标记为FREE_SPACE。而updateRaytraceBounds 会根据测量的距离，更新扩张（min_x, min_y, max_x, max_y）。 updateBounds 在根据测量数据完成clear 操作之后，就开始了mark 操作，对每个测量到的点，标记为obstacle ：
double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
// 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;
}
unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
touch(px, py, min_x, min_y, max_x, max_y);
}
函数 updateFootprint：
void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y)
{
if (!footprint_clearing_enabled_) return;
transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);//这里获得了在当前机器人位姿（robot_x, robot_y, robot_yaw）条件下，机器人轮廓点在global坐标系下的值
for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
{
touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);//再次保留或者扩张Bounds
}
}
函数 updateCosts：
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
if (footprint_clearing_enabled_)
{
setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);//设置机器人轮廓所在区域为FREE_SPACE
}
switch (combination_method_)
{
case 0: // Overwrite调用的CostmapLayer提供的方法
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;
default: // Nothing
break;
}
}
ObstacleLayer 主要内容就是这些～～～接下来是InflationLayer