costmap插件开发方法

最新推荐文章于 2024-06-24 09:19:35 发布

存在即合理L

最新推荐文章于 2024-06-24 09:19:35 发布

阅读量804

点赞数

文章标签：自动驾驶

原文链接：https://gitee.com/ahao1q97/cost_map

版权

本文详细介绍了如何在ROS的costmap_2d包中创建自定义地图层，包括创建SimpleLayer和GridLayer插件，用于设置伪装障碍点。通过这些插件，可以在不改变真实环境的情况下阻止机器人进入特定区域。此外，还涉及到costmap的多层结构，如StaticLayer、ObstacleLayer和InflationLayer，并解释了如何将自定义层集成到路径规划模块中。

摘要由CSDN通过智能技术生成

参考连接

Code处理逻辑

ROS中costmap_2d这个包提供了一个可以配置的结构维护costmap,其中Costmap通过costmap_2d::Costmap2DROS对象利用传感器数据和静态地图中的信息来存储和更新现实世界中障碍物的信息。costmap_2d::Costmap2DROS为用户提供了纯粹的2维索引，这样可以只通过columns查询障碍物。举个例子来说，一个桌子和一双鞋子在xy平面的相同位置，有不同的Z坐标，在costm_2d::Costmap2DROS目标对应的的costmap中，具有相同的cost值。这旨在帮助规划平面空间。

Costmap由多层组成，例如在costmap_2d包中，1.StaticLayer（静态地图层）是第一层， 2.ObstacleLayer（障碍物层）是第二层，3.InflationLayer（膨胀层）是第三层。这三层组合成了master map（最终的costmap），供给路线规划模块使用。

最终输出mast map给path planing 模块使用。 Costmap主接口是costmap_2d::Costmap2DROS，它维持了Costmap在ROS中大多数的相关的功能。它用所包含的costmap_2d::LayeredCostmap类来跟踪每一个层。每层使用pluginlib（ROS插件机制）来实例化并添加到LayeredCostmap类的对象中。各个层可以被独立的编译，且允许使用C++接口对costmap做出任意的改变。

Costmap ROS接口：

ROS对costmap进行了复杂的封装，提供给用户的主要接口是Costmap2DROS,而真正的地图信息是储存在各个Layer中。下图可以简要说明Costmap的各种接口的关系：
在这里插入图片描述 Costmap的ObstacleLayer和StaticLayer都继承于CostmapLayer和Costmap2D,因为它们都有自己的地图，Costmap2D为它们提供存储地图的父类，CostmapLayer为它们提供一些对地图的操作方法。而inflationLayer因为没有维护真正的地图所以只和CostmapLayer一起继承于Layer，Layer提供了操作master map的途径。 LayerdCostmap为Costmap2DROS（用户接口）提供了加载地图层的插件机制，每个插件（即地图层）都是Layer类型的。

创建自定义用户层

示例是为costmap新创建一层并在该层设置一个伪装障碍点，不让机器人移动到该处。实际应用中比如你不想让机器人去某一个地方但该地方又没有真正障碍，你就可以通过这种方式设置一个伪装障碍。
编写插件cpp和header文件
1.1在src/create_new_layers/src目录下添加如下2个cpp文件，创建了2个插件可供调用，我们示例中主要使用grid_layer.cpp，

simple_layer.cpp

#include<create_new_layers/simple_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;

namespace simple_layer_namespace
{

SimpleLayer::SimpleLayer() {}

void SimpleLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &SimpleLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}


void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
}

void SimpleLayer::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;

  mark_x_ = robot_x + cos(robot_yaw);
  mark_y_ = robot_y + sin(robot_yaw);

  *min_x = std::min(*min_x, mark_x_);
  *min_y = std::min(*min_y, mark_y_);
  *max_x = std::max(*max_x, mark_x_);
  *max_y = std::max(*max_y, mark_y_);
}

void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;
  unsigned int mx;
  unsigned int my;
  if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
    master_grid.setCost(mx, my, LETHAL_OBSTACLE);
  }
}

} // end namespace

grid_layer.cpp

#include<create_new_layers/grid_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;

namespace simple_layer_namespace
{

unsigned flag = 0;

GridLayer::GridLayer() {}

void GridLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;
  default_value_ = NO_INFORMATION;
  matchSize();

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &GridLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}


void GridLayer::matchSize()
{
  Costmap2D* master = layered_costmap_->getCostmap();
  resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
            master->getOriginX(), master->getOriginY());
}


