Nav2代价地图实现和原理--Nav2源码解读之CostMap2D(上)

本文介绍了Costmap2D的几个核心类，包括Costmap2D、CostmapLayer和Layer。包括底下派生的子类StaticLayer和ObstacleLayer

m0_73800387

1633人浏览 · 2024-08-06 08:50:00

m0_73800387 · 2024-08-06 08:50:00 发布

前言

本文将结合Nav2 Costmap_2d功能包源码，对CostMap2D实现进行解读
本文默认读者有ROS2以及C++使用经验，故不对一些基础内容进行讲解。
源码链接
本文包含大量源码，以注释加解读的形式，对源码进行详解

CostMap2D简介

Costmap2D功能包提供了一种用于机器人导航和路径规划的一种数据结构，它广泛用于机器人操作系统（ROS）中。Costmap2D的主要功能是创建一个二维代价地图，这个地图用来表示环境中各个位置对于机器人移动的代价。代价地图是一个非常重要的概念，因为它可以帮助机器人理解周围环境，从而做出更安全的路径规划决策。
以下是Costmap2D的一些关键特性：

代价表示：在Costmap2D中，每个单元格都有一个与之关联的代价值，这个值表示机器人通过该单元格的难易程度。通常代价分为几个等级，如：
- 自由空间（free space）：机器人可以自由移动的区域，通常代价为0。
- 可通行区域（inflate obstacles）：通常有一定的代价，表示机器人可以通行，但可能需要绕行障碍物。
- 障碍物（obstacles）：代价很高，表示机器人不应该进入这些区域。
多层结构：Costmap2D通常分为多层，包括静态层、膨胀层（障碍物膨胀层）和动态层。
- 静态层（Static layer）：包含地图中固定障碍物的信息，如墙壁。
- 膨胀层（Obstacle layer）：通过膨胀算法处理传感器数据，为障碍物周围增加安全缓冲区。
- 动态层（Dynamic layer）：包含时间变化的障碍物信息，如移动的物体或临时障碍。
更新机制：Costmap2D可以根据传感器的输入实时更新。例如，激光测距仪或摄像头的数据可以用来更新障碍物的位置。
插件式架构：在ROS中，Costmap2D允许使用插件来扩展其功能，这意味着可以根据需要增加新的层或修改现有层的处理方式。
集成与导航堆栈：Costmap2D是ROS导航堆栈中的一个关键组件，与全局规划器和局部规划器紧密集成，用于生成从当前点到目标点的安全路径。

整体框架

以下是Nav2 Costmap_2d功能包的所有文件一览

Nav2 Costmap_2d
│  CHANGELOG.rst
│  CMakeLists.txt
│  costmap_plugins.xml
│  package.xml
│  README.md
│
├─include
│  └─nav2_costmap_2d- 包含了代价地图核心功能的头文件，如代价地图类、层、观测数据结构等。
│      │  clear_costmap_service.hpp
│      │  costmap_2d.hpp
│      │  costmap_2d_publisher.hpp
│      │  costmap_2d_ros.hpp
│      │  costmap_layer.hpp
│      │  costmap_math.hpp
│      │  costmap_subscriber.hpp
│      │  costmap_topic_collision_checker.hpp
│      │  cost_values.hpp
│      │  denoise_layer.hpp
│      │  exceptions.hpp
│      │  footprint.hpp
│      │  footprint_collision_checker.hpp
│      │  footprint_subscriber.hpp
│      │  inflation_layer.hpp
│      │  layer.hpp
│      │  layered_costmap.hpp
│      │  observation.hpp
│      │  observation_buffer.hpp
│      │  obstacle_layer.hpp
│      │  range_sensor_layer.hpp
│      │  static_layer.hpp
│      │  voxel_layer.hpp
│      │
│      ├─costmap_filters-包含代价地图过滤器的头文件，用于对代价地图进行处理。
│      │      binary_filter.hpp
│      │      costmap_filter.hpp
│      │      filter_values.hpp
│      │      keepout_filter.hpp
│      │      speed_filter.hpp
│      │
│      └─denoise
│              image.hpp
│              image_processing.hpp
│
├─plugins
│  │  denoise_layer.cpp
│  │  inflation_layer.cpp
│  │  obstacle_layer.cpp
│  │  range_sensor_layer.cpp
│  │  static_layer.cpp
│  │  voxel_layer.cpp
│  │
│  └─costmap_filters代价地图层的实现
│          binary_filter.cpp
│          costmap_filter.cpp
│          keepout_filter.cpp
│          speed_filter.cpp
│
├─src
│      clear_costmap_service.cpp
│      costmap_2d.cpp
│      costmap_2d_cloud.cpp
│      costmap_2d_markers.cpp
│      costmap_2d_node.cpp
│      costmap_2d_publisher.cpp
│      costmap_2d_ros.cpp
│      costmap_layer.cpp
│      costmap_math.cpp
│      costmap_subscriber.cpp
│      costmap_topic_collision_checker.cpp
│      footprint.cpp
│      footprint_collision_checker.cpp
│      footprint_subscriber.cpp
│      layer.cpp
│      layered_costmap.cpp
│      observation_buffer.cpp
│
└─test
    │  CMakeLists.txt
    │  costmap_params.yaml
    │  module_tests.cpp
    │  simple_driving_test.xml
    │  testing_helper.hpp
    │
    ├─integration
    │      CMakeLists.txt
    │      costmap_tester.cpp
    │      costmap_tests_launch.py
    │      dyn_params_tests.cpp
    │      footprint_tests.cpp
    │      inflation_tests.cpp
    │      obstacle_tests.cpp
    │      range_tests.cpp
    │      test_costmap_2d_publisher.cpp
    │      test_costmap_subscriber.cpp
    │      test_costmap_topic_collision_checker.cpp
    │
    ├─map
    │      TenByTen.pgm
    │      TenByTen.yaml
    │
    ├─regression
    │      CMakeLists.txt
    │      costmap_bresenham_2d.cpp
    │      order_layer.cpp
    │      order_layer.hpp
    │      order_layer.xml
    │      plugin_api_order.cpp
    │
    ├─test_launch_files
    │      costmap_map_server.launch.py
    │
    └─unit
            binary_filter_test.cpp
            CMakeLists.txt
            copy_window_test.cpp
            costmap_conversion_test.cpp
            costmap_cost_service_test.cpp
            costmap_filter_service_test.cpp
            costmap_filter_test.cpp
            declare_parameter_test.cpp
            denoise_layer_test.cpp
            footprint_collision_checker_test.cpp
            image_processing_test.cpp
            image_test.cpp
            image_tests_helper.hpp
            keepout_filter_test.cpp
            keepout_filter_test.jpg
            lifecycle_test.cpp
            speed_filter_test.cpp

其中有几个文件为核心实现，我们重点关注其实现源码
- Costmap2D—代价地图数据结构
- Layer—代价地图层插件的抽象基类
- LayeredCostmap—多层代价地图插件合体机
- Costmap2DLayer—多层代价地图插件的抽象基类
- StaticLayer—静态地图层
- ObstacleLayer—障碍地图层

`Costmap2D`—代价地图数据结构

二维代价地图（2D costmap）是一种网格化的数据结构，用于表示机器人所处的环境。在这个网格中，每个单元格都与其相关的“代价”值关联

类的声明和构造函数

class Costmap2D
{
public:
  // 构造函数，初始化代价地图的尺寸、分辨率、原点等参数
  Costmap2D(
    unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
    double origin_x, double origin_y, unsigned char default_value = 0);

  // 复制构造函数，用于创建当前对象的副本
  Costmap2D(const Costmap2D & map);

  // 从OccupancyGrid消息构造代价地图
  explicit Costmap2D(const nav_msgs::msg::OccupancyGrid & map);

  // 赋值运算符重载，用于将一个代价地图复制到另一个
  Costmap2D & operator=(const Costmap2D & map);

  // 析构函数
  virtual ~Costmap2D();
};

核心功能成员函数

// 获取或设置特定单元格的代价值
unsigned char getCost(unsigned int mx, unsigned int my) const;
void setCost(unsigned int mx, unsigned int my, unsigned char cost);

// 将地图坐标转换为世界坐标，反之亦然
void mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const;
bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const;

// 代价地图的尺寸和原点等信息
unsigned int getSizeInCellsX() const;
unsigned int getSizeInCellsY() const;
double getSizeInMetersX() const;
double getSizeInMetersY() const;
double getOriginX() const;
double getOriginY() const;
double getResolution() const;

// 重置代价地图的尺寸和原点
void resizeMap(
  unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
  double origin_y);

// 将多边形区域内的单元格设置为特定代价
bool setConvexPolygonCost(
  const std::vector<geometry_msgs::msg::Point> & polygon,
  unsigned char cost_value);

