roscpp message_filters Synchronizer.registerCallback problem
Hi,
For the example of message filter in this example , I tried to register callback from a class member function as in the following code:
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
using namespace sensor_msgs;
using namespace message_filters;
typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
void callback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
// Solve all of perception here...
}
class A {
public:
void my_callback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
}
void init(const Synchronizer<MySyncPolicy>& sync) {
//Fail to compile
sync.registerCallback(boost::bind(&A::my_callback, this, _1, _2));
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "vision_node");
ros::NodeHandle nh;
message_filters::Subscriber<Image> image1_sub(nh, "image1", 1);
message_filters::Subscriber<Image> image2_sub(nh, "image2", 1);
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));
A a;
// this is OK
sync.registerCallback(boost::bind(&A::my_callback, &a, _1, _2));
// but this fails
a.init(sync);
ros::spin();
return 0;
}
I could call registerCallback
of the A class method in the main sync.registerCallback(boost::bind(&A::my_callback, &a, _1, _2));
. But when I made the call in the init
of the class definition it failed to compiled with the following errors:
In member function ‘void A::init(const message_filters::Synchronizer<message_filters::sync_policies::approximatetime<sensor_msgs::image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> > > >&)’:
/home/mango/workspace/cyngn_ros/src/lev-ros/lev_localization/src/message_test.cpp:26:69: error: no matching function for call to ‘message_filters::Synchronizer<message_filters::sync_policies::approximatetime<sensor_msgs::image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> > > >::registerCallback(boost::_bi::bind_t<void, boost::_mfi::mf2<void,="" a,="" const="" boost::shared_ptr<const="" sensor_msgs::image_<std::allocator<void=""> > >&, const boost::shared_ptr<const sensor_msgs::image_<std::allocator<void=""> > >&>, boost::_bi::list3<boost::_bi::value<a*>, boost::arg<1>, boost::arg<2> > >) const’
sync.registerCallback(boost::bind(&A::my_callback, this, _1, _2));
/opt/ros/kinetic/include/message_filters/synchronizer.h:302:14: note: candidate: message_filters::Connection message_filters::Synchronizer<policy>::registerCallback(C&) [with C = boost::_bi::bind_t<void, boost::_mfi::mf2<void,="" a,="" const="" boost::shared_ptr<const="" sensor_msgs::image_<std::allocator<void=""> > >&, const boost::shared_ptr<const sensor_msgs::image_<std::allocator<void=""> > >&>, boost::_bi::list3<boost::_bi::value<a*>, boost::arg<1>, boost::arg<2> > >; Policy = message_filters::sync_policies::ApproximateTime<sensor_msgs::image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> > >] <near match="">
Connection registerCallback(C& callback)
/opt/ros/kinetic/include/message_filters/synchronizer.h:302:14: note: conversion of argument 1 would be ill-formed:
/home/mango/workspace/cyngn_ros/src/lev-ros/lev_localization/src/message_test.cpp:26:38: error: invalid initialization of non-const reference of type ‘boost::_bi::bind_t<void, boost::_mfi::mf2<void,="" a,="" const="" boost::shared_ptr<const="" sensor_msgs::image_<std::allocator<void=""> > >&, const boost::shared_ptr<const sensor_msgs::image_<std::allocator<void=""> > >&>, boost::_bi::list3<boost::_bi::value<a*>, boost::arg<1>, boost::arg<2> > >&’ from an rvalue of type ‘boost::_bi::bind_t<void, boost::_mfi::mf2<void,="" a,="" const="" boost::shared_ptr<const="" sensor_msgs::image_ ...