Unable to synchronize messages [closed]

edit

Hello,

I'm implementing nodelet based of PCLNodelet class, I need to synchronize messages from two topics, point cloud and plane model. Here is the code:

plane.h:

#ifndef PLANE_H_
#define PLANE_H_

#include <pcl_ros/pcl_nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/pass_through.h>

namespace pcl_ros
{
    namespace sync_policies = message_filters::sync_policies;

    class DistanceFromPlane : public PCLNodelet
    {
        protected:

            void onInit ();

            void input_indices_model_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices, const ModelCoefficientsConstPtr &model);

            ModelCoefficientsConstPtr model_;

            message_filters::Subscriber<ModelCoefficients> sub_model_;

            message_filters::PassThrough<PointIndices> nf_;

            inline void input_callback (const PointCloudConstPtr &input)
            {
                PointIndices cloud;
                cloud.header.stamp = input->header.stamp;
                nf_.add (boost::make_shared<PointIndices> (cloud));
                NODELET_INFO("Input callback called ...");
            }

            // For debug purposes
            inline void model_callback (const ModelCoefficientsConstPtr &model)
            {
                NODELET_INFO("Model callback called ...");
            }

            boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_;
            boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_;

        public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    };
}

#endif

and plane.cpp:

#include <pluginlib/class_list_macros.h>
#include "plane.h"

void pcl_ros::DistanceFromPlane::onInit ()
{
    PCLNodelet::onInit ();

    pub_output_ = pnh_->advertise<pcl::PointCloud<pcl::PointXYZI> > ("output", max_queue_size_);

    sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);

    sub_model_.subscribe (*pnh_, "plane_model", max_queue_size_);

    sub_input_filter_.registerCallback (bind (&DistanceFromPlane::input_callback, this, _1));

    // For debug purposes
    sub_model_.registerCallback (bind (&DistanceFromPlane::model_callback, this, _1));


    if (approximate_sync_)
    {
        sync_input_indices_model_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices, ModelCoefficients> > > (max_queue_size_);
        sync_input_indices_model_a_->connectInput (sub_input_filter_, nf_, sub_model_);
        sync_input_indices_model_a_->registerCallback (bind (&DistanceFromPlane::input_indices_model_callback, this, _1, _2, _3));
    }
    else
    {
        sync_input_indices_model_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices, ModelCoefficients> > > (max_queue_size_);
        sync_input_indices_model_e_->connectInput (sub_input_filter_, nf_, sub_model_);
        sync_input_indices_model_e_->registerCallback (bind (&DistanceFromPlane::input_indices_model_callback, this, _1, _2, _3));
    }
}

void pcl_ros::DistanceFromPlane::input_indices_model_callback (const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices, const ModelCoefficientsConstPtr &model)
{
    if (isValid (cloud) && isValid(model)) {
        NODELET_INFO("Recieved valid data ... ");
    }

    // For debug purposes
    NODELET_INFO("Combined callback called ... ");
}

typedef pcl_ros::DistanceFromPlane DistanceFromPlane;
PLUGINLIB_DECLARE_CLASS (pcl, DistanceFromPlane, DistanceFromPlane, nodelet::Nodelet);

the problem is that I am getting both "Input callback called ..." and "Model callback called ..." but I can't the callback for connected input (input_indices_model_callback) is never called, can you help?

Thanks