ROS使用Pluginlib自定义代价地图-虚拟墙

一、重写地图层

1、 包含头文件

#include <costmap_prohibition_layer/costmap_prohibition_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(costmap_prohibition_layer_namespace::CostmapProhibitionLayer, costmap_2d::Layer)

想要实现重写地图层，需要派生类里面重写或者说覆盖以下函数：

void onInitialize()：在costmap执行初始化initialize后会执行这个函数，相当于为用户提供的初始化接口。
void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)：计算插件图层要更新到主图层区域的大小，每个图层都可以增加这个尺寸
void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)：将每个图层的代价值更新到主图层

2、自定义地图层参考costmap_prohibition_layer ​

2.1、重写onInitialize函数

void CostmapProhibitionLayer::onInitialize()
{
  //ros节点句柄
  ros::NodeHandle nh("~/" + name_);
  current_ = true;
  
  //创建动态配置的服务器和回调函数，并为服务器配置回调函数
  _dsrv = new dynamic_reconfigure::Server<CostmapProhibitionLayerConfig>(nh);
  dynamic_reconfigure::Server<CostmapProhibitionLayerConfig>::CallbackType cb =
      boost::bind(&CostmapProhibitionLayer::reconfigureCB, this, _1, _2);
  _dsrv->setCallback(cb);

  // 获得地图数据 分辨率
  costmap_2d::Costmap2D *costmap = layered_costmap_->getCostmap();
  _costmap_resolution = costmap->getResolution();

  // set initial bounds
  _min_x = _min_y = _max_x = _max_y = 0;
  
  // 参考代码是从yaml文件中读取虚拟墙数据
  std::string params = "prohibition_areas";
  if (!parseProhibitionListFromYaml(&nh, params))
    ROS_ERROR_STREAM("Reading prohibition areas from '" << nh.getNamespace() << "/" << params << "' failed!");
  
  _fill_polygons = true;
  nh.param("fill_polygons", _fill_polygons, _fill_polygons);
  
  // 计算更新区域的最大最小值
  computeMapBounds();
  
  ROS_INFO("CostmapProhibitionLayer initialized.");
}

2.2、重写updateBounds函数

/*
updateBounds()函数用来根据刚才computeMapBounds()计算得到的最大最小区域更新costmap中定义的最大最小区域变量double *min_x, double *min_y, double *max_x, double *max_y：
*/
void CostmapProhibitionLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, 
                                           double *min_x, double *min_y, double *max_x, double *max_y)
{
    if (!enabled_)
        return;
    std::lock_guard<std::mutex> l(_data_mutex);
    
    if (_prohibition_points.empty() && _prohibition_polygons.empty())
        return;
    *min_x = std::min(*min_x, _min_x);
    *min_y = std::min(*min_y, _min_y);
    *max_x = std::max(*max_x, _max_x);
    *max_y = std::max(*max_y, _max_y);
}

2.3、重写updateCosts函数

/*
    updateCosts()是最主要的功能，用来将地图层的代价更新到主图层，这里面主要有两个循环部分
    第一个循环是更新禁止通行的区域，第二个循环是更新禁止通行的点
    因为源码是对点线和多边形分开操作的
*/

void CostmapProhibitionLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  std::lock_guard<std::mutex> l(_data_mutex);
  
  // 对多边形更新代价
  for (int i = 0; i < _prohibition_polygons.size(); ++i)
  {
      setPolygonCost(master_grid, _prohibition_polygons[i], LETHAL_OBSTACLE, min_i, min_j, max_i, max_j, _fill_polygons);
  }
      
  // 对点更新代价
  for (int i = 0; i < _prohibition_points.size(); ++i)
  {
    unsigned int mx;
    unsigned int my;
    if (master_grid.worldToMap(_prohibition_points[i].x, _prohibition_points[i].y, mx, my))
    {
      master_grid.setCost(mx, my, LETHAL_OBSTACLE);
    }
  }
}

         其中在两个循环中都存在LETHAL_OBSTACLE，这是将禁行区域设置成致命障碍。

3、对参考代码改进

3.1、参考代码实际使用的时候会发现，如果yaml中设置了禁行区域（大于三个点），显示的效果是不仅多边形区域的边会被更新成代价，多边形的内部区域也会更新成代价，这个我觉着不是必要的，甚至还可能占用一定的资源。