保护成员函数和私有成员

size_x_ 和 size_y_: 分别表示代价地图在x轴和y轴上的尺寸，以单元格为单位。
resolution_: 表示每个单元格的分辨率，即每个单元格代表多少实际世界的单位。
origin_x_ 和 origin_y_: 表示代价地图在世界坐标系中的原点位置。
costmap_: 是一个指向 unsigned char 类型的指针，它指向一个二维数组，用于存储代价地图中每个单元格的代价值。这个数组的大小由 size_x_ 和 size_y_ 决定。
default_value_: 表示代价地图中每个单元格的默认代价值。如果某个单元格的代价值没有被特别设置，它将使用这个默认值。这个值通常被设置为 0，表示未知区域或自由空间。

protected:
  // 初始化、删除和重置代价地图数据结构
  virtual void initMaps(unsigned int size_x, unsigned int size_y);
  virtual void deleteMaps();
  virtual void resetMaps();

private:
  // 代价地图的数据和参数
  unsigned int size_x_;
  unsigned int size_y_;
  double resolution_;
  double origin_x_;
  double origin_y_;
  unsigned char * costmap_;
  unsigned char default_value_;
};

其他重要成员

MapLocation 结构体: 用于存储地图坐标对。

struct MapLocation
{
  unsigned int x;
  unsigned int y;
};

mutex_t * access_: 用于同步访问代价地图的互斥锁。

  typedef std::recursive_mutex mutex_t;
  mutex_t * getMutex()
  {
    return access_;
  }

MarkCell 和 PolygonOutlineCells 内部类: 用于在代价地图上执行特定操作的辅助类。

`Costmap2D`具体实现

下面来看看Costmap2D的具体实现，这里只挑重要的部分

地图创建

首先我们来看构造函数，首先构造函数传入构造一个地图必须的参数，并传给initMaps

Costmap2D::Costmap2D(
  unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
  double origin_x, double origin_y, unsigned char default_value)
: resolution_(resolution), origin_x_(origin_x),
  origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
{
  access_ = new mutex_t();
  // 创建代价地图
  initMaps(cells_size_x, cells_size_y);
  //重置代价地图中的所有单元格的代价值
  resetMaps();
}

在initMaps中，根据传入的地图大小，进行1维uchar类型的数组创建

void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
  std::unique_lock<mutex_t> lock(*access_);
  delete[] costmap_;
  size_x_ = size_x;
  size_y_ = size_y;
  costmap_ = new unsigned char[size_x * size_y];
}

resetMaps 函数的作用是重置代价地图中的所有单元格的代价值

void Costmap2D::resetMaps()
{
  std::unique_lock<mutex_t> lock(*access_);
  memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

代价值获取与设置

由于二维地图Costmap存储的时候为一维类型的数组unsigned char[size_x * size_y]，因此输入代价地图的x,y坐标值时需要转换为对应的索引值获取代价地图的(mx,my)的索引的代价值

unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
{
  return costmap_[getIndex(mx, my)];
}

getIndex函数结算如下

  inline unsigned int getIndex(unsigned int mx, unsigned int my) const
  {
    return my * size_x_ + mx;
  }

同理更改代价值也是一个道理

void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
  costmap_[getIndex(mx, my)] = cost;
}

`Layer`—代价地图层插件的抽象基类

Layer 类是一个抽象基类，用于定义多层代价地图插件（Layered Costmap Plugins）的接口。多层代价地图插件是一种设计模式，允许在代价地图中添加不同的层，以实现不同的功能，

核心方法

// 初始化层，设置层名、变换缓冲区、节点、回调组等参数
void initialize(
  LayeredCostmap * parent,
  std::string name,
  tf2_ros::Buffer * tf,
  const nav2_util::LifecycleNode::WeakPtr & node,
  rclcpp::CallbackGroup::SharedPtr callback_group);

// 重置层
virtual void reset() = 0;

// 判断层是否可以被清除
virtual bool isClearable() = 0;

// 更新层在代价地图中的边界
virtual void updateBounds(
  double robot_x, double robot_y, double robot_yaw, double * min_x,
  double * min_y,
  double * max_x,
  double * max_y) = 0;

// 更新代价地图中的代价值
virtual void updateCosts(
  Costmap2D & master_grid,
  int min_i, int min_j, int max_i, int max_j) = 0;

// 使层的大小与父代价地图匹配
virtual void matchSize() {}
// 当机器人的足迹发生变化时调用
virtual void onFootprintChanged() {}
// 获取层的名字
std::string getName() const;
// 检查层的数据是否是最新的
bool isCurrent() const;
// 获取层是否启用
bool isEnabled() const;
// 获取层的足迹
const std::vector<geometry_msgs::msg::Point> & getFootprint() const;

// 声明 ROS 参数
void declareParameter(
  const std::string & param_name,
  const rclcpp::ParameterValue & value);

// 检查是否声明了 ROS 参数
bool hasParameter(const std::名称 & param_name);

// 获取 ROS 参数的全名
std::string getFullName(const std::string & param_name);

辅助方法

// 在初始化后调用，用于子类的初始化
void onInitialize();

数据成员

rclcpp_lifecycle::LifecycleNode::WeakPtr node_ 是一个弱引用指针，用于指向一个 LifecycleNode 对象。这个弱引用指针的主要用途是在不需要独占访问权的情况下引用 LifecycleNode 对象，以避免不必要的内存泄漏。

// 指向包含该层的代价地图的指针
LayeredCostmap * layered_costmap_;
// 层的名字
std::string name_;
// 变换缓冲区
tf2_ros::Buffer * tf_;
// ROS 回调组
rclcpp::CallbackGroup::SharedPtr callback_group_;
// ROS 节点弱引用
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
// 本地声明的 ROS 参数
std::unordered_set<std::string> local_params_;
// 足迹规格
std::vector<geometry_msgs::msg::Point> footprint_spec_;

`Layer`实现

由于是纯虚函数，大部分Layer类内的函数都是接口，均没有实现

初始化

对象设置了其父对象、名称、变换缓冲区、ROS 节点和回调组等参数，并调用子类的初始化方法。

void Layer::initialize(
  LayeredCostmap * parent,
  std::string name,
  tf2_ros::Buffer * tf,
  const nav2_util::LifecycleNode::WeakPtr & node,
  rclcpp::CallbackGroup::SharedPtr callback_group)
{
  //设置当前层对象为父类 `LayeredCostmap` 的指针
  layered_costmap_ = parent;
  name_ = name;
  tf_ = tf;
  node_ = node;
  callback_group_ = callback_group;
  {
    auto node_shared_ptr = node_.lock();
    logger_ = node_shared_ptr->get_logger();
    clock_ = node_shared_ptr->get_clock();
  }
  //这是子类必须实现的方法，用于执行子类的特定初始化操作。
  onInitialize();
}

`LayeredCostmap`—多层代价地图插件合体机

LayeredCostmap类用于实例化不同的层插件（layer plugins）并将它们集成到一个统一的代价地图中。

类的声明和构造函数

class LayeredCostmap
{
public:
  // 构造函数，初始化全局框架、滚动窗口等参数
  LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);
  // 析构函数，清理资源
  ~LayeredCostmap();
};

核心方法

// 更新底层代价地图
void updateMap(double robot_x, double robot_y, double robot_yaw);

// 获取全局框架ID
std::string getGlobalFrameID() const;

// 调整代价地图的大小、分辨率或原点
void resizeMap(
  unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
  double origin_y,
  bool size_locked = false);

// 获取更新后的边界
void getUpdatedBounds(double & minx, double & miny, double & maxx, double & maxy);

// 检查代价地图是否为最新数据
bool isCurrent();

// 获取主代价地图指针
Costmap2D * getCostmap();

// 添加新的层插件到插件列表
void addPlugin(std::shared_ptr<Layer> plugin);

// 添加新的代价地图过滤器插件到过滤器列表
void addFilter(std::shared_ptr<Layer> filter);

// 设置机器人的足迹
void setFootprint(const std::vector<geometry_msgs::msg::Point> & footprint_spec);

// 获取机器人的足迹
const std::vector<geometry_msgs::msg::Point> & getFootprint();

// 获取环绕半径
double getCircumscribedRadius();

// 获取内切半径
double getInscribedRadius();

// 检查机器人是否超出了代价地图的边界
bool isOutofBounds(double robot_x, double robot_y);

数据成员

