PCL - coordinates from PointCloud2 message
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();
}
This worked! You were right - I filtered out the points that were not finite and got left with the data I needed.
Thanks