在ROS中读入两个PCD文件，拼接，并在RVIZ中显示
发布时间
阅读量：
阅读量
在上一篇中得到了几张PCD文件．现在把他们拼接起来．
拼接方法：两个点云直接相加．
注意点：
１
pcl::PointCloud<pcl::PointXYZ>::Ptr clouda_ptr (new pcl::PointCloud<pcl::PointXYZ>);//创建点云指针，存储点坐标xyz这里创建的智能指针用来存储操作点云．后面访问对象成员用－＞．当然也可以直接定义一个类，用．来访问成员．
２
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/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 <iostream>
#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
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<pcl::PointXYZ>::Ptr clouda_ptr (new pcl::PointCloud<pcl::PointXYZ>);//创建点云指针，存储点坐标xyz
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudb_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudc_ptr (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/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<pcl::PointXYZ> ("/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<sensor_msgs::PointCloud2> ("cloudc", 1);
pubb = nh.advertise<sensor_msgs::PointCloud2> ("cloudb", 1);
puba = nh.advertise<sensor_msgs::PointCloud2> ("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:拼接后的结果
全部评论 (0)
还没有任何评论哟~