void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
}

void GridLayer::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;

  if (flag == 0)
  {
	  flag = 1;
  }else
	return;

  double mark_x = robot_x + cos(robot_yaw), mark_y = robot_y + sin(robot_yaw);
  unsigned int mx;
  unsigned int my;
  if(worldToMap(mark_x, mark_y, mx, my)){
	       setCost(mx, my,LETHAL_OBSTACLE);
  }

  *min_x = std::min(*min_x, mark_x);
  *min_y = std::min(*min_y, mark_y);
  *max_x = std::max(*max_x, mark_x);
  *max_y = std::max(*max_y, mark_y);
}

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

  for (int j = min_j; j < max_j; j++)
  {
    for (int i = min_i; i < max_i; i++)
    {
      int index = getIndex(i, j);
      if (costmap_[index] == NO_INFORMATION)
        continue;
      master_grid.setCost(i, j, costmap_[index]); 
    }
  }
}

} // end namespace

1.2头文件
simple_layer.h

#ifndef SIMPLE_LAYER_H_
#define SIMPLE_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/GenericPluginConfig.h>
#include <dynamic_reconfigure/server.h>

namespace simple_layer_namespace
{

class SimpleLayer : public costmap_2d::Layer
{
public:
  SimpleLayer();

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

private:
  void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);

  double mark_x_, mark_y_;
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
};
}
#endif

grid_layer.h

#ifndef GRID_LAYER_H_
#define GRID_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/GenericPluginConfig.h>
#include <dynamic_reconfigure/server.h>

namespace simple_layer_namespace
{

class GridLayer : public costmap_2d::Layer, public costmap_2d::Costmap2D
{
public:
  GridLayer();

  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);
  bool isDiscretized()
  {
    return true;
  }

  virtual void matchSize();

private:
  void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
};
}
#endif

1.3在src/create_new_layers/目录下创建插件描述符文件costmap_plugins.xml，

<class_libraries>
  <library path="lib/libsimple_layer">
    <class type="simple_layer_namespace::SimpleLayer" base_class_type="costmap_2d::Layer">
      <description>Demo Layer that adds a point 1 meter in front of the robot</description>
    </class>
  </library>
  <library path="lib/libgrid_layer">
    <class type="simple_layer_namespace::GridLayer" base_class_type="costmap_2d::Layer">
      <description>Demo Layer that marks all points that were ever one meter in front of the robot</description>
    </class>
  </library>
</class_libraries>

1.4 导出插件在package.xml文件导出插件，供costmap_2d包使用

1.5 编译文件修改修改CMakeLists.txt文件，

include_directories( include ${catkin_INCLUDE_DIRS} )

add_library(simple_layer src/simple_layer.cpp) add_library(grid_layer src/grid_layer.cpp) 1.6 编译插件并导出目录

（1）在工作空间主目录costmap2d_tutorial_study_ws下，执行命令catkin_make；

（2）导出二进制插件目录供调用，执行命令source costmap2d_tutorial_study_ws/devel/setup.bash

1.7 检查插件是否编译成功

rospack plugins --attrib=plugin costmap_2d

从如下输出内容应该看到costmap_2d包插件已经包含你刚才创建的插件，

frontier_exploration /opt/ros/kinetic/share/frontier_exploration/costmap_plugins.xml create_new_layers /home/xxx/costmap2d_tutorial_study_ws/src/create_new_layers/costmap_plugins.xml costmap_2d /opt/ros/kinetic/share/costmap_2d/costmap_plugins.xml

2 调用新插件

以下我们将调用刚刚创建的新层插件，通过简单修改rbx1示例代码完成，为了完成示例一共修改2个导航配置文件。

2.1 修改rbx1_nav/config/fake/costmap_common_params.yaml

添加obstacles，说明障碍数据来源，同时强调了命名空间

obstacles: observation_sources: scan scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

2.2 修改rbx1_nav/config/fake/global_costmap_params.yaml

本示例我们在全局代价地图配置文件添加如下代码，加入插件规范请参考Configuring Layered Costmaps

plugins: - {name: static_map, type: “costmap_2d::StaticLayer”} - {name: obstacles, type: “costmap_2d::VoxelLayer”} - {name: gridlayer, type: “simple_layer_namespace::GridLayer”} - {name: inflation_layer, type: “costmap_2d::InflationLayer”}

注意：这里我们假定会发布static map并使用它

具体操作参考以下链接
https://gitee.com/ahao1q97/cost_map