// 主代价地图和最终合并后的代价地图
Costmap2D primary_costmap_, combined_costmap_;
// 全局框架ID
std::string global_frame_;
// 滚动窗口标志
bool rolling_window_;
// 是否跟踪未知空间
bool track_unknown_;
// 当前状态标志
bool current_;
// 更新后的边界
double minx_, miny_, maxx_, maxy_;
// 代价地图的边界
unsigned int bx0_, bxn_, by0_, byn_;
// 插件列表和过滤器列表
std::vector<std::shared_ptr<Layer>> plugins_, filters_;
// 是否初始化
bool initialized_;
// 是否锁定大小
bool size_locked_;
// 原子化环绕半径和内切半径
std::atomic<double> circumscribed_radius_, inscribed_radius_;
// 机器人的足迹
std::shared_ptr<std::vector<geometry_msgs::msg::Point>> footprint_;

`LayeredCostmap`实现

初始化

track_unknown这个标志指示是否应跟踪未知空间。
- 如果 track_unknown 标志被设置为 true，则 LayeredCostmap 对象将跟踪未知空间。这意味着如果某个区域在代价地图中没有明确的代价值（例如，由激光扫描仪扫描到的未知区域），那么这个区域将被视为具有高代价，从而在路径规划中被避免。
- 相反，如果 track_unknown 标志被设置为 false，则代价地图不跟踪未知空间。在这种情况下，代价地图的默认值被设置为 0，表示自由空间，允许机器人自由移动。

LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)

: primary_costmap_(), combined_costmap_(),
  global_frame_(global_frame),
  rolling_window_(rolling_window),
  current_(false),
  minx_(0.0),
  miny_(0.0),
  maxx_(0.0),
  maxy_(0.0),
  bx0_(0),
  bxn_(0),
  by0_(0),
  byn_(0),
  initialized_(false),
  size_locked_(false),
  circumscribed_radius_(1.0),
  inscribed_radius_(0.1),
  footprint_(std::make_shared<std::vector<geometry_msgs::msg::Point>>())
{
  if (track_unknown) {
    primary_costmap_.setDefaultValue(255);
    combined_costmap_.setDefaultValue(255);
  } else {
    primary_costmap_.setDefaultValue(0);
    combined_costmap_.setDefaultValue(0);
  }
}

载入插件和过滤器

依次根据传入的插件push到插件容器中

void LayeredCostmap::addPlugin(std::shared_ptr<Layer> plugin)
{
  std::unique_lock<Costmap2D::mutex_t> lock(*(combined_costmap_.getMutex()));
  plugins_.push_back(plugin);
}
void LayeredCostmap::addFilter(std::shared_ptr<Layer> filter)
{
  std::unique_lock<Costmap2D::mutex_t> lock(*(combined_costmap_.getMutex()));
  filters_.push_back(filter);
}

更新代价地图（核心）

updateMap更新总地图层的核心函数，以下这部分我们将详细拆解其中的所有函数

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
//以下本小节所有内容都在这个函数里头
}

这里先获取了主代价地图 combined_costmap_ 的互斥锁，以确保在更新代价地图时不会发生竞态条件。这是因为在某些插件（如 VoxelLayer）的 updateBounds 函数中可能存在线程不安全的操作。

std::unique_lock<Costmap2D::mutex_t> lock(*(combined_costmap_.getMutex()));

启用滚动窗口rolling_window_，则根据机器人的位置更新主代价地图和最终合并后的代价地图的原点。滚动窗口是一种技术，允许代价地图随着机器人的移动而更新，而不是重新创建整个地图。

if (rolling_window_) 
{ 
double new_origin_x = robot_x - combined_costmap_.getSizeInMetersX() / 2; double new_origin_y = robot_y - combined_costmap_.getSizeInMetersY() / 2; primary_costmap_.updateOrigin(new_origin_x, new_origin_y); combined_costmap_.updateOrigin(new_origin_x, new_origin_y); 
}

检查机器人是否超出代价地图边界

  if (isOutofBounds(robot_x, robot_y)) {
    RCLCPP_WARN(
      rclcpp::get_logger("nav2_costmap_2d"),
      "Robot is out of bounds of the costmap!");
  }

更新代价地图的区域
- 依次更新插件plugin和过滤器filter的updateBounds(是一个纯虚函数，由具体的插件实现)，更新代价地图的边界
- 同时如果边界发生了非法变化，即新边界超出了原边界，它会打印警告信息，并记录了造成非法边界变化的插件或过滤器的名称。

 if (plugins_.size() == 0 && filters_.size() == 0) {
    return;
  }

  minx_ = miny_ = std::numeric_limits<double>::max();
  maxx_ = maxy_ = std::numeric_limits<double>::lowest();

  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
    plugin != plugins_.end(); ++plugin)
  {
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) {
      RCLCPP_WARN(
        rclcpp::get_logger(
          "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
        "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s",
        prev_minx, prev_miny, prev_maxx, prev_maxy,
        minx_, miny_, maxx_, maxy_,
        (*plugin)->getName().c_str());
    }
  }
  for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
    filter != filters_.end(); ++filter)
  {
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    (*filter)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) {
      RCLCPP_WARN(
        rclcpp::get_logger(
          "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
        "is now [tl: (%f, %f), br: (%f, %f)]. The offending filter is %s",
        prev_minx, prev_miny, prev_maxx, prev_maxy,
        minx_, miny_, maxx_, maxy_,
        (*filter)->getName().c_str());
    }
  }

负责计算代价地图需要更新的区域，并确保这个区域是有效的，即不会超出代价地图的边界。

int x0, xn, y0, yn;
  combined_costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
  combined_costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

  x0 = std::max(0, x0);
  xn = std::min(static_cast<int>(combined_costmap_.getSizeInCellsX()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(static_cast<int>(combined_costmap_.getSizeInCellsY()), yn + 1);

  RCLCPP_DEBUG(
    rclcpp::get_logger(
      "nav2_costmap_2d"), "Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);

  if (xn < x0 || yn < y0) {
    return;
  }

如果没启用过滤器，则按照插件的顺序调用方法updateCosts更新代价地图

if (filters_.size() == 0) {

    combined_costmap_.resetMap(x0, y0, xn, yn);
    for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
      plugin != plugins_.end(); ++plugin)
    {
      (*plugin)->updateCosts(combined_costmap_, x0, y0, xn, yn);
    }
  } else {
	//见下
	}

如果启用了过滤器，安装下列步骤进行处理
- 1.先由插件更新代价地图
- 2.对窗口进行拷贝到最终代价地图。 primary_costmap_保持不变，供插件进一步使用。
- 1. 按顺序应用过滤器，以便过滤器的工作不被插件在下次updateMap()调用时考虑
- 更新边界
- 标记代价地图已初始化:initialized_ = true;

 else {
 //1.先由插件更新代价地图
    primary_costmap_.resetMap(x0, y0, xn, yn);
    for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
      plugin != plugins_.end(); ++plugin)
    {
      (*plugin)->updateCosts(primary_costmap_, x0, y0, xn, yn);
    }

//2.对窗口进行拷贝到最终代价地图。 `primary_costmap_`保持不变，供插件进一步使用。
    if (!combined_costmap_.copyWindow(primary_costmap_, x0, y0, xn, yn, x0, y0)) {
      RCLCPP_ERROR(
        rclcpp::get_logger("nav2_costmap_2d"),
        "Can not copy costmap (%i,%i)..(%i,%i) window",
        x0, y0, xn, yn);
      throw std::runtime_error{"Can not copy costmap"};
    }

    //3. 按顺序应用过滤器，以便过滤器的工作不被插件在下次updateMap()调用时考虑
    for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
      filter != filters_.end(); ++filter)
    {
      (*filter)->updateCosts(combined_costmap_, x0, y0, xn, yn);
    }
  }
  //更新边界
  bx0_ = x0;
  bxn_ = xn;
  by0_ = y0;
  byn_ = yn;

  initialized_ = true;
  }

设置机器人足迹

calculateMinAndMaxDistances 函数计算机器人的内切半径（inscribed radius）和环绕半径（circumscribed radius）。这两个半径分别表示机器人的最小和最大安全移动范围，它们是基于机器人的足迹计算得出的。
std::atomic_store 函数将新的机器人足迹更新到 footprint_ 成员变量中。由于机器人足迹被多个规划器/控制器使用，并且不是通过其他方式锁定的，因此使用原子操作来确保在多线程环境中的一致性。
最后遍历 LayeredCostmap 对象的 plugins_ 和 filters_ 两个向量，并调用每个插件的 onFootprintChanged 方法通知所有层改变各个层的机器人足迹。

