【Nav2中文网】六、插件教程（一）编写新的二维代价地图（Costmap2D）插件

• 本教程来自：Nav2中文网
• Nav2交流社区：https://fishros.org.cn/forum
• ROS2/Nav2千人交流群：(QQ)139707339
• 更多精彩教程请关注微信公众号：鱼香ROS
• 欢迎添加机器人小伊微信，解锁机器人学习特殊服务

编写新的二维代价地图（Costmap2D）插件

• 概述
• 必要条件
• 教程步骤

概述

本教程演示如何创建自己的简单的代价地图2D 插件 。 [校准@小鱼]

在开始本教程之前，请参考此 视频 ，其中包含有关代价地图2D层设计和插件基本操作原理的信息。 [校准@小鱼]

必要条件

假如ROS 2、Gazebo 和 TurtleBot3包是在安装或本地构建的。请确保 Nav 2项目也是在本地构建的，构建过程可以参考 编译和安装 。 [校准@小鱼]

教程步骤 1- 写一个新代价地图2D（Costmap2D）插件 [校准@小鱼]

为了方便演示，下面这个例子将创建代价地图（costmap）插件，然后放置重复代价梯度到costmap中。有注解代码可以找到本教程在 navigation2_tutorials 库的 nav2_gradient_costmap_plugin ROS 2d的包。当你自己想要创建2D代价地图插件时请参考它。 [校准@小鱼]

插件类 nav2_gradient_costmap_plugin::GradientLayer 继承于基类 nav2_costmap_2d::Layer: [校准@小鱼]

namespace nav2_gradient_costmap_plugin
{

class GradientLayer : public nav2_costmap_2d::Layer

基类提供了一系列虚函数 API处理costmap层插件。这些函数在运行 LayeredCostmap 时被调用 。函数列表，在插件的代码中的描述和是否必须实现情况见下表: [校准@小鱼]

Virtual method方法描述必须重写？onInitialize()方法在插件初始化结束时调用。通常会在方法里声明ROS参数。任何需要初始化的操作都应该在该方法里完成。 [校准@小鱼]否 [校准@小鱼]updateBounds()方法问插件: 面积costmap层需要更新。有3输入参数方法: 机器人位置和定位4输出参数: 指针窗口边界。这些边界用于性能原因: 更新区域内窗口是新信息，避免更新整个costmap每次迭代。 [待校准@2056]是 [校准@小鱼]updateCosts()每次需要重新计算costmap时都会调用方法。它仅在其边界窗口内更新costmap层。方法有4个输入参数: 计算窗口边界和1个输出参数: 参考结果costmap master_grid 。 Layer 类为插件提供了一个内部成本图， costmap_ ，用于更新。的 master_grid 应更新值窗口边界使用以下更新方法: updateWithAddition() ， updateWithMax() ， updateWithOverwrite() 或 updateWithTrueOverwrite() 。 [待校准@2058]是 [校准@小鱼]matchSize()方法在每次更改地图大小时调用。 [校准@小鱼]否 [校准@小鱼]onFootprintChanged()每次改变足迹（footprint）时都会调用方法。 [校准@小鱼]否 [校准@小鱼]reset()在costmap重置时可能有任何代码被执行。 [校准@小鱼]是 [校准@小鱼]

在我们的示例中，这些方法具有以下功能: [校准@小鱼]

1. 参数 GradientLayer::onInitialize() 包含声明ROS参数的默认值: [校准@小鱼]

declareParameter("enabled", rclcpp::ParameterValue(true));
node_->get_parameter(name_ + "." + "enabled", enabled_);

并设置 need_recalculation_ 边界重新计算标志: [校准@小鱼]

need_recalculation_ = false;

2. GradientLayer::updateBounds() 再次计算窗口边界，如果 need_recalculation_ 值 true 并且无视 need_recalculation_ 值而进行更新。 [校准@小鱼]
3. GradientLayer::updateCosts() - 在这个方法中，是没有与前一图层合并，而是直接写入梯度到产生的成本地图 master_grid 。 这相当于内部工作 costmap_ 并且调用 updateWithTrueOverwrite() 方法。以下是主成本图的梯度制作算法: [校准@小鱼]

int gradient_index;
for (int j = min_j; j < max_j; j++) {
  // Reset gradient_index each time when reaching the end of re-calculated window
  // by OY axis.
  gradient_index = 0;
  for (int i = min_i; i < max_i; i++) {
    int index = master_grid.getIndex(i, j);
    // setting the gradient cost
    unsigned char cost = (LETHAL_OBSTACLE - gradient_index*GRADIENT_FACTOR)%255;
    if (gradient_index