Write PCD File from a Topic - Hydro + PCL (Migration)
Hello, I'm doing a hand gesture recognition project and I am having some troubles with the migration of the PCL data types to ROS Hydro. I started with Hydro + PCL this week so, not all the concepts are really clear for me. I'm using Ubuntu 12.04.
My problem is to transform the data that come from my passthrough filter (sensor_msgs/PointCloud2) to a pcl::PointCloud pcl::PointXYZRGB, so I can write the data using the savePCDFileASCII function.
My code is:
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h> //hydro
#include <sensor_msgs/PointCloud2.h> //hydro
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
ros::Publisher pub;
void
cloud_cb (const pcl::PCLPointCloud2ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZRGB> cloud_filtered;
pcl::fromROSMsg(*input, cloud_filtered);
//cloud_filtered.header = pcl_conversions::toPCL(input.header);
//pcl_conversions::toPCL(input, cloud_filtered);
pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud_filtered);
pub.publish(input);
}
int
main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "pcl_node");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("/passthrough", 1, cloud_cb);
// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
// Spin
ros::spin ();
//End Program
return (0);
}
As you guys can see in my code, I tried some transformations after reading the hydro/Migration page, but I couldn't figure it out what i need to do to make my code work.
The error while doing catkin_make is a no matching function because the type error:
/home/breil/catkin_ws/src/pcl_tutorial/src/example.cpp: In function ‘void cloud_cb(const PCLPointCloud2ConstPtr&)’:
/home/breil/catkin_ws/src/pcl_tutorial/src/example.cpp:21:41: error: no matching function for call to ‘fromROSMsg(const pcl::PCLPointCloud2&, pcl::PointCloud<pcl::PointXYZRGB>&)’
/home/breil/catkin_ws/src/pcl_tutorial/src/example.cpp:21:41: note: candidate is:
/opt/ros/hydro/include/pcl_conversions/pcl_conversions.h:486:8: note: template<class T> void pcl::fromROSMsg(const PointCloud2&, pcl::PointCloud<PointT>&)
Thank you very much for your attention.
I think you do not need this to... and fromRosMSG() anymore in hydro. A least for publishing you just put the cloud into the publish method of a sensro_msgs::PointCloud2 publisher and unlike in prev versions it just works. But I dont know the opposite way..
Yes Wolf, the publisher is working fine with the type. The problem is to save the PCD File, because I can't use sensor_msgs/PointCloud2 type. The pcl::io::savePCDFileASCII function. So I need to do some transformation before. int pcl::io::savePCDFileASCII(const string&, const pcl::PointCloud<pointt>&)