使用cartographer_ros发布的地图直接用于导航实现动态地图路线规划

使用cartographer_ros发布的地图直接用于导航实现动态地图路线规划

当你看到网上一些cartographer_ros的教程时，都会教你将cartographer生成的地图转成pgm后再使用map_server发布地图用于导航…等等，为什么不直接使用cartographer_ros发布的地图直接用于导航，还要转一手呢？

用cartographer_ros发布的实时地图直接用于导航，我可以想到以下几个使用场景：

• 实现在建图的时候进行导航。
• 实现动态地图导航，根据环境地图更新及时规划新的导航路线。
• 基于RRT（快速探索随机树）实现机器人自主探索建图。

那就赶紧试看看。

操作环境基于：

操作系统：Ubuntu 20.04

ROS版本：ROS2 Galactic

先用cartographer建个图（操作过程省略…）。

然后启用cartographer纯定位模式进行导航测试。

运行cartographer_node加载刚才建好的地图，运行cartographer_occupancy_grid_node用于发布地图话题。接着运行导航订阅地图话题为静态层并添加膨胀层。然后打开RVIZ添加地图话题和全局代价地图话题我们看到下面这张图。

地图黑色的障碍只有很少的像素点被转成了代价地图障碍。而且发送目标点规划的全局路线也直接从障碍上穿过去了。这是什么情况？

为了对比，我换成用map_server来发布刚刚建好的地图。

运行cartographer_node加载刚才建好的地图运行纯定位模式，运行map_server加载地图发布地图话题，运行导航。

再次用RVIZ看看效果。

可以看到代价地图正常转换了障碍，路线规划也正常了。

为什么cartographer_ros发布的地图话题不能正常使用？

对比一下cartographer_ros直接发布的地图（上图）和map_server发布的地图（下图）有什么不一样。

很明显发现，map_server只有3种颜色，黑色为障碍，浅色为可通行区域，其它为未知区域。而cartographer_ros直接发布的地图灰度是渐变，黑色的地方也不够黑。

说明cartographer_ros直接发布的地图数据不是导航标准的，不能被正常识别。

在navigation2的源代码中可以找到导航对占据栅格值的定义。

navigation2/nav2_util/include/occ_grid_values.hpp

namespace nav2_util
{

/**
  * @brief OccupancyGrid data constants
  */
static constexpr int8_t OCC_GRID_UNKNOWN = -1;
static constexpr int8_t OCC_GRID_FREE = 0;
static constexpr int8_t OCC_GRID_OCCUPIED = 100;

}  // namespace nav2_util

occupied为100，表示有物体。free为0，表示无物体可通行，unknow为-1，表示未知区域。

看看map_server源代码中对cartographer_ros生成的地图文件的处理。

navigation2/nav2_map_server/src/map_io.cpp

if (load_parameters.occupied_thresh < occ) {
    map_cell = nav2_util::OCC_GRID_OCCUPIED;
} else if (occ < load_parameters.free_thresh) {
    map_cell = nav2_util::OCC_GRID_FREE;
} else {
    map_cell = nav2_util::OCC_GRID_UNKNOWN;
}

跟yaml参数文件里occupied_thresh，free_thresh两个值，大于occupied_thresh转成OCC_GRID_OCCUPIED，小于free_thresh转成OCC_GRID_FREE，free_thresh和occupied_thresh之间的值转成OCC_GRID_UNKNOWN。

由此可以总结出cartographer占据栅格值与导航的占据栅格值的区别：

	cartographer	navigation
occupied	>occupied_thresh	100
free	<free_thresh	0
unknow	free_thresh~occupied_thresh	-1

知道问题原因后，就可以直接修改cartographer_ros发布地图话题的源代码，使它直接发布导航标准的占据栅格值。

找到cartographer_ros生成占据栅格消息的代码。

cartographer_ros/cartographer_ros/src/msg_conversion.cpp

找到CreateOccupancyGridMsg函数，改为如下代码：

std::unique_ptr<nav_msgs::msg::OccupancyGrid> CreateOccupancyGridMsg(
    const cartographer::io::PaintSubmapSlicesResult& painted_slices,
    const double resolution, const std::string& frame_id,
    const rclcpp::Time& time) {
  auto occupancy_grid = absl::make_unique<nav_msgs::msg::OccupancyGrid>();

  const int width = cairo_image_surface_get_width(painted_slices.surface.get());
  const int height =
      cairo_image_surface_get_height(painted_slices.surface.get());

  const int8_t occ_grid_unknown = -1;
  const int8_t occ_grid_free = 0;
  const int8_t occ_grid_occupied = 100;
  const double occupied_thresh = 0.65;
  const double free_thresh = 0.196;

  occupancy_grid->header.stamp = time;
  occupancy_grid->header.frame_id = frame_id;
  occupancy_grid->info.map_load_time = time;
  occupancy_grid->info.resolution = resolution;
  occupancy_grid->info.width = width;
  occupancy_grid->info.height = height;
  occupancy_grid->info.origin.position.x =
      -painted_slices.origin.x() * resolution;
  occupancy_grid->info.origin.position.y =
      (-height + painted_slices.origin.y()) * resolution;
  occupancy_grid->info.origin.position.z = 0.;
  occupancy_grid->info.origin.orientation.w = 1.;
  occupancy_grid->info.origin.orientation.x = 0.;
  occupancy_grid->info.origin.orientation.y = 0.;
  occupancy_grid->info.origin.orientation.z = 0.;

  const uint32_t* pixel_data = reinterpret_cast<uint32_t*>(
      cairo_image_surface_get_data(painted_slices.surface.get()));
  occupancy_grid->data.reserve(width * height);
  for (int y = height - 1; y >= 0; --y) {
    for (int x = 0; x < width; ++x) {
      const uint32_t packed = pixel_data[y * width + x];
      const unsigned char color = packed >> 16;
      const unsigned char observed = packed >> 8;
      int value =
          observed == 0
              ? -1
              : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);

      if (occupied_thresh * 100. < value) {
        value = occ_grid_occupied;
      } else if (free_thresh * 100. < value) {
        value = occ_grid_unknown;
      } else if (-1 < value) {
        value = occ_grid_free;
      }

      CHECK_LE(-1, value);
      CHECK_GE(100, value);
      occupancy_grid->data.push_back(value);
    }
  }

  return occupancy_grid;
}

很简单，就是把value的值重新改一下，加上这几句代码：

      if (occupied_thresh * 100. < value) {
        value = occ_grid_occupied;
      } else if (free_thresh * 100. < value) {
        value = occ_grid_unknown;
      } else if (-1 < value) {
        value = occ_grid_free;
      }

编译后运行看一下效果：

可以看到地图已经被改成只有3种颜色，障碍物体能正常识别，并生成膨胀层。

occupied_thresh和free_thresh两个常量可以继续调整，以达到更好的效果。

最后看一下代价地图静态层地图实时更新

Good!