void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::msg::Point> & footprint_spec)
{
//计算内切半径（inscribed radius）和环绕半径（circumscribed radius）
  std::pair<double, double> inside_outside = nav2_costmap_2d::calculateMinAndMaxDistances(footprint_spec);

  std::atomic_store(
    &footprint_,
    std::make_shared<std::vector<geometry_msgs::msg::Point>>(footprint_spec));
  inscribed_radius_.store(std::get<0>(inside_outside));
  circumscribed_radius_.store(std::get<1>(inside_outside));

  for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
    plugin != plugins_.end();
    ++plugin)
  {
    (*plugin)->onFootprintChanged();
  }
  for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
    filter != filters_.end();
    ++filter)
  {
    (*filter)->onFootprintChanged();
  }
}

`Costmap2DLayer`—多层代价地图插件的抽象基类

Costmap2DLayer类也是一个抽象基类，用于定义多层代价地图插件（Layered Costmap Plugin）中层的接口。它继承自 Layer 类和 Costmap2D 类，提供了用于代价地图插件层的通用功能。
该对象不仅包含一个Layer层，还包含一个内部costmap对象来填充和维护状态。

核心方法

// 是否离散化
bool isDiscretized();

// 匹配大小
virtual void matchSize();

// 清除区域
virtual void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert);

// 添加额外边界
void addExtraBounds(double mx0, double my0, double mx1, double my1);

// 更新主代价地图
void updateWithTrueOverwrite(
  nav2_costmap_2d::Costmap2D & master_grid,
  int min_i, int min_j, int max_i, int max_j);

// 更新主代价地图（非覆盖）
void updateWithOverwrite(
  nav2_costmap_2d::Costmap2D & master_grid,
  int min_i, int min_j, int max_i, int max_j);

// 更新主代价地图（最大值）
void updateWithMax(
  nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
  int max_j);

// 更新主代价地图（最大值，不覆盖未知区域）
void updateWithMaxWithoutUnknownOverwrite(
  nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
  int max_j);

// 更新主代价地图（加法）
void updateWithAddition(
  nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
  int max_j);

// 
void touch(double x, double y, double * min_x, double * min_y, double * max_x, double * max_y);

// 使用额外边界
void useExtraBounds(double * min_x, double * min_y, double * max_x, double * max_y);

数据成员

额外边界（extra bounds）是ROS2导航系统中的一种机制，用于扩展代价图（Costmap）的更新区域。在CostmapLayer类中，通过addExtraBounds方法可以设置一个额外的矩形区域，这个区域可以被包含在代价图的更新操作中。
当代价图层需要更新其代价映射时，它会首先使用默认的更新区域，即代价图本身。如果指定了额外边界，这个额外的矩形区域会被添加到代价图的更新操作中，从而使得代价图的更新区域不仅限于代价图本身，还包括了额外的边界区域。

// 额外边界
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
// 是否使用额外边界
bool has_extra_bounds_;

CostmapLayer实现

更新主代价地图

该方法用于将代价图层的代价映射完全覆盖到主代价图中，即主代价图的每个单元格的值都会被代价图层的对应单元格的值所替换。

void CostmapLayer::updateWithTrueOverwrite(
  nav2_costmap_2d::Costmap2D & master_grid, int min_i,
  int min_j,
  int max_i,
  int max_j)
{
  if (!enabled_) {
    return;
  }

  if (costmap_ == nullptr) {
    throw std::runtime_error("Can't update costmap layer: It has't been initialized yet!");
  }

  unsigned char * master = master_grid.getCharMap();
  unsigned int span = master_grid.getSizeInCellsX();

  for (int j = min_j; j < max_j; j++) {
    unsigned int it = span * j + min_i;
    for (int i = min_i; i < max_i; i++) {
      master[it] = costmap_[it];
      it++;
    }
  }
}

更新代价地图的边界框

touch函数用于更新代价图层的边界框，以包含指定的(x, y)位置。

void CostmapLayer::touch(
  double x, double y, double * min_x, double * min_y, double * max_x,
  double * max_y)
{
  *min_x = std::min(x, *min_x);
  *min_y = std::min(y, *min_y);
  *max_x = std::max(x, *max_x);
  *max_y = std::max(y, *max_y);
}

匹配主代价地图的大小

使用layered_costmap_->getCostmap()获取主代价图的尺寸，然后使用resizeMap()调整代价图层的大小。
确保代价图层和主代价图具有相同的尺寸，以便正确地更新代价图层。

void CostmapLayer::matchSize()
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  Costmap2D * master = layered_costmap_->getCostmap();
  resizeMap(
    master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
    master->getOriginX(), master->getOriginY());
}

清空指定区域。

清空代价图中的一个区域
如果invert为真，则清空除指定区域外的所有内容。

void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert)
{
  current_ = false;
  unsigned char * grid = getCharMap();
  for (int x = 0; x < static_cast<int>(getSizeInCellsX()); x++) {
    bool xrange = x > start_x && x < end_x;

    for (int y = 0; y < static_cast<int>(getSizeInCellsY()); y++) {
      if ((xrange && y > start_y && y < end_y) == invert) {
        continue;
      }
      int index = getIndex(x, y);
      if (grid[index] != NO_INFORMATION) {
        grid[index] = NO_INFORMATION;
      }
    }
  }
}

添加和使用额外的代价地图边界

添加额外的边界

void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
{
  extra_min_x_ = std::min(mx0, extra_min_x_);
  extra_max_x_ = std::max(mx1, extra_max_x_);
  extra_min_y_ = std::min(my0, extra_min_y_);
  extra_max_y_ = std::max(my1, extra_max_y_);
  has_extra_bounds_ = true;
}

使用额外边界更新代价图层的边界框。

void CostmapLayer::useExtraBounds(double * min_x, double * min_y, double * max_x, double * max_y)
{
  if (!has_extra_bounds_) {
    return;
  }

  *min_x = std::min(extra_min_x_, *min_x);
  *min_y = std::min(extra_min_y_, *min_y);
  *max_x = std::max(extra_max_x_, *max_x);
  *max_y = std::max(extra_max_y_, *max_y);
  extra_min_x_ = 1e6;
  extra_min_y_ = 1e6;
  extra_max_x_ = -1e6;
  extra_max_y_ = -1e6;
  has_extra_bounds_ = false;
}

`StaticLayer`—静态地图层

静态地图层（Static Layer）是机器人导航系统中的一种多层代价地图插件（Layered Costmap Plugin）。它用于将来自 SLAM（Simultaneous Localization and Mapping）或其他地图生成方法的静态地图集成到代价地图中。
它继承自Costmap2DLayer类

核心方法

// 初始化层
void onInitialize();

// 激活层
void activate();

// 停用层
void deactivate();

// 重置代价图
void reset();

// 更新主代价图的边界
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(
  nav2_costmap_2d::Costmap2D & master_grid,
  int min_i, int min_j, int max_i, int max_j);

// 匹配主代价图的大小
void matchSize();

// 处理来自话题的新地图
void processMap(const nav_msgs::msg::OccupancyGrid & new_map);

// 处理来自地图服务器的地图更新
void incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map);

// 处理来自地图服务器的特定地图区域更新
void incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update);

// 解释静态地图值以转换为代价
unsigned char interpretValue(unsigned char value);

// 动态参数回调
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);

数据成员

// 转换后的机器人足迹
std::vector<geometry_msgs::msg::Point> transformed_footprint_;
// 是否启用足迹清理
bool footprint_clearing_enabled_;
// 更新足迹
void updateFootprint(
  double robot_x, double robot_y, double robot_yaw, double * min_x,
  double * min_y,
  double * max_x,
  double * max_y);

// 全局框架
std::string global_frame_;
// 地图框架
std::string map_frame_;

// 是否已更新数据
bool has_updated_data_;

// 订阅者
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_update_sub_;

// 参数
std::string map_topic_;
bool map_subscribe_transient_local_;
bool subscribe_to_updates_;
bool track_unknown_space_;
bool use_maximum_;
unsigned char lethal_threshold_;
unsigned char unknown_cost_value_;
bool trinary_costmap_;
bool map_received_;
bool map_received_in_update_bounds_;
tf2::Duration transform_tolerance_;
nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_;
// 动态参数处理句柄
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;

`StaticLayer`实现

参数获取

enabled：代价图层是否启用。默认值为true。
subscribe_to_updates：代价图层是否应订阅地图的更新。默认值为false。
map_subscribe_transient_local：代价图层是否应以瞬时局部（transient local）的方式订阅地图话题。默认值为true。
transform_tolerance：变换容忍度，即坐标变换时的最大误差。默认值为0.0。
map_topic：代价图层订阅的地图话题。默认值为空字符串。
footprint_clearing_enabled：代价图层是否应清理机器人的足迹。默认值为false。
track_unknown_space：代价图层是否应跟踪未知空间。默认值为false。
use_maximum：代价图层是否应使用最大值更新主代价图。默认值为false。
lethal_cost_threshold：致命代价阈值。默认值为0。
unknown_cost_value：未知代价值。默认值为0。
trinary_costmap：代价图层是否应使用三元代价图。默认值为false。

