message_filters::sync::ApproximateTime does not call the callback
I am trying to subscribe to a pointcloud and it's normal from to different subscribers and pass it on to a common function via approximate time sync. Please find minimal code below:
void OrganizedMultiPlaneSegmentation::onInit() {
sub = private_nh.subscribe("/cloud/cloud_out_merged", 1,
&OrganizedMultiPlaneSegmentation::segment, this,
ros::TransportHints().tcpNoDelay());
}
void OrganizedMultiPlaneSegmentation::segment(
const sensor_msgs::PointCloud2& msg) {
ros::NodeHandle& nh_ = getNodeHandle();
sub1_.subscribe(nh_, "/cloud/cloud_out_merged", 5);
if (sub1_.getSubscriber().getNumPublishers() == 1) {
// NODELET_INFO("Got a subscriber to scan, starting subscriber to merged
// pointcloud");
}
sub2_.subscribe(nh_, "/cloud_out_normal", 5);
if (sub2_.getSubscriber().getNumPublishers() == 1) {
// NODELET_INFO("Got a subscriber to scan, starting subscriber to normal
// pointcloud");
}
typedef sync_policies::ApproximateTime<sensor_msgs::PointCloud2,
sensor_msgs::PointCloud2>
MySyncPolicy;
Synchronizer<MySyncPolicy> sync(MySyncPolicy(40), sub1_, sub2_);
// sync.setAgePenalty(1.0);
sync.registerCallback(boost::bind(
&OrganizedMultiPlaneSegmentation::segmentPlanes, this, _1, _2));
}
// Callback function to be called
void OrganizedMultiPlaneSegmentation::segmentPlanes(
const PointCloud2ConstPtr& cloud_msg1,
const PointCloud2ConstPtr& cloud_msg2) {
pcl::PointCloud<pointT>::Ptr cloud(new pcl::PointCloud<pointT>());
pcl::fromROSMsg(*cloud_msg1, *cloud);
pcl::PointCloud<pcl::Normal>::Ptr normalCloud(
new pcl::PointCloud<pcl::Normal>());
I have tried changing queue size of the subscriber, the buffer size of the policy, and both don't seem to work. Please let me know if there is something obvious that I am doing wrong,or what can be possible causes and remedies for this. Happy to provide more information and thank you!