StaticTransformBroadcaster error: Can't see pointCloud2 on RViz

asked 2020-05-11 17:14:39 -0500

RayROS gravatar image

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.

edit retag flag offensive close merge delete