Ask Your Question
1

How to convert LaserScan to PCLPointCloud2?

asked 2020-10-16 06:58:09 -0500

wiyogo gravatar image

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?

edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
2

answered 2020-10-16 09:43:33 -0500

JackB gravatar image

updated 2020-10-16 11:36:07 -0500

I have not tried to run and validate the code, but you should as you say at the end, "change the callback parameter to const sensor_msgs::LaserScan::ConstPtr& ". If you do not do that the callback will never work because ROS needs it to be a boost shared_ptr.

So with that back to the way that it should be, we need to fix passing the projector_.projectLaser(scan_msg, pntCloud, 30.0); the right argument. To do this you will need to dereference the smart_ptr.

So the call would look like:

projector_.projectLaser(*scan_msg, pntCloud, 30.0);

I think this should solve your concern about "since this function needs LaserScan&. How can I fix this code?"

EDIT You may actually need to double dereference it, because the callback is passed is a reference to a smart_ptr, so it would actually look like:

 projector_.projectLaser(**scan_msg, pntCloud, 30.0);
edit flag offensive delete link more

Comments

Thanks! So sensor_msgs::LaserScan::ConstPtr is another type of pointer. projector_.projectLaser(*scan_msg, pntCloud, 30.0); once deference works. What is the reason not to use void cloud_cb(const sensor_msgs::LaserScan* scanmsg)? ConstPtr is not a general C++ type.

wiyogo gravatar image wiyogo  ( 2020-10-16 16:44:54 -0500 )edit

See this answer to understand the ConstPtr

JackB gravatar image JackB  ( 2020-10-17 14:23:26 -0500 )edit
1

answered 2020-10-16 13:27:07 -0500

duck-development gravatar image

the right subscribe signatuure for laser scannner is

void laserCB(const sensor_msgs::LaserScan::ConstPtr& msg){
ROS_INFO("LaserScan (val,angle)=(%f,%f", msg->range_min,msg->angle_min);
}

and for the projector you write

 projector_.projectLaser(*msg, pntCloud, 30.0);

you find an working example plus description here

ros wiki

edit flag offensive delete link more

Comments

thanks for the answer!

wiyogo gravatar image wiyogo  ( 2020-10-16 16:46:09 -0500 )edit

After build the program, how can I visualize it in rviz? Adding the PointCloud2 in Rviz, I got this error:

For frame [laser]: Fixed Frame [map] does not exist

Somehow I need to assign a map frame or something similar.

wiyogo gravatar image wiyogo  ( 2020-10-16 16:51:42 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-10-16 06:58:09 -0500

Seen: 40 times

Last updated: Oct 16