在Elevation Mapping中提取点云（x，y，z）数据

在Elevation Mapping中提取点云（x，y，z）数据

elevation mapping github：
https://github.com/ANYbotics/elevation_mapping
在使用过程图，希望对高程图中的点云数据进行进一步处理，或读取某个点的数值。

elevation mapping发布的topic

首先需要了解elevation mapping发布的topic

具体思路

由于这两个topic的数据类型没有区别，因此提取具体的点云数据的具体方式没有差别。只需要将GridMap格式转为pointcloud2格式即可，下面时具体步骤：
数据类型转换：

// 接收gridmap之后转换为pointcloud2
GridMapRosConverter::fromMessage(message, gridMap);
GridMapRosConverter::toPointCloud(gridMap, pointLayer, pointCloud);

pointcloud2类型提取xyz不是太方便，转换pointcloud2为pointcloud类型：

// pointcloud2转pointcloud，保存pcd那一步不存也可以
sensor_msgs::convertPointCloud2ToPointCloud(pointCloud, out_pointcloud);
pcl::fromROSMsg(pointCloud, cloud);
pcl::io::savePCDFileASCII ("/home/robot/dog_ws/gird_map_pcd_test.pcd", cloud);//保存pcd

接下来可以对每个点的xyz参数进行分析：

point.z = out_pointcloud.points[i].z;
point.x = out_pointcloud.points[i].x;
point.y = out_pointcloud.points[i].y;

代码

一段较为完整的代码如下：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "grid_map_core/GridMap.hpp"
#include "grid_map_demos/FiltersDemo.hpp"

#include "grid_map_ros/GridMapRosConverter.hpp"
#include "grid_map_ros/GridMapMsgHelpers.hpp"
#include <grid_map_cv/grid_map_cv.hpp>

// ROS
#include <sensor_msgs/point_cloud2_iterator.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

// STL
#include <limits>
#include <algorithm>
#include <vector>

//pointcloud
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

//pcl point

#include<pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>
#include<pcl/io/pcd_io.h>
using namespace grid_map;
void chatterCallback(const grid_map_msgs::GridMap& message)
{
     //ROS_INFO("%f",message->data.std_msgs.Float32MultiArray.float32());

     // Convert message to map.
     GridMap gridMap;
     std::string pointLayer = ("elevation", "upper_bound", "lower_bound") ;
     sensor_msgs::PointCloud2 pointCloud;
     sensor_msgs::PointCloud out_pointcloud;
     pcl::PointCloud<pcl::PointXYZ> cloud;
     


     GridMapRosConverter::fromMessage(message, gridMap);
     GridMapRosConverter::toPointCloud(gridMap,
                                       pointLayer,
                                       pointCloud);
     sensor_msgs::convertPointCloud2ToPointCloud(pointCloud, out_pointcloud);



     pcl::fromROSMsg(pointCloud, cloud);
     pcl::io::savePCDFileASCII ("/home/robot/dog_ws/gird_map_pcd_test.pcd", cloud);//保存pcd



     //ROS_INFO_STREAM(out_pointcloud);
     for(int i = 0 ; i < out_pointcloud.points.size(); ++i){    
         geometry_msgs::Point32 point;

         //Do something here
         point.z = out_pointcloud.points[i].z;
         point.x = out_pointcloud.points[i].x;
         point.y = out_pointcloud.points[i].y;
         //可以在下面进行你需要的操作
         // ...
         // ...            
 }
 
int main(int argc, char **argv)
{
     ros::init(argc,argv,"map");
     ros::NodeHandle n;
     ros::Subscriber sub = n.subscribe("/elevation_mapping/elevation_map",1000,chatterCallback); //订阅的topic
     ros::spin();
 
     return 0;
 }

补充内容

备注：使用时除了在省略号中补全你需要进行的操作外，还需要注意配置cmakelist那个文件，之后编译
以下是需要加的部分：

1.
add_executable(
  map
  src/Map.cpp
)
2.
target_link_libraries(
  map
  ${catkin_LIBRARIES}
)