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

最新推荐文章于 2024-06-10 17:25:50 发布

原创

最新推荐文章于 2024-06-10 17:25:50 发布 · 1.2k 阅读

21 ·

CC 4.0 BY-SA版权

文章标签：

#c++

文章详细描述了如何重写和扩展CostmapProhibitionLayer，包括覆盖onInitialize,updateBounds,和updateCosts函数，以及使用自定义参数和rviz进行禁行区域设置。着重于处理多边形和点的致命障碍及清除功能。

一、重写地图层

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.para