void StaticLayer::getParameters()
{
  int temp_lethal_threshold = 0;
  double temp_tf_tol = 0.0;

  declareParameter("enabled", rclcpp::ParameterValue(true));
  declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false));
  declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true));
  declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0));
  declareParameter("map_topic", rclcpp::ParameterValue(""));
  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false));

  auto node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }

  node->get_parameter(name_ + "." + "enabled", enabled_);
  node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
  std::string private_map_topic, global_map_topic;
  node->get_parameter(name_ + "." + "map_topic", private_map_topic);
  node->get_parameter("map_topic", global_map_topic);
  if (!private_map_topic.empty()) {
    map_topic_ = private_map_topic;
  } else {
    map_topic_ = global_map_topic;
  }
  node->get_parameter(
    name_ + "." + "map_subscribe_transient_local",
    map_subscribe_transient_local_);
  node->get_parameter("track_unknown_space", track_unknown_space_);
  node->get_parameter("use_maximum", use_maximum_);
  node->get_parameter("lethal_cost_threshold", temp_lethal_threshold);
  node->get_parameter("unknown_cost_value", unknown_cost_value_);
  node->get_parameter("trinary_costmap", trinary_costmap_);
  node->get_parameter("transform_tolerance", temp_tf_tol);

  // 参数设置
  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
  map_received_ = false;
  map_received_in_update_bounds_ = false;

  transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);

  // 为动态参数回调添加回调组
  dyn_params_handler_ = node->add_on_set_parameters_callback(
    std::bind(
      &StaticLayer::dynamicParametersCallback,
      this, std::placeholders::_1));
}

动态参数回调

dynamicParametersCallback方法的具体实现，用于处理动态参数的更新。
- 遍历参数列表parameters中的每个参数。
- 对特定参数进行特判，更新

rcl_interfaces::msg::SetParametersResult StaticLayer::dynamicParametersCallback(
  std::vector<rclcpp::Parameter> parameters)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  rcl_interfaces::msg::SetParametersResult result;
//遍历参数列表parameters中的每个参数。
  for (auto parameter : parameters) {
    const auto & param_type = parameter.get_type();
    const auto & param_name = parameter.get_name();

    if (param_name == name_ + "." + "map_subscribe_transient_local" ||
      param_name == name_ + "." + "map_topic" ||
      param_name == name_ + "." + "subscribe_to_updates")
    {
      RCLCPP_WARN(
        logger_, "%s is not a dynamic parameter "
        "cannot be changed while running. Rejecting parameter update.", param_name.c_str());
    } else if (param_type == ParameterType::PARAMETER_DOUBLE) {
      if (param_name == name_ + "." + "transform_tolerance") {
        transform_tolerance_ = tf2::durationFromSec(parameter.as_double());
      }
    } else if (param_type == ParameterType::PARAMETER_BOOL) {
      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
        enabled_ = parameter.as_bool();

        x_ = y_ = 0;
        width_ = size_x_;
        height_ = size_y_;
        has_updated_data_ = true;
        current_ = false;
      }
    } else if (param_type == ParameterType::PARAMETER_BOOL) {
      if (param_name == name_ + "." + "footprint_clearing_enabled") {
        footprint_clearing_enabled_ = parameter.as_bool();
      }
    }
  }
  result.successful = true;
  return result;
}

初始化

onInitialize方法的具体实现，用于在层初始化时执行的设置和订阅操作
- 获取代价图层集合（layered_costmap_）的全局框架ID
- 调用getParameters方法来获取代价图层的参数。
- 订阅代价地图
- 如果需要订阅更新，订阅更新代价地图

void
StaticLayer::onInitialize()
{
  global_frame_ = layered_costmap_->getGlobalFrameID();

  getParameters();//获取参数

  rclcpp::QoS map_qos(10);  // initialize to default
  if (map_subscribe_transient_local_) {
    map_qos.transient_local();
    map_qos.reliable();
    map_qos.keep_last(1);
  }

  RCLCPP_INFO(
    logger_,
    "Subscribing to the map topic (%s) with %s durability",
    map_topic_.c_str(),
    map_subscribe_transient_local_ ? "transient local" : "volatile");

  auto node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }

  map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
    map_topic_, map_qos,
    std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1));

  if (subscribe_to_updates_) {
    RCLCPP_INFO(logger_, "Subscribing to updates");
    map_update_sub_ = node->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
      map_topic_ + "_updates",
      rclcpp::SystemDefaultsQoS(),
      std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
  }
}

代价地图订阅回调

调用nav2_util::validateMsg函数来验证新地图消息的完整性。
将新地图消息存储在map_buffer_变量中，以便在需要时使用
调用processMap处理新地图

void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
{
  if (!nav2_util::validateMsg(*new_map)) {
    RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting.");
    return;
  }
  if (!map_received_) {
    processMap(*new_map);
    map_received_ = true;
    return;
  }
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  map_buffer_ = new_map;
}

代价地图处理

用于处理从话题接收的新地图。
- 判断地图是否需要更新–大小、分辨率和原点是否与新地图匹配。
- 如果改变— 调整代价图层集合的尺寸以匹配新地图，并更新所有层，包括当前层。
- 更新新地图代价值

void
StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
{
  RCLCPP_DEBUG(logger_, "StaticLayer: Process map");

  unsigned int size_x = new_map.info.width;
  unsigned int size_y = new_map.info.height;

  RCLCPP_DEBUG(
    logger_,
    "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y,
    new_map.info.resolution);

  // - 检查代价图层的大小、分辨率和原点是否与新地图匹配。如果不匹配，则继续执行。
  Costmap2D * master = layered_costmap_->getCostmap();
  if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
    master->getSizeInCellsY() != size_y ||
    master->getResolution() != new_map.info.resolution ||
    master->getOriginX() != new_map.info.origin.position.x ||
    master->getOriginY() != new_map.info.origin.position.y ||
    !layered_costmap_->isSizeLocked()))
  {
    // 调整代价图层集合的尺寸以匹配新地图，并更新所有层，包括当前层。
    RCLCPP_INFO(
      logger_,
      "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
      new_map.info.resolution);
    layered_costmap_->resizeMap(
      size_x, size_y, new_map.info.resolution,
      new_map.info.origin.position.x,
      new_map.info.origin.position.y,
      true);
  } else if (size_x_ != size_x || size_y_ != size_y ||  // NOLINT
    resolution_ != new_map.info.resolution ||
    origin_x_ != new_map.info.origin.position.x ||
    origin_y_ != new_map.info.origin.position.y)
  {
    // 如果代价图层不是滚动式的，并且代价图层的主代价图的尺寸、分辨率或原点与新地图不匹配，或者代价图层的大小没有被锁定，则继续执行。
    RCLCPP_INFO(
      logger_,
      "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
      new_map.info.resolution);
    resizeMap(
      size_x, size_y, new_map.info.resolution,
      new_map.info.origin.position.x, new_map.info.origin.position.y);
  }

  unsigned int index = 0;

  
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());

  // 更新地图代价
  for (unsigned int i = 0; i < size_y; ++i) {
    for (unsigned int j = 0; j < size_x; ++j) {
      unsigned char value = new_map.data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }

  map_frame_ = new_map.header.frame_id;

  x_ = y_ = 0;
  width_ = size_x_;
  height_ = size_y_;
  has_updated_data_ = true;

  current_ = true;
}

静态地图值转换函数

用于解释静态地图值并将其转换为代价图可以使用的代价。
- NO_INFORMATION
  - 表示没有足够的信息来确定当前单元格的代价。
  - 通常用于新获取的地图区域，或者由于某些原因（如遮挡或传感器故障）而无法确定代价的区域。
  - 在导航中，通常不会将机器人引导到NO_INFORMATION区域，因为这可能意味着未知的风险。
- FREE_SPACE
  - 表示当前单元格是机器人可以自由移动的空间。
  - 机器人可以安全地导航到这个区域，不会遇到障碍物。
  - FREE_SPACE通常对应于地图上的无障碍区域。
