StaticTransformBroadcaster error: Can't see pointCloud2 on RViz
Hello, I am trying to use a StaticTransformBroadcaster
in order to avoid the usual opening of an additional terminal and type rosrun tf static_transform_publisher 0 0 0 0 0 0 1 base_link frame_id 10
.
In order to shrink the problem, I prepared below a minimal verifiable example that you can copy/paste on your machine and it should work. I indicated below (using an <--
)the point where I get the error:
#include <ros/ros.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_ros/publisher.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/publisher.h>
#include <string>
class PMDCloudPublisher
{
protected:
std::string tf_frame;
ros::NodeHandle nh;
ros::NodeHandle private_nh;
public:
sensor_msgs::PointCloud2 cloud;
std::string file_name = "/home/emanuele/Desktop/table_scene_lms400.pcd", cloud_topic;
pcl_ros::Publisher<sensor_msgs::PointCloud2> pub;
PMDCloudPublisher()
: tf_frame("/base_link"),
private_nh("~")
{
cloud_topic = "cloud";
pub.advertise(nh, cloud_topic.c_str(), 1);
private_nh.param("frame_id", tf_frame, std::string("/base_link"));
ROS_INFO_STREAM("Publishing data on topic \"" << nh.resolveName(cloud_topic) << "\" with frame_id \"" << tf_frame << "\"");
}
int start()
{
if (file_name == "" || pcl::io::loadPCDFile(file_name, cloud) == -1)
return (-1);
cloud.header.frame_id = tf_frame;
return (0);
}
bool spin ()
{
int nr_points = cloud.width * cloud.height;
std::string fields_list = pcl::getFieldsList(cloud);
ros::Rate r(40);
while(nh.ok ())
{
ROS_DEBUG_STREAM_ONCE("Publishing data with " << nr_points
<< " points " << fields_list
<< " on topic \"" << nh.resolveName(cloud_topic)
<< "\" in frame \"" << cloud.header.frame_id << "\"");
pubLocalFrame(cloud, nh);
cloud.header.stamp = ros::Time::now();
pub.publish(cloud);
r.sleep();
}
return (true);
}
void pubLocalFrame(sensor_msgs::PointCloud2 cloud, ros::NodeHandle nh)
{
static tf2_ros::StaticTransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "base_link";
transformStamped.child_frame_id = "frame_id";
transformStamped.transform.translation.x = cloud.x; `<-- error here`
transformStamped.transform.translation.y = cloud.y; `<-- error here`
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);
}
};
int main(int argc, char* argv[])
{
ros::init (argc, argv, "cloud_publisher");
ros::NodeHandle nh;
PMDCloudPublisher c;
nh.getParam("pcdFile", c.file_name);
if (c.start () == -1)
{
ROS_ERROR_STREAM("Could not load file \"" << c.file_name <<"\". Exiting...");
return (-1);
}
ROS_INFO_STREAM("Loaded a point cloud with " << c.cloud.width * c.cloud.height
<< " points (total size is " << c.cloud.data.size() << ") and the following channels: " << pcl::getFieldsList (c.cloud));
c.spin();
return 0;
}
Thank you very much for pointing to the right direction for solving this problem.
Asked by RayROS on 2020-05-11 17:14:39 UTC
Comments