grid_map（四）：simple demo学习

原创已于 2022-04-09 23:05:42 修改 · 2.3k 阅读

18 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #ubuntu

于 2022-02-07 21:44:03 首次发布

无人驾驶车辆学习专栏收录该内容

45 篇文章

订阅专栏

本文详细介绍了ROS环境下grid_map库的simple_demo及其可视化过程。通过grid_map_demos包的simple_demo_node创建并更新栅格地图，然后使用grid_map_visualization节点进行地图可视化。内容涵盖launch文件、节点配置、数据处理及rviz效果展示。

0、前言

接着下面链接的2.1 simple demo。grid_map（二）：ubuntu16.04安装测试grid_map 1.6.0-1.6.2https://blog.youkuaiyun.com/BIT_HXZ/article/details/122783924?spm=1001.2014.3001.5502

simple demo简单演示由两部分组成，grid_map_demos 包里面的simple demo节点和 grid_map_visualization 包里面的grid_map_visualization节点。

simple_demo.launch如下：

<launch>
  <!-- Launch the grid map simple demo node -->
  <node pkg="grid_map_demos" type="simple_demo" name="grid_map_simple_demo" output="screen" />
  <!-- Launch the grid map visualizer -->
  <node pkg="grid_map_visualization" type="grid_map_visualization" name="grid_map_visualization" output="screen">
    <rosparam command="load" file="$(find grid_map_demos)/config/simple_demo.yaml" />
  </node>
  <!-- Launch RViz with the demo configuration -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find grid_map_demos)/rviz/grid_map_demo.rviz" />
</launch>

1、simple_demo_node节点

文件地址在：

home/meng/grid_map_ws/src/grid_map/grid_map_demos/src/simple_demo_node.cpp

注释后文件为

#include <ros/ros.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_msgs/GridMap.h>
#include <cmath>

using namespace grid_map;

int main(int argc, char** argv)
{
  // Initialize node and publisher.
  ros::init(argc, argv, "grid_map_simple_demo");
  ros::NodeHandle nh("~");
  //参考grid_map_ws/src/grid_map/grid_map_msgs/msg/ 文件夹下的
  //两个自定义消息类型理解<grid_map_msgs::GridMap>,下面地图定义等部分同样需要参考
  //发布话题：/grid_map_simple_demo/grid_map
  ros::Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);

  // Create grid map.
  GridMap map({"elevation"});//创建栅格地图，图层layer名为elevation
  map.setFrameId("map");
  map.setGeometry(Length(1.2, 2.0), 0.03);//地图在x方向上1.2m，在y方向上2m，分辨率0.03m
  //map.getLength().x():地图在x方向上长度,    map.getLength().y():y方向上长度
  //map.getSize()(0): x方向格子数    map.getSize()(1))：y方向格子数
  ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
    map.getLength().x(), map.getLength().y(),
    map.getSize()(0), map.getSize()(1));

  // Work with grid map in a loop.
  ros::Rate rate(30.0);
  while (nh.ok()) {

    // Add data to grid map.
    ros::Time time = ros::Time::now();
    //在for循环中使用迭代器，使用GridMapIterator迭代整个栅格地图
    for (GridMapIterator it(map); !it.isPastEnd(); ++it) {
      Position position;
      map.getPosition(*it, position);//将迭代到的位置赋值给position
      //toSec()把时间戳转化为浮点型格式，其定义如下：
      //double toSec()  const { return (double)sec + 1e-9*(double)nsec; };
      //下面式子的公式为：栅格地图某位置的值=-0.04+0.2*sin(3t+5y)*x,即为变化的量
      map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x();
    }

    // Publish grid map.
    //toNSec()转换为整型的纳秒，定义:
    //uint64_t toNSec() const {return (uint64_t)sec*1000000000ull + (uint64_t)nsec;  }
    map.setTimestamp(time.toNSec());//设置时间戳
    grid_map_msgs::GridMap message;
    GridMapRosConverter::toMessage(map, message);//转化为ros消息
    publisher.publish(message);
    //ROS_INFO_THROTTLE输出频率日志，第一个参数interval代表以秒为单位的时间量，时间量指的是两个日志出现的最小时间间隔
    ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec());

    // Wait for next cycle.
    rate.sleep();
  }

  return 0;
}

2、grid_map_visualization节点配置文件

先看launch文件里面加载的配置文件，地址为：

/grid_map_ws/src/grid_map/grid_map_demos/config/simple_demo.yaml

#需要被可视化的grid map话题名称，是订阅的话题
grid_map_topic: /grid_map_simple_demo/grid_map 

#需要发布的话题，名称为：/grid_map_visualization/name,如/grid_map_visualization/elevation_points
grid_map_visualizations:   #话题名前缀
  - name: elevation_points #话题名后缀
    type: point_cloud      #话题类型
    params:
     layer: elevation      #rviz显示的所在图层

  #将栅格地图的一层可视化为占用栅格。
  #使用layer参数指定要可视化的图层，并使用data_min和data_max指定上下限。
  - name: elevation_grid
    type: occupancy_grid
    params:
     layer: elevation
    #  data_min: 0.08        #默认
    #  data_max: -0.16
    #  data_min: 1           #试验1
    #  data_max: -1
     data_min: -0.16         #试验2
     data_max: 0.16

相应得到rviz效果图如下：

配置文件中订阅的话题为：（/grid_map_simple_demo/grid_map显示时大概是以颜色来表示不同的“高度”）

配置文件中发布的话题为：

3、grid_map_visualization节点启动

grid_map_visualization节点涉及的文件较多，参考grid_map_visualization包的CMakeLists.txt的生成可执行文件部分：

## Declare a cpp executable
add_executable(
  ${PROJECT_NAME}
  src/grid_map_visualization_node.cpp
  src/GridMapVisualization.cpp
  src/GridMapVisualizationHelpers.cpp
  src/visualizations/VisualizationBase.cpp
  src/visualizations/VisualizationFactory.cpp
  src/visualizations/PointCloudVisualization.cpp
  src/visualizations/FlatPointCloudVisualization.cpp
  src/visualizations/VectorVisualization.cpp
  src/visualizations/OccupancyGridVisualization.cpp
  src/visualizations/GridCellsVisualization.cpp
  src/visualizations/MapRegionVisualization.cpp
)

这时需要观察roslaunch 时终端打印输出内容，打开vscode搜索.

[ INFO] [1644322605.259528797]: Grid map visualization node started. ##这句可以搜索到
[ INFO] [1644322605.259742335]: Grid map (timestamp 1644322605.259412) published.
[ INFO] [1644322605.260908349]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'elevation_points'.
[ INFO] [1644322605.260928369]: grid_map_visualizations: Configured visualization of type 'occupancy_grid' with name 'elevation_grid'.
[ INFO] [1644322605.261776145]: Grid map visualization initialized.  ##这句可以搜索到

在vscode中可以直接搜索到“Grid map visualization node started”和“Grid map visualization initialized”这两句，分别在“--下面地址--”的25行和131行，其中“grid_map_visualizations: Configured visualization of type 'xx' with name 'xx'.”在118行。接下来就可以以GridMapVisualization.cpp文件为主要线索理解grid_map_visualization节点了

grid_map_ws/src/grid_map/grid_map_visualization/src/GridMapVisualization.cpp

借由上面的“线索”，grid_map_visualization节点文件的执行顺序为：

1: grid_map_ws/src/grid_map/grid_map_visualization/src/grid_map_visualization_node.cpp
2: grid_map_ws/src/grid_map/grid_map_visualization/src/GridMapVisualization.cpp

--未完--