- LETHAL_OBSTACLE
  - 表示当前单元格是机器人无法通过的障碍物。
  - 机器人不应该尝试导航到这个区域，因为它可能导致碰撞或其他危险。
  - LETHAL_OBSTACLE通常对应于地图上的不可通过区域，如墙壁、不可移动的物体或危险区域。`

unsigned char
StaticLayer::interpretValue(unsigned char value)
{
  // 
  if (track_unknown_space_ && value == unknown_cost_value_) {
    return NO_INFORMATION;
  } else if (!track_unknown_space_ && value == unknown_cost_value_) {
    return FREE_SPACE;
  } else if (value >= lethal_threshold_) {
    return LETHAL_OBSTACLE;
  } else if (trinary_costmap_) {
    return FREE_SPACE;
  }

  double scale = static_cast<double>(value) / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}

更新代价地图订阅回调

用于处理来自地图话题的更新消息。
- 检查更新消息的边界是否超出代价图层的边界。
- 将更新消息中的值转换为代价图可以使用的值，并更新代价图。

void
StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  if (update->y < static_cast<int32_t>(y_) ||
    y_ + height_ < update->y + update->height ||
    update->x < static_cast<int32_t>(x_) ||
    x_ + width_ < update->x + update->width)
  {
    RCLCPP_WARN(
      logger_,
      "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
      "Static layer origin: %d, %d   bounds: %d X %d\n"
      "Update origin: %d, %d   bounds: %d X %d",
      x_, y_, width_, height_, update->x, update->y, update->width,
      update->height);
    return;
  }

  if (update->header.frame_id != map_frame_) {
    RCLCPP_WARN(
      logger_,
      "StaticLayer: Map update ignored. Current map is in frame %s "
      "but update was in frame %s",
      map_frame_.c_str(), update->header.frame_id.c_str());
    return;
  }

  unsigned int di = 0;
  for (unsigned int y = 0; y < update->height; y++) {
    unsigned int index_base = (update->y + y) * size_x_;
    for (unsigned int x = 0; x < update->width; x++) {
      unsigned int index = index_base + x + update->x;
      costmap_[index] = interpretValue(update->data[di++]);
    }
  }

  has_updated_data_ = true;
}

更新边界

用于根据机器人的位置和方向更新代价图层的边界
更新机器人足迹

void
StaticLayer::updateBounds(
  double robot_x, double robot_y, double robot_yaw, double * min_x,
  double * min_y,
  double * max_x,
  double * max_y)
{
  if (!map_received_) {
    map_received_in_update_bounds_ = false;
    return;
  }
  map_received_in_update_bounds_ = true;

  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());

  if (map_buffer_) {
    processMap(*map_buffer_);
    map_buffer_ = nullptr;
  }

  if (!layered_costmap_->isRolling() ) {
    if (!(has_updated_data_ || has_extra_bounds_)) {
      return;
    }
  }

  useExtraBounds(min_x, min_y, max_x, max_y);

  double wx, wy;

  mapToWorld(x_, y_, wx, wy);
  *min_x = std::min(wx, *min_x);
  *min_y = std::min(wy, *min_y);

  mapToWorld(x_ + width_, y_ + height_, wx, wy);
  *max_x = std::max(wx, *max_x);
  *max_y = std::max(wy, *max_y);

  has_updated_data_ = false;

  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}

更新机器人足迹

用于根据机器人的位置、方向和足迹更新代价图层的边界。

void
StaticLayer::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_);

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

更新代价地图代价

据代价图层的代价映射更新主代价图的代价
- 检查代价图层是否被启用。如果未启用，则直接返回。
- 检查在更新边界时是否接收到地图map_received_in_update_bounds_。如果没有接收到，则每10次调用打印一次警告信息，然后返回。
- 检查是否启用了footprint_clearing_enabled_足迹清理功能。如果启用了，则将转换后的足迹设置为自由空间代价FREE_SPACE。
- 检查代价图层集合是否是滚动式的isRolling。如果不是，则继续执行。
- 检查是否使用最大值更新use_maximum_。
  - 如果不是，则使用完全覆盖updateWithTrueOverwrite的方式更新主代价图；
  - 如果是，则使用最大值updateWithMax更新主代价图。
- 如果代价图层集合是滚动式的，则执行以下操作：
  - 尝试获取map_frame_机器人坐标系和global_frame_世界坐标系之间的变换。
  - mapToWorld将主代价图的每个单元格坐标从机器人坐标系转换到世界坐标系。
  - worldToMap将世界坐标系中的每个单元格坐标转换回代价图层坐标系。
  - 根据代价图层的代价映射更新主代价图的对应单元格。
- 将当前代价图层设置为活跃的。current_

void
StaticLayer::updateCosts(
  nav2_costmap_2d::Costmap2D & master_grid,
  int min_i, int min_j, int max_i, int max_j)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  if (!enabled_) {
    return;
  }
  if (!map_received_in_update_bounds_) {
    static int count = 0;
    // throttle warning down to only 1/10 message rate
    if (++count == 10) {
      RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received");
      count = 0;
    }
    return;
  }

  if (footprint_clearing_enabled_) {
    setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
  }

  if (!layered_costmap_->isRolling()) {
    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
    if (!use_maximum_) {
      updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
    } else {
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
    }
  } else {
    // If rolling window, the master_grid is unlikely to have same coordinates as this layer
    unsigned int mx, my;
    double wx, wy;
    // Might even be in a different frame
    geometry_msgs::msg::TransformStamped transform;
    try {
      transform = tf_->lookupTransform(
        map_frame_, global_frame_, tf2::TimePointZero,
        transform_tolerance_);
    } catch (tf2::TransformException & ex) {
      RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what());
      return;
    }
    // Copy map data given proper transformations
    tf2::Transform tf2_transform;
    tf2::fromMsg(transform.transform, tf2_transform);

    for (int i = min_i; i < max_i; ++i) {
      for (int j = min_j; j < max_j; ++j) {
        // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        // Transform from global_frame_ to map_frame_
        tf2::Vector3 p(wx, wy, 0);
        p = tf2_transform * p;
        // Set master_grid with cell from map
        if (worldToMap(p.x(), p.y(), mx, my)) {
          if (!use_maximum_) {
            master_grid.setCost(i, j, getCost(mx, my));
          } else {
            master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
          }
        }
      }
    }
  }
  current_ = true;
}

`ObstacleLayer`—障碍地图层

ObstacleLayer的类，它是CostmapLayer类的子类，用于处理激光扫描和点云数据，并将这些数据填充到二维代价图（costmap）中。

核心方法

// 构造函数
ObstacleLayer();

// 析构函数
virtual ~ObstacleLayer();

// 初始化层
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(
  nav2_costmap_2d::Costmap2D & master_grid,
  int min_i, int min_j, int max_i, int max_j);

// 停用层
virtual void deactivate();

// 激活层
virtual void activate();

// 重置代价图
virtual void reset();

// 代价图是否可清理
virtual bool isClearable() {return true;}

// 动态参数回调
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);

// 重置观察缓冲区
void resetBuffersLastUpdated();

// 激光扫描回调
void laserScanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr message, const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer);

// 激光扫描有效无穷大值回调
void laserScanValidInfCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr message, const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer);

// 点云2回调
void pointCloud2Callback(sensor_msgs::msg::PointCloud2::ConstSharedPtr message, const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer);

// 测试用：添加静态观察
void addStaticObservation(nav2_costmap_2d::Observation & obs, bool marking, bool clearing);

// 测试用：清除静态观察
void clearStaticObservations(bool marking, bool clearing);

数据成员

// 转换后的足迹
std::vector<geometry_msgs::msg::Point> transformed_footprint_;
// 是否启用足迹清理
bool footprint_clearing_enabled_;
// 全局框架
std::string global_frame_;
// 最小障碍物高度
double min_obstacle_height_;
// 最大障碍物高度
double max_obstacle_height_;
// 激光投影
laser_geometry::LaserProjection projector_;
// 观察消息过滤器
std::vector<std::shared_ptr<message_filters::SubscriberBase<rclcpp_lifecycle::LifecycleNode>>> observation_subscribers_;
// 确保传感器变换可用
std::vector<std::shared_ptr<tf2_ros::MessageFilterBase>> observation_notifiers_;
// 存储各种传感器的观察
std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> observation_buffers_;
// 用于标记障碍物的观察缓冲区
std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> marking_buffers_;
// 用于清除障碍物的观察缓冲区
std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> clearing_buffers_;
// 动态参数处理句柄
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
// 测试用：静态清除观察
std::vector<nav2_costmap_2d::Observation> static_clearing_observations_;
// 测试用：静态标记观察
std::vector<nav2_costmap_2d::Observation> static_marking_observations_;
// 滚动窗口
bool rolling_window_;
// 是否已重置
bool was_reset_;
// 组合方法
nav2_costmap_2d::CombinationMethod combination_method_;

`ObstacleLayer`实现

初始化

onInitialize用于在层初始化时执行的设置和订阅操作。以下是该方法的步骤总结：
- 从参数服务器获取代价图层的配置参数，
  - enabled用于指示ObstacleLayer是否被启用。
  - footprint_clearing_enabled用于指示是否启用脚印清理功能，即在代价图层中清除机器人脚印下的区域。
  - min_obstacle_height和max_obstacle_height最小最大障碍物高度。
  - combination_method代价图层如何组合新的传感器数据与现有的代价图数据。
  - observation_sources：定义代价图层订阅的观测源
- 创建动态参数处理句柄，设置dynamicParametersCallback用于处理参数的变化。
- 获取滚动窗口状态，用于确定是否需要进行坐标变换。
- 根据是否track_unknown_space跟踪未知空间，设置默认代价值。
  - 是default_value_ = NO_INFORMATION;
  - 不是default_value_ = FREE_SPACE;
- 匹配代价图层的大小，确保与主代价图层匹配。 ObstacleLayer::matchSize();
- 设置当前代价图层为活跃的，并设置是否已重置标志。
- 获取全局框架，用于坐标变换。 global_frame_ = layered_costmap_->getGlobalFrameID();
- 遍历配置的观测源，并为每个观测源创建一个观察缓冲区。std::string source;
- 声明缓冲区参数
  - source.topic指定观测源订阅的主题名称。
  - source.sensor_frame指定传感器数据的坐标系
  - source.observation_persistence观测数据在代价图层中的持续时间
  - source.expected_update_rate观测数据的预期更新频率。
  - source.data_type观测源的数据类型。
  - source.min_obstacle_height和source.max_obstacle_height代价图层处理的最小最大障碍物高度。
  - source.inf_is_valid观测数据中的无穷大值是否有效。
  - source.marking用于标记代价图中的障碍物。
  - source.clearing是否将观测数据用于清除代价图中的障碍物。
  - source.obstacle_max_range和obstacle_min_range":障碍物最大和最小障碍物距离。
  - source.raytrace_max_range和source.raytrace_min_range指定代价图层进行射线追踪时的最大和最小距离。
- 载入缓冲区
```
observation_buffers_.push_back(
```
```
std::shared_ptr<ObservationBuffer
>(
  new ObservationBuffer(
    node, topic, observation_keep_time, expected_update_rate,
    min_obstacle_height,
    max_obstacle_height, obstacle_max_range, obstacle_min_range, raytrace_max_range,
    raytrace_min_range, *tf_,
    global_frame_,
    sensor_frame, tf2::durationFromSec(transform_tolerance))));
  ```
```
- 为每个观测源创建一个订阅者，订阅相应的话题，并创建一个过滤器来处理坐标变换。
  - LaserScan
  - PointCloud2
