Ubuntu18.04 + ROS 下学习点云库PCL的使用(二)

序言

本文是延续Ubuntu18.04 + ROS 下学习点云库PCL的使用(一）的续篇，在上一篇中，我们配置好ROS下PCL的编程环境并成功创建100个无序点云，最后在可视化工具RVIZ成功查看。

今天我们继续PCL的基础学习，通过订阅和发布ros话题，对上面创建的点云进行读写处理。

 一、程序文件

1、 “pcd_write.cpp”文件

在之前创建的“pcl_01”功能包的src目录下，新建一个“pcd_write.cpp”文件，将下列代码复制进去。

#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);
    pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud);
}
 
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、“pcd_read.cpp”文件

再新建“pcd_read.cpp”文件，将代码复制进去。

#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>
 
main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_read");
 
    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
 
    sensor_msgs::PointCloud2 output;
    pcl::PointCloud<pcl::PointXYZ> cloud;
 
    pcl::io::loadPCDFile ("write_pcd_test.pcd", cloud);
 
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";
 
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
 
    return 0;
}

3、修改CmakeLists.txt文件

add_executable(pcd_read src/pcd_read.cpp)
add_executable(pcd_write src/pcd_write.cpp)

add_dependencies(pcd_read ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(pcd_write ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})


target_link_libraries(pcd_read
  ${catkin_LIBRARIES}
)
target_link_libraries(pcd_write
  ${catkin_LIBRARIES}
)

二、运行程序

首先，开启一个终端启动roscore

roscore

 然后启动上次的发布点云节点

rosrun pcl_01 pcl_create

启动保存点云为pcd文件的节点

rosrun pcl_01 pcd_create

这时候会发现，工作空间下多了一个“write_pcd_test.pcd”文件，就是将创建的点云进行保存。

我们可以用PCL tools中的pcl_viewer来查看文件信息，指令为：

pcl_viewer write_pcd_test.pcd

结果如下： 

这样即为成功。

 这时候，我们关闭pcl_create节点，用pcd_read节点读取pcd文件

rosrun pcl_01 pcd_read

功能是把pcd文件的点云信息读取并发布出来，这时我们可以启动rviz来查看，该步骤省略。

总结

今天我们进行的是在ROS下保存和读取点云消息，我们在入门之后，可以利用官方教程进阶，这样学习速度会大大提升。具体每行代码的分析，大家在跑通之后再逐句分析效率会更高。