costmap_2d 之 obstacle layer

obstacle_layer作为一个地图层，具备基本的更新边界和更新代价的功能，同时由于该层负责处理障碍物，于是衍生了一些接收障碍物数据的回调函数，比如激光回调函数，点云回调函数。

对于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();

增加外部传入的传感器数据处理:

1.基础操作

1.1 初始化

    //获取特定主题的参数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);

1.2 更新边界

// 传入参数
// robot_x、robot_y和robot_yaw 机器人的x，y坐标值和偏航角
// min_y、min_y、max_x和max_y 更新边界轮廓
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,double* min_y, double* max_x, double* max_y)
{// 判断是否调用滚动窗口，若是，则进行更新原点坐标// updateOrigin() 确定更新边界轮廓的原点坐标 // getSizeInMetersX() 返回的是 （size_x_ - 1 + 0.5) * resolution_// getSizeInMetersY() 返回的是 （size_y_ - 1 + 0.5) * resolution_if (rolling_window_)updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);// 确定更新边界轮廓尺寸useExtraBounds(min_x, min_y, max_x, max_y);bool current = true;std::vector<Observation> observations, clearing_observations;// get the marking observations// 获取Marking观测值current = current && getMarkingObservations(observations);// get the clearing observations// 获取Clearing观测值current = current && getClearingObservations(clearing_observations);// update the global current status// 更新全局当前状态current_ = current;// raytrace freespace// 使用raytrace方法清除障碍物for (unsigned int i = 0; i < clearing_observations.size(); ++i){raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);}// place the new obstacles into a priority queue... each with a priority of zero to begin with// 将新障碍物放入优先级队列...每个都以零优先级开始,进行observations容器的遍历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_;//声明指针//定义若干容器，来存储x,y,zsensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z){double px = *iter_x, py = *iter_y, pz = *iter_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);// 更新参数中指定的边界框以包括位置（x，y）}}//更新机器人的footprintupdateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

1.3 更新代价

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----->自由空间setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);}switch (combination_method_){case 0:  // Overwrite// 覆盖更新（updateWithOverwrite），将所操作的地图层的每个有效值（不含未知状态（NO_INFORMATION））写进主栅格地图// master_grid----->在所操作的地图层的特定边界盒子（二维环境为矩形）的主栅格对象，// min_i、min_j----->特定边界盒子的左下角顶点的x、y坐标// max_i、max_j----->特定边界盒子的右上角顶点的x、y坐标updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);break;case 1:  // Maximum// 最大值更新（updateWithMax），将主栅格的新值更新为所操作的地图层值和主栅格层值中的最大值// 如果主代价值是未知状态（NO_INFORMATION），则被覆盖更新// 如果所操作的地图层的代价值是未知状态（NO_INFORMATION），则主代价值不变updateWithMax(master_grid, min_i, min_j, max_i, max_j);break;default:  // Nothingbreak;}
}

2.关键操作

2.1 处理激光数据

分别对不同的sensor类型，注册不同的回调函数。

1.激光回调函数:处理缓冲激光扫描消息的回调

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 (tf2::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);}catch (std::runtime_error &ex){ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());return; //ignore this message}// buffer the point cloud//点云信息处理缓冲区buffer->lock();               // 锁定buffer->bufferCloud(cloud);   // 缓冲区buffer->unlock();             // 解除锁定
}

2.激光回调函数:处理需要过滤才能将 Inf 值转换为range_max的激光扫描消息的回调

void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,const boost::shared_ptr<ObservationBuffer>& buffer)
{// Filter positive infinities ("Inf"s) to max_range.//Step 1:将正无穷大("Inf")筛选为max_rangefloat epsilon = 0.0001;  // a tenth of a millimeter  // a tenth of a millimeter 十分之一毫米sensor_msgs::LaserScan message = *raw_message;for (size_t i = 0; i < message.ranges.size(); i++){float range = message.ranges[ i ];if (!std::isfinite(range) && range > 0){message.ranges[ i ] = message.range_max - epsilon;}}// project the laser into a point cloud//Step 2: 将激光投射到点云中sensor_msgs::PointCloud2 cloud;cloud.header = message.header;// project the scan into a point cloud//Step 3: 将扫描投影到点云中try{projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);}catch (tf2::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);}catch (std::runtime_error &ex){ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());return; //ignore this message}// buffer the point cloud//Step 4: 缓冲点云buffer->lock();buffer->bufferCloud(cloud);buffer->unlock();
}

2.2 处理点云数据

处理点云数据这里如何实现呢？先看ObservationBuffer.cpp源码分析，因为它有一个bufferCloud函数；

ObservationBuffer.cpp

