ROS从入门到精通2-8：Gazebo仿真之快速生成二维地图真值

0 专栏介绍

本专栏旨在通过对ROS的系统学习，掌握ROS底层基本分布式原理，并具有机器人建模和应用ROS进行实际项目的开发和调试的工程能力。

详情：《ROS从入门到精通》

---

1 为什么需要地图真值？

栅格地图真值不会受到传感器误差、环境变化等因素的影响，导致地图的不确定性和误差。地图真值可以用作SLAM算法的评估指标，帮助研究人员和开发者了解SLAM算法在不同环境和条件下的性能表现。

通过将SLAM生成的地图与栅格地图真值进行对比，可以识别出SLAM算法的优势、局限性和改进方向，以及进行算法调试和参数优化。在一些对地图精度要求较高的应用中，使用栅格地图真值进行评估和对比可以提供更可靠的结果。同时，栅格地图真值可以用于生成合成数据集，从而扩大数据集的规模和多样性。通过将真实数据与栅格地图真值结合，可以创建具有不同特征、噪声和变化的合成数据集，用于训练和验证SLAM算法的鲁棒性和泛化能力。

如下所示是通过SLAM建立的地图(左)和对应的地图真值(右)

接下来介绍如何基于Gazebo世界场景，快速生成二维地图真值

2 Gazebo插件实现

2.1 单线扫描碰撞信息

原理是基于Gazebo的RayShape引擎，扫描一定高度下世界环境中的障碍物，并把障碍物信息映射到二维地图上。在仿真环境中，这种扫描绝对精准，因此建立的地图是真值地图

核心代码如下所示

gazebo::physics::PhysicsEnginePtr engine = world->Physics();
      engine->InitForThread();
gazebo::physics::RayShapePtr ray =
    boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
        engine->CreateShape("ray", gazebo::physics::CollisionPtr()));

boost::gil::fill_pixels(image._view, blank);
for (int i = 0; i < count_vertical; ++i)
{
  x = i * dX_vertical + msg->lowerleft().x();
  y = i * dY_vertical + msg->lowerleft().y();
  for (int j = 0; j < count_horizontal; ++j)
  {
    x += dX_horizontal;
    y += dY_horizontal;

    start.X(x);
    end.X(x);
    start.Y(y);
    end.Y(y);
    ray->SetPoints(start, end);
    ray->GetIntersection(dist, entityName);
    if (!entityName.empty())
      image._view(i, j) = fill;
  }
}

2.2 写入.pgm地图文件

将扫描的障碍信息保存到.pgm地图文件中

void pgm_write_view(CollisionMapRequestPtr &msg, boost::gil::gray8_view_t &view)
{
  // Write image to pgm file
  std::cout << "running" << std::endl;
  int h = view.height();
  int w = view.width();

  std::ofstream ofs;
  ofs.open(msg->filename() + ".pgm");
  ofs << "P2" << '\n';          // grayscale
  ofs << w << ' ' << h << '\n'; // width and height
  ofs << 255 << '\n';           // max value
  for (int y = 0; y < h; ++y)
  {
    for (int x = 0; x < w; ++x)
      ofs << (int)view(x, y)[0] << ' ';
    ofs << '\n';
  }
  ofs.close();
}

2.3 写入.yaml元文件

保存用于导航的地图元文件

void yaml_write(CollisionMapRequestPtr &msg)
{
  std::string map_meta_file = msg->filename() + ".yaml";
  std::cout << "Writing map occupancy data to " << map_meta_file << std::endl;

  FILE *yaml = fopen(map_meta_file.c_str(), "w");

  // resolution: 0.100000
  // origin: [0.000000, 0.000000, 0.000000]
  // #
  // negate: 0
  // occupied_thresh: 0.65
  // free_thresh: 0.196

  fprintf(yaml,
          "image: %s\nresolution: %f\norigin: [%f, %f, 0.00]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
          (msg->filename() + ".pgm").c_str(), msg->resolution(),
          msg->lowerleft().x(), msg->lowerleft().y());

  fclose(yaml);
}

3 快速建图测试

使用时在需要建图的.world文件中加入世界插件

<plugin filename="libcollision_map_creator.so" name="collision_map_creator"/>

接着启动一个终端打开三维世界

<launch>
  <arg name="world_file" default="$(find pgm_map_creator)/worlds/workshop.world"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="false"/>
    <arg name="gui" value="true" />
    <arg name="world_name" value="$(arg world_file)"/>
  include>
launch>

最后在新终端启动扫描建图功能包，发送建图请求

<launch>
  <arg name="map_name" default="map" />
  <arg name="save_folder" default="$(find pgm_map_creator)/maps" />
  <arg name="xmin" default="-15" />
  <arg name="xmax" default="15" />
  <arg name="ymin" default="-15" />
  <arg name="ymax" default="15" />
  <arg name="scan_height" default="5" />
  <arg name="resolution" default="0.01" />

  <node pkg="pgm_map_creator" type="request_publisher" name="request_publisher" output="screen"
    args="'($(arg xmin),$(arg ymax))($(arg xmax),$(arg ymax))($(arg xmax),$(arg ymin))($(arg xmin),$(arg ymin))'
          $(arg scan_height) $(arg resolution) $(arg save_folder)/$(arg map_name)">
  node>
launch>

下面是两个建立地图真值的实例

4 机器人导航测试

建立的地图可以很好地应用于机器人导航，在部分运动规划实验中可以实现与建图过程的解耦

本文完整工程代码请通过下方名片联系博主获取

---

更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系