grid_map（四）：simple demo学习

0、前言

        接着下面链接的2.1 simple demo。grid_map（二）：ubuntu16.04安装测试grid_map 1.6.0-1.6.2https://blog.csdn.net/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如下：

1、simple_demo_node节点

        文件地址在：

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

        注释后文件为

#include 
#include 
#include 
#include 

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_simple_demo/grid_map
  ros::Publisher publisher = nh.advertise("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

--未完--