ROS naviagtion analysis: costmap_2d--ObstacleLayer

构造函数

  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 bufferobservation_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 origindouble 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