How to convert LaserScan to PCLPointCloud2?
Currently, I'm trying to read the laserscanner data and convert to PCLPointCloud2 for processing. I took the pcl_ros tutorial code and adapted the callback parameter to sensor_msgs::LaserScan
. However, I get a compiler error:
/usr/include/boost/function/function_template.hpp:118:11: error: invalid initialization of reference of type ‘const sensor_msgs::LaserScan_<std::allocator<void> >&’ from expression of type ‘const boost::shared_ptr<const sensor_msgs::LaserScan_<std::allocator<void> > >’
This is the code:
#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include "laser_geometry/laser_geometry.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
ros::Publisher pub;
void cloud_cb(const sensor_msgs::LaserScan &scan_msg)
{
// Container for original & filtered data
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
// Convert to PCL data type
laser_geometry::LaserProjection projector_;
sensor_msgs::PointCloud2 pntCloud;
projector_.projectLaser(scan_msg, pntCloud, 30.0);
pcl_conversions::toPCL(pntCloud, *cloud);
// Perform the actual filtering
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr);
sor.setLeafSize(0.1, 0.1, 0.1);
sor.filter(cloud_filtered);
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
pcl_conversions::moveFromPCL(cloud_filtered, output);
// Publish the data
pub.publish(output);
}
int main(int argc, char **argv)
{
// Initialize ROS
ros::init(argc, argv, "my_pcl_tutorial");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("input", 1000, &cloud_cb);
// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2>("output", 1);
// Spin
ros::spin();
}
I can change the callback paramater to const sensor_msgs::LaserScan::ConstPtr&
. However, I have issue with the projector_.projectLaser(scan_msg, pntCloud, 30.0);
since this fucntion needs LaserScan&
. How can I fix this code?