Ask Your Question
1

Display PointCloud2 in Rviz

asked 2015-08-05 12:54:19 -0500

rbaleksandar gravatar image

updated 2015-08-05 15:06:31 -0500

Hi!

I realize this question has been chewed a lot here but from all the answers I've read nothing seems to work. I'm using ROS Indigo on Ubuntu 14.04 LTS (32bit).

Before I start reading and publishing my own data I decided to check out how things actually work with one of these PCD files.

Here is my publisher:

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <string>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

using std::string;

int main(int argc, char** argv)
{
  ros::init (argc, argv, "cloud_publisher");
  ros::NodeHandle nh;

  string pcdFile = "";
  nh.getParam("pcdFile", pcdFile);

  if(pcdFile == "") {
    ROS_ERROR_STREAM("Unable to find file \"" << pcdFile << "\"");
    nh.shutdown();
  }

  ros::Publisher pub = nh.advertise<PointCloud> ("cloud_publisher/cloud_from_file", 1);

  PointCloud::Ptr msg(new PointCloud);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcdFile, *msg) == -1)
  {
    ROS_ERROR_STREAM("Unable to read from file \"" << pcdFile << "\"");
    nh.shutdown();
    return -1;
  }

  ros::Rate r(10);

  while (nh.ok())
  {
    msg->header.frame_id = "/my_frame";
    //msg->header.stamp = ros::Time::now().toNSec();
    pub.publish(msg);
    ros::spinOnce();
    r.sleep();
  }

  return 0;
}

Basically I read a PCD file and send it on its way to the rest of the ROS network. I want to see the result in RViz. Now here is the thing - I get the ever so lovely error

No tf data. Actual error: Fixed Frame [map] does not exist

in my global status of Rviz. I also added a PointCloud2 item to RViz but the moment I selected the topic that my cloud_publisher is publishing roslaunch (I use a launch file to load the PCD file in my node) breaks with another known error the moment I set the topic of my PointCloud2 to cloud_publisher/cloud_from_file:

terminate called after throwing an instance of 'std::runtime_error'
what(): Time is out of dual 32-bit range [cloud_publisher-1] process has died [pid 2015, exit code -6, cmd /home/rosuser/catkin_ws/devel/lib/pcl_misc/cloud_publisher __name:=cloud_publisher __log:=/home/rosuser/.ros/log/32182a0e-3b92-11e5-9e58-001e0ba4cf00/cloud_publisher-1.log]. log file: /home/rosuser/.ros/log/32182a0e-3b92-11e5-9e58-001e0ba4cf00/cloud_publisher-1*.log all processes on machine have died, roslaunch will exit

I read that you have to either provide a transformation (using TF) from the map fixed frame to all others that you publish (in my case I have only my_frame) or simply set my_frame to be the global frame too. Well neither is working due to the crash of my node.

Can you please help me resolve this issue? It is not possible that displaying a single item (in my case a point cloud) in Rviz is that complicated.

Thanks!

EDIT: I just tried using rostopic echo /cloud_publisher/cloud_from_file and my publisher crashed again! It seems the issue is not because of Rviz (although the missing TF data is still bothering me...). Something fishy is going on with the topic itself...

EDIT2: I forgot to mention that I have also tried this solution. I also wrote the wrong architecture. I have a 32bit CPU.

EDIT3: I tried borrowing code from ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2015-08-05 15:18:11 -0500

rbaleksandar gravatar image

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.

edit flag offensive delete link more

Comments

How do you get your pcd-file into the program? I tried typing the path to the file into the program but that doesnt work. Can you please help me?

jrgen gravatar imagejrgen ( 2017-10-04 03:00:56 -0500 )edit

It's been a while so I might be wrong but looking at the code in my answer you can see that I pass it as a parameter with the name pcdFile (defined in the roslaunch file).Further rosparam can also handle loading files by using <rosparam command="load" file="<your_pcd_file>"/>. Hope this helps

rbaleksandar gravatar imagerbaleksandar ( 2017-10-04 03:16:37 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2015-08-05 12:54:19 -0500

Seen: 3,897 times

Last updated: Oct 04 '17