Display PointCloud2 in Rviz

edit

Okay, I got everything working now. The code from pcd_to_pointcloud worked perfectly:

#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, 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 << "\"");
      cloud.header.stamp = ros::Time::now();
      pub.publish(cloud);

      r.sleep();
    }
    return (true);
  }
};

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;
}

The problem I mentioned in EDIT4 was due to the fact that catkin_make has yet again screwed things up when I rebuilded my package and it actually had the old binaries of my cloud_publisher resulting in the same error with a different code. I had to delete build and devel and after this clean-up it works now.

For those who are interested after you start publishing the point cloud with the publisher in this answer do the following:

• open Rviz
• select /base_frame as your fixed frame (by default you get /map
• add a PointCloud2
• for the newly added point cloud select the topic to be /cloud (this is how it's named by the publisher above, you can of course change it with whatever you like)

Voila! You have displayed your point cloud inside Rviz with a dummy TF. :) Usually when you are using OpenNI a whole frame tree is provided by the drivers so you don't neet to bother tinkering with the TF data. But in my case when you need to simply display a PCD file or something similar, you need to use a workaround.