- 设置过滤器的目标框架，用于坐标变换。


void ObstacleLayer::onInitialize()
{
  bool track_unknown_space;
  double transform_tolerance;

  // The topics that we'll subscribe to from the parameter server
  std::string topics_string;

  declareParameter("enabled", rclcpp::ParameterValue(true));
  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
  declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
  declareParameter("combination_method", rclcpp::ParameterValue(1));
  declareParameter("observation_sources", rclcpp::ParameterValue(std::string("")));

  auto node = node_.lock();
  if (!node) {
    throw std::runtime_error{"Failed to lock node"};
  }

  node->get_parameter(name_ + "." + "enabled", enabled_);
  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
  node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
  node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
  node->get_parameter("track_unknown_space", track_unknown_space);
  node->get_parameter("transform_tolerance", transform_tolerance);
  node->get_parameter(name_ + "." + "observation_sources", topics_string);

  int combination_method_param{};
  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
  combination_method_ = combination_method_from_int(combination_method_param);

  dyn_params_handler_ = node->add_on_set_parameters_callback(
    std::bind(
      &ObstacleLayer::dynamicParametersCallback,
      this,
      std::placeholders::_1));

  RCLCPP_INFO(
    logger_,
    "Subscribed to Topics: %s", topics_string.c_str());

  rolling_window_ = layered_costmap_->isRolling();

  if (track_unknown_space) {
    default_value_ = NO_INFORMATION;
  } else {
    default_value_ = FREE_SPACE;
  }

  ObstacleLayer::matchSize();
  current_ = true;
  was_reset_ = false;

  global_frame_ = layered_costmap_->getGlobalFrameID();

  auto sub_opt = rclcpp::SubscriptionOptions();
  sub_opt.callback_group = callback_group_;

  // now we need to split the topics based on whitespace which we can use a stringstream for
  std::stringstream ss(topics_string);

  std::string source;
  while (ss >> source) {
    // get the parameters for the specific topic
    double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
    std::string topic, sensor_frame, data_type;
    bool inf_is_valid, clearing, marking;

    declareParameter(source + "." + "topic", rclcpp::ParameterValue(source));
    declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));
    declareParameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0));
    declareParameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0));
    declareParameter(source + "." + "data_type", rclcpp::ParameterValue(std::string("LaserScan")));
    declareParameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));
    declareParameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(0.0));
    declareParameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));
    declareParameter(source + "." + "marking", rclcpp::ParameterValue(true));
    declareParameter(source + "." + "clearing", rclcpp::ParameterValue(false));
    declareParameter(source + "." + "obstacle_max_range", rclcpp::ParameterValue(2.5));
    declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0));
    declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0));
    declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0));

    node->get_parameter(name_ + "." + source + "." + "topic", topic);
    node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame);
    node->get_parameter(
      name_ + "." + source + "." + "observation_persistence",
      observation_keep_time);
    node->get_parameter(
      name_ + "." + source + "." + "expected_update_rate",
      expected_update_rate);
    node->get_parameter(name_ + "." + source + "." + "data_type", data_type);
    node->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height);
    node->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height);
    node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
    node->get_parameter(name_ + "." + source + "." + "marking", marking);
    node->get_parameter(name_ + "." + source + "." + "clearing", clearing);

    if (!(data_type == "PointCloud2" || data_type == "LaserScan")) {
      RCLCPP_FATAL(
        logger_,
        "Only topics that use point cloud2s or laser scans are currently supported");
      throw std::runtime_error(
              "Only topics that use point cloud2s or laser scans are currently supported");
    }

    // get the obstacle range for the sensor
    double obstacle_max_range, obstacle_min_range;
    node->get_parameter(name_ + "." + source + "." + "obstacle_max_range", obstacle_max_range);
    node->get_parameter(name_ + "." + source + "." + "obstacle_min_range", obstacle_min_range);

    // get the raytrace ranges for the sensor
    double raytrace_max_range, raytrace_min_range;
    node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range);
    node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range);


    RCLCPP_DEBUG(
      logger_,
      "Creating an observation buffer for source %s, topic %s, frame %s",
      source.c_str(), topic.c_str(),
      sensor_frame.c_str());

    // create an observation buffer
    observation_buffers_.push_back(
      std::shared_ptr<ObservationBuffer
      >(
        new ObservationBuffer(
          node, topic, observation_keep_time, expected_update_rate,
          min_obstacle_height,
          max_obstacle_height, obstacle_max_range, obstacle_min_range, raytrace_max_range,
          raytrace_min_range, *tf_,
          global_frame_,
          sensor_frame, tf2::durationFromSec(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());
    }

    RCLCPP_DEBUG(
      logger_,
      "Created an observation buffer for source %s, topic %s, global frame: %s, "
      "expected update rate: %.2f, observation persistence: %.2f",
      source.c_str(), topic.c_str(),
      global_frame_.c_str(), expected_update_rate, observation_keep_time);

    rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data;
    custom_qos_profile.depth = 50;

    // create a callback for the topic
    if (data_type == "LaserScan") {
      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
          rclcpp_lifecycle::LifecycleNode>>(node, topic, custom_qos_profile, sub_opt);
      sub->unsubscribe();

      auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
        *sub, *tf_, global_frame_, 50,
        node->get_node_logging_interface(),
        node->get_node_clock_interface(),
        tf2::durationFromSec(transform_tolerance));

      if (inf_is_valid) {
        filter->registerCallback(
          std::bind(
            &ObstacleLayer::laserScanValidInfCallback, this, std::placeholders::_1,
            observation_buffers_.back()));

      } else {
        filter->registerCallback(
          std::bind(
            &ObstacleLayer::laserScanCallback, this, std::placeholders::_1,
            observation_buffers_.back()));
      }

      observation_subscribers_.push_back(sub);

      observation_notifiers_.push_back(filter);
      observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05));

    } else {
      auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
          rclcpp_lifecycle::LifecycleNode>>(node, topic, custom_qos_profile, sub_opt);
      sub->unsubscribe();

      if (inf_is_valid) {
        RCLCPP_WARN(
          logger_,
          "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
      }

      auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>>(
        *sub, *tf_, global_frame_, 50,
        node->get_node_logging_interface(),
        node->get_node_clock_interface(),
        tf2::durationFromSec(transform_tolerance));

      filter->registerCallback(
        std::bind(
          &ObstacleLayer::pointCloud2Callback, this, std::placeholders::_1,
          observation_buffers_.back()));

      observation_subscribers_.push_back(sub);
      observation_notifiers_.push_back(filter);
    }

    if (sensor_frame != "") {
      std::vector<std::string> target_frames;
      target_frames.push_back(global_frame_);
      target_frames.push_back(sensor_frame);
      observation_notifiers_.back()->setTargetFrames(target_frames);
    }
  }
}

