TransformBroadcaster not showing PointCloud2 on RViz
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
:
Below both a print screen of RViz
not showing anything and the whole node:
#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?