PCL - coordinates from PointCloud2 message

asked 2017-07-03 16:30:59 -0500

sagemsux gravatar image

Hey guys,

So I'm a little new to ROS, so forgive some amateurishness. I'm working with a LiDAR that publishes a topic (/Sensor/points) in the format sensor_msgs::PointCloud2. I'm trying to write a subscriber program (C++) to take this data and output the x,y,z coordinates. I'm later hoping to store these values for some analysis.

I've converted the PointCloud2 format to a PointCloud<pointxyz> format and then, when I print points.size(), the LiDAR is outputting over 40,000 points every time the callback function is invoked. I'm assuming that these are 40,000 sets of x,y and z coordinates and I want to access these.

I've tried to do points[0] (many different numbers apart from zero as well) and all I get is (nan,nan,nan). Any help would be greatly appreciated! I've attached my code below (except the include fies).

For reference, this is ROS-Indigo on Ubuntu 14.04 LTS.

Thanks,

Rayal

void interpet(const sensor_msgs::PointCloud2 msg)
{                               
  pcl::PointCloud<pcl::PointXYZ> msg_;
  pcl::fromROSMsg(msg,msg_);


  ROS_INFO_STREAM(msg_.points[0]); //index 0 because I'm not really sure what to do
}

int main(int argc, char **argv)
{
  ros::init(argc,argv,"Receiver");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("Sensor/points",1000,&interpet);

  ros::spin();
}
edit retag flag offensive close merge delete

Comments

1

It is possible to have to NaN on the edges of a pointcloud, I would recomend to check whether you get valid data in the pointcloud by parsing all points and find the NaN and maybe you can count them and see what percentage of NaNs you get.

angeltop gravatar imageangeltop ( 2017-07-04 02:34:43 -0500 )edit

This worked! You were right - I filtered out the points that were not finite and got left with the data I needed.

Thanks

sagemsux gravatar imagesagemsux ( 2017-07-07 09:27:12 -0500 )edit