雷达数据处理

将处理接收到的激光扫描数据，将其转换为点云，并将其存储在指定的观察缓冲区中，以便代价图层可以处理这些数据并更新代价图。

void ObstacleLayer::laserScanCallback(
  sensor_msgs::msg::LaserScan::ConstSharedPtr message,
  const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer)
{
  // project the laser into a point cloud
  sensor_msgs::msg::PointCloud2 cloud;
  cloud.header = message->header;

  // project the scan into a point cloud
  try {
    projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
  } catch (tf2::TransformException & ex) {
    RCLCPP_WARN(
      logger_,
      "High fidelity enabled, but TF returned a transform exception to frame %s: %s",
      global_frame_.c_str(),
      ex.what());
    projector_.projectLaser(*message, cloud);
  } catch (std::runtime_error & ex) {
    RCLCPP_WARN(
      logger_,
      "transformLaserScanToPointCloud error, it seems the message from laser is malformed."
      " Ignore this message. what(): %s",
      ex.what());
    return;
  }

  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(cloud);
  buffer->unlock();
}

点云2数据处理

直接存储

void
ObstacleLayer::pointCloud2Callback(
  sensor_msgs::msg::PointCloud2::ConstSharedPtr message,
  const std::shared_ptr<ObservationBuffer> & buffer)
{
  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(*message);
  buffer->unlock();
}

边界更新

用于根据机器人的位置和方向更新代价图层的边界。以下是该方法的步骤总结：
- 如果代价图层集合是滚动式的，更新代价图层的原始点以匹配机器人的新位置。
```
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
```
- 检查代价图层是否启用。如果未启用，则直接返回。
- 使用额外的边界来更新边界框。useExtraBounds(min_x, min_y, max_x, max_y);
- 获取标记障碍物的观察和清除障碍物的观察。
  - getMarkingObservations
  - getClearingObservations
- 更新全局当前状态。current_ = current;
- 对清除障碍物的观察进行射线追踪，以清除自由空间。
```
for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}
```
- 对于标记障碍物的观察，将新的障碍物放入优先队列中，每个障碍物优先级为零。
  - 如果障碍物高度太高或离机器人太远，则不将其添加到代价图中。
  - 如果点太近，则不将其考虑在内。
```
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);
```
- 对于标记障碍物的观察中的每个点，计算其地图坐标，并更新代价图层的代价映射。
```
  unsigned int index = getIndex(mx, my);
  costmap_[index] = LETHAL_OBSTACLE;
  touch(px, py, min_x, min_y, max_x, max_y);
```
完整实现

void ObstacleLayer::updateBounds(
  double robot_x, double robot_y, double robot_yaw, double * min_x,
  double * min_y, double * max_x, double * max_y)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  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);
  }

  // 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 sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);

    double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
    double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;

    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");

    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
      double px = *iter_x, py = *iter_y, pz = *iter_z;

      // if the obstacle is too low, we won't add it
      if (pz < min_obstacle_height_) {
        RCLCPP_DEBUG(logger_, "The point is too low");
        continue;
      }

      // if the obstacle is too high or too far away from the robot we won't add it
      if (pz > max_obstacle_height_) {
        RCLCPP_DEBUG(logger_, "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_max_range) {
        RCLCPP_DEBUG(logger_, "The point is too far away");
        continue;
      }

      // if the point is too close, do not conisder it
      if (sq_dist < sq_obstacle_min_range) {
        RCLCPP_DEBUG(logger_, "The point is too close");
        continue;
      }

      // now we need to compute the map coordinates for the observation
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my)) {
        RCLCPP_DEBUG(logger_, "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);
}

更新代价

用于根据代价图层的代价映射更新主代价图的代价。
根据combination_method_,选择不同的更新策略来更新主代价图的代价
- Overwrite代价图层将完全覆盖主代价图中的现有代价，使用新的传感器数据来更新代价图。
- Max代价图层将在主代价图中保留最高代价的值。如果新的传感器数据表示一个更高的代价（例如，障碍物或更高的未知区域代价），则新的代价将覆盖旧的代价。
- MaxWithoutUnknownOverwrite代价图层将在主代价图中保留最高代价的值，但不覆盖未知区域。

void
ObstacleLayer::updateCosts(
  nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
  int max_i,
  int max_j)
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  if (!enabled_) {
    return;
  }

  // if not current due to reset, set current now after clearing
  if (!current_ && was_reset_) {
    was_reset_ = false;
    current_ = true;
  }

  if (footprint_clearing_enabled_) {
    setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
  }

  switch (combination_method_) {
    case CombinationMethod::Overwrite:
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
      break;

总结

本文介绍了Costmap2D的几个核心类，包括Costmap2D、CostmapLayer和Layer。包括底下派生的子类StaticLayer和ObstacleLayer
CostmapLayer的子类通常需要实现下述函数

  virtual void matchSize();
  virtual void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert);
  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(
    nav2_costmap_2d::Costmap2D & master_grid,
    int min_i, int min_j, int max_i, int max_j);

几种更新代价的方法

  void updateWithTrueOverwrite(nav2_costmap_2d::Costmap2D & master_grid,int min_i, int min_j, int max_i, int max_j);
  void updateWithOverwrite(
    nav2_costmap_2d::Costmap2D & master_grid,
    int min_i, int min_j, int max_i, int max_j);
  void updateWithMax(
    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
    int max_j);
 void updateWithMaxWithoutUnknownOverwrite(
    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
    int max_j);
  void updateWithAddition(
    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
    int max_j);

预告

下节将介绍Costmap2D剩下的几个类inflation_layer，costmap_2d_ros,costmap_2d_publisher

开放原子开发者工作坊

开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！

更多推荐

OpenLoong项目通过技术监督委员会（TOC）评审

开放原子开发者工作坊

开发者谈开源：KWDB开源数据库的未来路径与生态构建实践

开放原子开发者工作坊

开发者谈开源：洞悉协作创新背后的机遇与挑战

近日，在2024开放原子开发者大会暨首届开源技术学术大会开幕式上，开放原子开源基金会与openKylin、EasyAda、KWDB开源项目举行捐赠签约仪式。一场捐赠签约仪式，让三个开源项目及其背后的开发者们受到瞩目。本次，我们与“龘”（EasyAda）核心维护者王伶卓开启了对话。

开放原子开发者工作坊

所有评论(0)

查看更多评论

m0_73800387

@m0_73800387

已为社区贡献15条内容

Nav2代价地图实现和原理--Nav2源码解读之CostMap2D(上)

m0_73800387

前言

CostMap2D简介

整体框架

Costmap2D—代价地图数据结构

类的声明和构造函数

核心功能成员函数

保护成员函数和私有成员

其他重要成员

Costmap2D具体实现

地图创建

代价值获取与设置

Layer—代价地图层插件的抽象基类

核心方法

辅助方法

数据成员

Layer实现

初始化

LayeredCostmap—多层代价地图插件合体机

类的声明和构造函数

核心方法

数据成员

LayeredCostmap实现

初始化

载入插件和过滤器

更新代价地图（核心）

设置机器人足迹

Costmap2DLayer—多层代价地图插件的抽象基类

核心方法

数据成员

CostmapLayer实现

更新主代价地图

更新代价地图的边界框

匹配主代价地图的大小

清空指定区域。

添加和使用额外的代价地图边界

StaticLayer—静态地图层

核心方法

数据成员

StaticLayer实现

参数获取

动态参数回调

初始化

代价地图订阅回调

代价地图处理

静态地图值转换函数

更新代价地图订阅回调

更新边界

更新机器人足迹

更新代价地图代价

ObstacleLayer—障碍地图层

核心方法

数据成员

ObstacleLayer实现

初始化

雷达数据处理

点云2数据处理

边界更新

更新代价

总结

预告

所有评论(0)

m0_73800387

`Costmap2D`—代价地图数据结构

`Costmap2D`具体实现

`Layer`—代价地图层插件的抽象基类

`Layer`实现

`LayeredCostmap`—多层代价地图插件合体机

`LayeredCostmap`实现

`Costmap2DLayer`—多层代价地图插件的抽象基类

`StaticLayer`—静态地图层

`StaticLayer`实现

`ObstacleLayer`—障碍地图层

`ObstacleLayer`实现