PCL、Rviz地图保存与加载

0.引言

• ref：PCL数据与ROS数据的转换。

1.write map

#include<ros/ros.h>
#include<pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>
#include<sensor_msgs/PointCloud2.h>
#include<pcl/io/pcd_io.h>

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(input, cloud);//从ROS类型消息转为PCL类型消息
  pcl::io::savePCDFileASCII ("/home/fb/map/test.pcd", cloud);//保存pcd
}
main (int argc, char **argv)
{
  ros::init (argc, argv, "pcl_write");
  ros::NodeHandle nh;
  ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB);//接收点云
  ros::spin();
  return 0;
}

2.load map

#include<ros/ros.h>
#include<pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>
#include<sensor_msgs/PointCloud2.h>
#include<pcl/io/pcd_io.h>//which contains the required definitions to load and store point clouds to PCD and other file formats.

main (int argc, char **argv)
{
  ros::init (argc, argv, "UandBdetect");
  ros::NodeHandle nh;
  ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
  pcl::PointCloud<pcl::PointXYZ> cloud;
  sensor_msgs::PointCloud2 output;
  pcl::io::loadPCDFile ("/home/fb/map/test.pcd", cloud);
  //Convert the cloud to ROS message
  pcl::toROSMsg(cloud, output);
  output.header.frame_id = "odom";//this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer
  ros::Rate loop_rate(1);
  while (ros::ok())
  {
    pcl_pub.publish(output);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

3.application

在我的实际应用中，要比较几种算法的性能，必须固定使用同一个地图：

//pcl::fromROSMsg(pointcloud_map, cloud);//1.2.注释掉改为直接加载地图
//pcl::io::savePCDFileASCII ("/home/fb/map/const_demo.pcd", cloud);//1.1.保存地图为pcd文件,然后注释掉
pcl::io::loadPCDFile ("/home/fb/map/const_demo.pcd", cloud);//1.3.加载地图

查看pcd文件：pcl_viewer const_demo.pcd

加载到Rviz：（不是直接加载，进行过空间裁剪，xyz空间大小限定在一个长方体内）