void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
{geometry_msgs::PointStamped global_origin;// create a new observation on the list to be populated//Step 1:在要填充的列表上创建新观测值observation_list_.push_front(Observation());// check whether the origin frame has been set explicitly or whether we should get it from the cloud//Step 2:检测原点frame是否已显式设置，或者我们是否应该从云中获取它。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//Step 3: 鉴于这些观察结果来自传感器...我们需要存储传感器的原点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_);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//Step 4:确保将观测缓冲区的射线扫描障碍物范围传递给观测值observation_list_.front().raytrace_range_ = raytrace_range_;observation_list_.front().obstacle_range_ = obstacle_range_;sensor_msgs::PointCloud2 global_frame_cloud;//Step 5: transform the point cloud 转换点云tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);global_frame_cloud.header.stamp = cloud.header.stamp;// now we need to remove observations from the cloud that are below or above our height thresholds//Step 6:现在我们需要从云中删除低于或高于我们高度阈值的观测值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;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//Step 7: 复制高度范围内的点sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();std::vector<unsigned char>::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_){std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);iter_obs += global_frame_cloud.point_step;++point_count;}}// resize the cloud for the number of legal points//Step 8: 调整云的大小以达到法定点数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//Step 9: 如果发生异常，我们需要从列表中删除空观测值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//Step 10: 如果更新成功，我们希望更新上次更新时间last_updated_ = ros::Time::now();//Step 11: we'll also remove any stale observations from the listpurgeStaleObservations();
}

1.pointCloud回调函数:处理缓冲点云消息的回调

void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,const boost::shared_ptr<ObservationBuffer>& buffer)
{sensor_msgs::PointCloud2 cloud2;if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2)){ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");return;}// buffer the point cloudbuffer->lock();buffer->bufferCloud(cloud2);buffer->unlock();
}

2.处理缓冲PointCloud2消息的回调

void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,const boost::shared_ptr<ObservationBuffer>& buffer)
{// buffer the point cloudbuffer->lock();buffer->bufferCloud(*message);buffer->unlock();
}

2.3 使用raytrace方法清除障碍物

void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,double* max_x, double* max_y)
{//Step 1:输入clearing_observation,会更新min_*,max_*,获取clearing_observation的原点x,y以及点云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//Step 2:获取传感器原点的地图坐标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_;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//Step 4:对于云中的每个点，我们希望从原点开始追踪一条线，并清除沿其的障碍物sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");//Step 5: 从迭代器iter_x开始遍历for (; iter_x != iter_x.end(); ++iter_x, ++iter_y){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//现在，如果需要，我们还需要确保光线跟踪到的点没有超出costmap和比例double a = wx - ox;double b = wy - oy;// the minimum value to raytrace from is the origin// 要进行射线扫描的最小值是原点if (wx < origin_x){double t = (origin_x - ox) / a;wx = origin_x;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;}// now that the vector is scaled correctly... we'll get the map coordinates of its endpoint//现在vector已正确缩放...我们将得到其端点的地图坐标unsigned int x1, y1;// check for legality just in case//检查合法性，以防万一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(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);}
}

2.4 更新清除射线的边界

void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,double* min_x, double* min_y, double* max_x, double* max_y)
{double dx = wx-ox, dy = wy-oy;double full_distance = hypot(dx, dy);double scale = std::min(1.0, range / full_distance);double ex = ox + dx * scale, ey = oy + dy * scale;touch(ex, ey, min_x, min_y, max_x, max_y);
}

2.5 更新机器人footprint

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_);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);}
}

其他

1）.添加静态观察

void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
{if (marking)static_marking_observations_.push_back(obs);if (clearing)static_clearing_observations_.push_back(obs);
}

2）.清除静态观察

void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
{if (marking)static_marking_observations_.clear();if (clearing)static_clearing_observations_.clear();
}

3）.获取标记观察

bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
{bool current = true;// get the marking observationsfor (unsigned int i = 0; i < marking_buffers_.size(); ++i){marking_buffers_[i]->lock();                                // 锁定观察缓冲区marking_buffers_[i]->getObservations(marking_observations); // 将所有当前观察的副本添加到执行的向量末端 current = marking_buffers_[i]->isCurrent() && current;      // 检查观察缓冲区是否按预期率更新marking_buffers_[i]->unlock();}marking_observations.insert(marking_observations.end(),static_marking_observations_.begin(), static_marking_observations_.end());// 在指定位置marking_observations.end()前“插入”区间[static_marking_observations_.begin(), static_marking_observations_.end())的所有元素return current;
}

4）.获取清除观察

bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
{bool current = true;// get the clearing observationsfor (unsigned int i = 0; i < clearing_buffers_.size(); ++i){clearing_buffers_[i]->lock();clearing_buffers_[i]->getObservations(clearing_observations);current = clearing_buffers_[i]->isCurrent() && current;clearing_buffers_[i]->unlock();}clearing_observations.insert(clearing_observations.end(),static_clearing_observations_.begin(), static_clearing_observations_.end());return current;
}

参考文献：

costmap_2d:obstacle_layer - 知乎 (zhihu.com)

ROS naviagtion analysis: costmap_2d–ObstacleLayer_BobLiao1987的博客-CSDN博客

ROS基础教程–CostMap_2D包的一些理解_ros 中二维数组定义_土豆西瓜大芝麻的博客-CSDN博客

【ROS-Navigation】Costmap2d代价地图源码分析——ObstacleLayer障碍物层_obstacle_layer_abicco_ds的博客-CSDN博客