How to Connect a tf::messageFilter to two subscribers

asked 2014-09-20 08:17:01 -0500

1493 ●40 ●58 ●75 http://www.scratchsyst...

Hi All,

I would like to synchronise two sensor inputs with a tf::messageFilter. How would I go about connecting both message_filters::subscribers to a single tf::messageFilter.

I have tried to sync both the subscribers via a synchroniser using an approximate time policy. I would like to connect this to the tf::messageFilter In the header file I have:

tf::TransformListener m_tfListener;
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::NavSatFix, sensor_msgs::Imu> MySyncPolicy;
    message_filters::Subscriber< sensor_msgs::NavSatFix > *m_gpsSub;
    message_filters::Subscriber<sensor_msgs::Imu> *m_imuSub;
    message_filters::Synchronizer<MySyncPolicy>* sync;

    tf::MessageFilter< sensor_msgs::NavSatFix, sensor_msgs::Imu>* m_gpsFilter;

in the .cpp file I have

 m_gpsSub= new message_filters::Subscriber<sensor_msgs::NavSatFix>(private_nh, "fix", 5);
  m_imuSub = new message_filters::Subscriber<sensor_msgs::Imu>(private_nh, "imu", 5) 
  m_gpsFilter = new tf::MessageFilter<sensor_msgs::NavSatFix, sensor_msgs::Imu>(m_tfListener, odomFrameId, 10) 
  sync= new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(20), *m_gpsSub, *m_imuSub);
  m_gpsFilter->connectInput(sync);
  m_gpsFilter->registerCallback(boost::bind(&SwarmbotMCL::gpsCallback, this, _1, _2));

however It complains that tf::MessageFilter can only take one input:

 error: wrong number of template arguments (2, should be 1)
/opt/ros/hydro/include/tf/message_filter.h:106:7: error: provided for ‘template<class M> class tf::MessageFilter’

does anyone know of the correct way to do this?

edit retag flag offensive close merge delete

add a comment

4

answered 2014-09-20 09:48:50 -0500

PeterMilani

1493 ●40 ●58 ●75 http://www.scratchsyst...

updated 2014-09-20 09:50:01 -0500

Based on the insight given in a previous answer that this is possible because the tf::message_filter is just another message filter, I came up with the following working solution. Sync your tf::message_filter with one (or possibly both) your message_filter subscribers. pass the pointer of the resulting tf::message_filter to your synchronizer, and link you synchronizer to the callback. As in the following reworked .cpp code :

m_gpsSub= new message_filters::Subscriber<sensor_msgs::NavSatFix>(private_nh, "fix", 5);

  m_imuSub = new message_filters::Subscriber<sensor_msgs::Imu>(private_nh, "imu", 5);
  //m_gpsSub->connectInput(m_imuSub);
  m_gpsFilter = new tf::MessageFilter<sensor_msgs::NavSatFix>(*m_gpsSub, m_tfListener, odomFrameId, 10);

  sync= new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(20), *m_gpsFilter,  *m_imuSub );
  //m_gpsFilter->connectInput(sync);
  sync->registerCallback(boost::bind(&SwarmbotMCL::gpsCallback, this, _1, _2));

edit flag offensive delete link

Comments

Thanks very much! It works!

nkuwenjian ( 2020-03-21 05:19:25 -0500 )edit

add a comment

0

answered 2014-09-20 09:23:04 -0500

paulbovbel

4518 ●12 ●49 ●80 http://www.bovbel.com/

updated 2014-09-22 08:56:02 -0500

I think you have to create two tf message filters, and pipe their output into the time filter, however I haven't tried this personally.

edit flag offensive delete link

Comments

I've think I've got that part working. The gps and imu subscribers are synched and will trigger a callback if linked to one. However, I also want to include the tf::MessageFilter into that process... It would be good if the tf::MessageFilter could be an input on the Synchronizer.