Converting PointCloud2: From ROS to PCL (converting in callback vs directly)

edit

Hello, I am subscribing to my point cloud topic generated by stereo_image_proc and trying to convert it from the ROS format (sensor_msgs::PointCloud2) to the PCL format (PointCloud< PointXYZ >) so I can use PCL functions. I know there are a few ways to do this, but one method is much slower and the other does not allow for message filtering as I want to synchronize my pointcloud callback with an image callback.

Method 1: Subscribe Directly as a PointCloud< PointXYZ > while including pcl_ros/point_cloud.h. Described here: http://wiki.ros.org/pcl_ros as well as below.

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>

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

void callback(const PointCloud::ConstPtr& msg)
{
  printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
  BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
    printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "sub_pcl");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe<PointCloud>("points2", 1, callback);
  ros::spin();
}

This method does not allow me to synchronize callbacks by message_filtering using the method described here: http://wiki.ros.org/message_filters#T... . My code compiles, however, I never get a synchronized callback. it was suggested in this post that it is not possible: http://answers.ros.org/question/18693...

Method 2: Subscribe to sensor_msgs::PointCloud2 and convert within the synchronized callback using fromROSMsg. Described below as well as here: http://wiki.ros.org/pcl/Tutorials#pcl...

#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>

...

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);

  pcl::ModelCoefficients coefficients;
  pcl::PointIndices inliers;
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

  seg.setInputCloud (cloud.makeShared ());
  seg.segment (inliers, coefficients);

  // Publish the model coefficients
  pcl_msgs::ModelCoefficients ros_coefficients;
  pcl_conversions::fromPCL(coefficients, ros_coefficients);
  pub.publish (coefficients);
}

This was my primary method of choice, however, fromROSMsg has slowed down (at least on my computer: have not tested on others) since a recent update. I used to be able to process pointclouds at 30Hz a few days ago, but now it is reduced to 5Hz (approx .1s for each call to fromROSMsg()). Is anyone else having speed issues related to fromROSMsg? Subscribing to PointCloud< PointXYZ > works much faster than fromROSMsg right now, but I am unable to synchronize my callbacks. Any recommendations would be of great help.

Ubuntu 14.04. ROS Indigo.

Thanks, -Frank