ROS_PCl_加载PCD点云数据文件与接收点云并保存

第一个程序是加载PCD数据文件,第二个程序是接收点云数据并写入pcd文档.

1.pcl_load.cpp

#include
#include
#include
#include
#include//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 ("pcl_output", 1);
  pcl::PointCloud cloud;
  sensor_msgs::PointCloud2 output;
  pcl::io::loadPCDFile ("/home/shuning/catkin_ws/src/imgpcl/data/test_pcd.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;
}

pcl_write.cpp:

#include
#include
#include
#include
#include

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
  pcl::PointCloud cloud;
  pcl::fromROSMsg(input, cloud);//从ROS类型消息转为PCL类型消息
  pcl::io::savePCDFileASCII ("/home/shuning/catkin_ws/src/imgpcl/data/write_pcd_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.test_pcd.pcd"

部分数据如下

FIELDS x y z intensity distance sid
SIZE 4 4 4 4 4 4
TYPE F F F F F F
COUNT 1 1 1 1 1 1
WIDTH 460400
HEIGHT 1
POINTS 460400
DATA ascii
-0.93387 -0.6825 -1.1865 12 1.485 7
-0.93173 -0.68323 -1.1878 10 1.485 8
-0.92185 -0.68054 -1.1831 10 1.475 10
-0.91748 -0.67961 -1.1815 10 1.471 11
-0.91479 -0.6799 -1.182 12 1.47 12
-0.90031 -0.68289 -1.1872 10 1.467 18
-0.89271 -0.67941 -1.1811 12 1.457 19
-0.87805 -0.67048 -1.1656 8 1.434 20

3.编译

cd catkin_ws

source devel/setup.bash

catkin_make

4.运行

roscore

rosrun imgpcl pcl_load

rosrun imgpcl pcl_write

rosrun rviz rviz

5.Rviz的配置

点击Add,添加pointcloud2,

Fixed  Frame设置为odom.

Topic 设置为/pcl_output

参考Ros机器人程序设计第二版