Conversion from Point Cloud (PCL) to PointCloud2 (ROS) not showing result on RViz

asked 2019-06-13 10:06:09 -0600

RayROS
108 ●43 ●47 ●52

updated 2019-06-13 10:09:23 -0600

Hello, I followed this tutorial using PCL libraries to perform the conversion from point cloud into a ROS readable message PointCloud2. After compiling and running the example everything works, but I can't see anything on RViz. I have been trying to catch the error but no luck so far. Also in this post they had the same problem but no solution was found. Also this post was useful but no answer was provided.

The code is the same as the tutorial, and I am providing it below for completeness:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.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::PointCloud2ConstPtr& cloud_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
    pcl_conversions::toPCL(*cloud_msg, *cloud);

    // Perform the actual filtering
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloudPtr);
    sor.setLeafSize (0.1f, 0.1f, 0.1f);
    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, "pcl_tutorial_cloud");
    ros::NodeHandle nh;
    // Create a ROS subscriber for the input point cloud
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("input", 1, cloud_cb);
    // Create a ROS publisher for the output point cloud
    pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
    // Spin
    ros::spin ();
}

Additional Source:

However, I didn't want to stop at that example only and tried to read from a .pcd file on my Desktop and try to publish it and showing PointCloud2on on RViz. And unfortunately this time too nothing was showing on RViz. Below the code that reads the file from my Desktop and try to publish and visualize point clouds on RViz:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.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::PointCloud2ConstPtr& cloud_msg)
{
    // Container for original & filtered data
    pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
    pcl::PCLPointCloud2 cloud_filtered;

    // Fill in the cloud data
    pcl::PCDReader reader;
    // Replace the path below with the path where you saved your file
    reader.read ("/home/to/Desktop/cloud_25.pcd", *cloud); 

    std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
         << " data points (" << pcl::getFieldsList (*cloud) << ").";

    // Convert to PCL data type
    pcl_conversions::toPCL(*cloud_msg, *cloud);

    // Perform the actual filtering
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloudPtr);
    sor.setLeafSize (0.1f, 0.1f, 0.1f);
    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, "pcl_tutorial_cloud");
    ros::NodeHandle nh;
    // Create a ROS subscriber for the input point cloud
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("input", 1, cloud_cb);
    // Create a ROS publisher for the ...

(more)

edit retag flag offensive close merge delete

add a comment

answered 2019-06-13 10:55:41 -0600

PeteBlackerThe3rd

flag of United Kingdom of Great Britain and Northern Ireland

9529 ●30 ●143 ●135 https://www.linkedin.c...

Your first code example is a point cloud filter not a point cloud generator, you have a node which is subscribing to a sensor_msgs/PointCloud2 (a ROS message type) filters it and then re-publishes it in the same message type on a different topic. This node isn't producing any point clouds and unless you are running another node which produces point clouds then it will not do anything. It is listening for point clouds on the input topic, so if nothing is publishing on that topic then the callback will never be executed.

Your second code example is closer but for some reason still has a subscriber listening for point clouds on the input topic. Because nothing is being published on input again this callback will never be executed. The error messages you posted appear to show you trying to execute the cpp source file directly. Do you build the node using catkin make and execute it using rosrun or roslaunch?

You'll want to get rid of the subscriber and callback completely and setup a while loop inside main, with a sensible rate such as 1 second and publish the point cloud from within that loop. The Pseudo code for you node should look something like this:

main()
{
    init node

    create pointcloud publisher

    create rate object with 1 second duration

    load point cloud from file

    while(ros::ok())
    {
        rate.sleep

        publish point cloud message
    }
}