在ROS中读入两个PCD文件，拼接，并在RVIZ中显示

在上一篇中得到了几张PCD文件．现在把他们拼接起来．

拼接方法：两个点云直接相加．

注意点：

　　１

 pcl::PointCloud::Ptr clouda_ptr (new pcl::PointCloud);//创建点云指针，存储点坐标xyz

　　这里创建的智能指针用来存储操作点云．后面访问对象成员用－＞．当然也可以直接定义一个类，用．来访问成员．

　　２

if (pcl::io::loadPCDFile ("/home/ethan/bags/pcl_2_bag/pcl_2_pcd1.pcd", *clouda_ptr) == -1)

　　要么把PCD文件放在工程目录下只写文件名（我在ROS下试了没搞成功），要么直接把路径也带上．

　　３

pcl::toROSMsg (*clouda_ptr, clouda_ros);

　　将标准PCL类型转换为ROS中的类型．

　　４

cloudc_ros.header.stamp = ros::Time::now();
cloudc_ros.header.frame_id = "sensor_framec";

　　转换后的消息只是格式有了，里面的成员还要我们自己填写，把时间戳和所在坐标系名字填上．在后面在RVIZ中通过消息名添加点云并观察点云的时候，要把frame_id填到fixed_frame里，才行．

　　５

　　把clouda和cloudb的frame_id都设为相加结果c的，使得他们都在同一个坐标系中，方便观察对比．

#include 
#include 
#include 
#include 
#include 
#include 

ros::Publisher pubc;
ros::Publisher pubb;
ros::Publisher puba;
int
main(int argc, char** argv) {
    // *****Initialize ROS
    ros::init (argc, argv, "contatenate");
    ros::NodeHandle nh;

    //*****load two pcd files
    pcl::PointCloud::Ptr clouda_ptr (new pcl::PointCloud);//创建点云指针，存储点坐标xyz
    pcl::PointCloud::Ptr cloudb_ptr (new pcl::PointCloud);
    pcl::PointCloud::Ptr cloudc_ptr (new pcl::PointCloud);
    if (pcl::io::loadPCDFile ("/home/ethan/bags/pcl_2_bag/pcl_2_pcd1.pcd", *clouda_ptr) == -1)
    {
        PCL_ERROR ("Couldn't read file pcl_2_pcd1.pcd ^.^\n");
        return (-1);
    }
    std::cerr << "size of a : " << clouda_ptr->width*clouda_ptr->height<< std::endl;
    if (pcl::io::loadPCDFile ("/home/ethan/bags/pcl_2_bag/pcl_2_pcd2.pcd", *cloudb_ptr) == -1)
    {
        PCL_ERROR ("Couldn't read file pcl_2_pcd2.pcd ^.^\n");
        return (-1);
    }
    std::cerr << "size of b : " << cloudb_ptr->width*cloudb_ptr->height<< std::endl;

    //*****contatenate
    *cloudc_ptr=*clouda_ptr+*cloudb_ptr;// cloud_a + cloud_b 意思是cloud_c包含了a和b中的点，c的点数 = a的点数+b的点数
    std::cerr << "size of c : " << cloudc_ptr->width*cloudc_ptr->height<< std::endl;

    //*****Convert the pcl/PointCloud data to sensor_msgs/PointCloud2
    sensor_msgs::PointCloud2 clouda_ros;
    sensor_msgs::PointCloud2 cloudb_ros;
    sensor_msgs::PointCloud2 cloudc_ros;
    pcl::toROSMsg (*clouda_ptr, clouda_ros);
    pcl::toROSMsg (*cloudb_ptr, cloudb_ros);
    pcl::toROSMsg (*cloudc_ptr, cloudc_ros);
    
    //*****publish these three clouds
    pubc = nh.advertise ("cloudc", 1);
    pubb = nh.advertise ("cloudb", 1);
    puba = nh.advertise ("clouda", 1);

    ros::Rate r(3);
    while(ros::ok())
    {
    cloudc_ros.header.stamp = ros::Time::now();
    cloudc_ros.header.frame_id = "sensor_framec";
    cloudb_ros.header.stamp = ros::Time::now();
    cloudb_ros.header.frame_id = "sensor_framec";
    clouda_ros.header.stamp = ros::Time::now();
    clouda_ros.header.frame_id = "sensor_framec";
    pubc.publish(cloudc_ros);
    pubb.publish(cloudb_ros);
    puba.publish(clouda_ros);
    r.sleep();
    }
    return 0;
}

结果：

clouda:

cloudb:

cloudc:拼接后的结果