void CostmapProhibitionLayer::rasterizePolygon(const std::vector<PointInt>& polygon, std::vector<PointInt>& polygon_cells, bool fill)
{
    // this implementation is a slighly modified version of Costmap2D::convexFillCells(...)

    //我们需要多边形最少需要3条边
    if(polygon.size() < 3)
        return;

    //根据给定的顶点数据 得到组成边的左右元素
    polygonOutlineCells(polygon, polygon_cells);

    if (!fill)
        return;

    //上面所述问题对应以下代码，实际使用时直接注释
    PointInt swap;
    unsigned int i = 0;
    while(i < polygon_cells.size() - 1)
    {
        if(polygon_cells[i].x > polygon_cells[i + 1].x)
        {
            swap = polygon_cells[i];
            polygon_cells[i] = polygon_cells[i + 1];
            polygon_cells[i + 1] = swap;

            if(i > 0)
            --i;
        }
        else
            ++i;
        }

        i = 0;
        PointInt min_pt;
        PointInt max_pt;
        int min_x = polygon_cells[0].x;
        int max_x = polygon_cells[(int)polygon_cells.size() -1].x;

        //walk through each column and mark cells inside the polygon
        for(int x = min_x; x <= max_x; ++x)
        {
            if(i >= (int)polygon_cells.size() - 1)
                break;

            if(polygon_cells[i].y < polygon_cells[i + 1].y)
            {
                min_pt = polygon_cells[i];
                max_pt = polygon_cells[i + 1];
            }
            else
            {
                min_pt = polygon_cells[i + 1];
                max_pt = polygon_cells[i];
            }

            i += 2;
            while(i < polygon_cells.size() && polygon_cells[i].x == x)
            {
                if(polygon_cells[i].y < min_pt.y)
                    min_pt = polygon_cells[i];
                else if(polygon_cells[i].y > max_pt.y)
                    max_pt = polygon_cells[i];
                ++i;
            }

            PointInt pt;
            //loop though cells in the column
            for(int y = min_pt.y; y < max_pt.y; ++y)
            {
                pt.x = x;
                pt.y = y;
                polygon_cells.push_back(pt);
            }
        }
  }

3.2、通过RVIZ的clickpoint手动设置禁行区域

//自定义多变形数据
  zone_pub_ = nh.advertise<geometry_msgs::PolygonStamped>("/record_zone", 1);
  point_sub_ = nh.subscribe("/clicked_point", 1, &CostmapProhibitionLayer::incomingPoint, this);

 参考代码已经给出_prohibition_points、_prohibition_polygons，我们只需要通过rviz手动个出点进行计算即可

//clickpoint
void CostmapProhibitionLayer::incomingPoint(const geometry_msgs::PointStampedConstPtr& point)
{
  if(!recording) return;

  record.emplace_back(*point);
  geometry_msgs::PolygonStamped polygon;
  polygon.header.frame_id = "map";
  polygon.header.stamp = ros::Time::now();
  geometry_msgs::Point32 point32;
  for (auto const& p : record) {
    point32.x = p.point.x;
    point32.y = p.point.y;
    point32.z = 0;
    polygon.polygon.points.emplace_back(point32);
  }
  zone_pub_.publish(polygon);

  //对多边形取点操作

  int polygon_size = polygon.polygon.points.size();
  std::vector<std::vector<geometry_msgs::Point>> _click_polygon;
  if(polygon_size == 1){
    geometry_msgs::Point point;
    point.x = polygon.polygon.points[0].x;
    point.y = polygon.polygon.points[0].y;
    _prohibition_points.push_back(point);
  }
  else if(polygon_size >= 2){
    geometry_msgs::Point point;
    invertPolyToPoint(polygon,point, _click_polygon);
    _prohibition_polygons = _click_polygon;
  }
  polygon.polygon.points.clear(); //主动清除
}

同时在上面的基础上添加了service服务，通过command数值来确定是记录clickpoint点更新地图层代价还是清除代价，清除代价主要就是将成致命障碍LETHAL_OBSTACLE切换成NO_INFORMATION。

//将地图层的代价更新到主图层
void CostmapProhibitionLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  std::lock_guard<std::mutex> l(_data_mutex);
  
  // set costs of polygons
  //将多边形区域全部设置成代价
  for (int i = 0; i < _prohibition_polygons.size(); ++i)
  {
      //多边行区域
      if(!clear)
        setPolygonCost(master_grid, _prohibition_polygons[i], LETHAL_OBSTACLE, min_i, min_j, max_i, max_j, _fill_polygons);
      else
        setPolygonCost(master_grid, _prohibition_polygons[i], NO_INFORMATION, min_i, min_j, max_i, max_j, _fill_polygons);
  }
      
  // set cost of points
  //设置点的代价
  for (int i = 0; i < _prohibition_points.size(); ++i)
  {
    unsigned int mx;
    unsigned int my;
    if (master_grid.worldToMap(_prohibition_points[i].x, _prohibition_points[i].y, mx, my))
    {
      if(!clear)
        master_grid.setCost(mx, my, LETHAL_OBSTACLE);
      else
        master_grid.setCost(mx, my, NO_INFORMATION);
    }
  }
}

4、实际效果