TransformBroadcaster not showing PointCloud2 on RViz

asked 2020-01-03 10:36:50 -0500

RayROS gravatar image

updated 2020-01-03 10:59:18 -0500

I am following as exercise the Navigation Stack Examples and am having problem when trying to publish the pointCloud2 message. I successfully publish the odometry message correctly but when I pass to publish the PointCloud2 I have some problems. For how to publish a pointCloud2 I followed this tutorial as from official documentation the message currently present on the Navigation Stack Sensor Stream over ROS is pointCloud but that is deprecated.

The only missing passage on how to publish a pointCloud2 on RViz was adding the TransformBroadcaster and that is exactly what I have done but still I get no tf data received:

no-tf-data

Below both a print screen of RViz not showing anything and the whole node:

rviz-points

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <ros/ros.h>
// PCL specific includes
#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>

#include <tf/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

ros::Publisher pub;

void cloudCb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    // Container for original and 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.1, 0.1, 0.1);
    sor.filter(cloud_filtered);

    // Convert to ROS data type
    sensor_msgs::PointCloud2 output;
    pcl_conversions::fromPCL(cloud_filtered, output);

    static tf2_ros::TransformBroadcaster br;
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "map";
    transformStamped.child_frame_id = "base_link";
    transformStamped.transform.translation.x = 0.1; 
    transformStamped.transform.translation.y = 0.1;
    transformStamped.transform.translation.z = 0.1;

    tf2::Quaternion q;
    q.setRPY(0,0,0);
    transformStamped.transform.rotation.x = q.x();  
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

    br.sendTransform(transformStamped);

    // Publish
    pub.publish(output);
}

int
main (int argc, char** argv)
{
    // Initialize ROS
    ros::init (argc, argv, "point_cloud_publisher");
    ros::NodeHandle nh;

    // Create a ROS subscriber for the input point cloud
    ros::Subscriber sub = nh.subscribe ("input", 1, cloudCb);

    // Create a ROS publisher for the output point cloud
    pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

    // Spin
    ros::spin();
}

Despite adding the TransformBroadcaster and folloeing the example on how to publish pointCloud2, RViz is not showing any pointclouds. Whay is that happening?

edit retag flag offensive close merge delete