Why the callback function binded by message_filters is not working?

asked 2018-08-16 22:05:32 -0500

Megacephalo gravatar image

Hi all,

I have referred to several ROS Answers questions and GitHub projects for coding reference but still with no avail. I want to have message_filters subscribe to 4 topics and synchronize in one single callback, as shown in the code below. The code can be compiled, but even with all the necessary topics provided (and updated at all time, each one of the topics) and subscribed to the node, the topics seems not to be accepted by the common callback function. What am I doing wrong? How to make the callback works properly? Thanks in advance!

In my header file

#include <memory>

#include <ros/ros.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

namespace alpha {
class A {
  public:
    A() ;
    ~A() {} ;

  private:
    std::unique_ptr< message_filters::Subscriber<geometry_msgs::TwistStamped> > twistStamped_sub_ ;
    std::unique_ptr< message_filters::Subscriber<sensor_msgs::LaserScan> > scan_sub_ ;
    std::unique_ptr< message_filters::Subscriber<nav_msgs::OccupancyGrid> > map_sub_ ;
    std::unique_ptr< message_filters::Subscriber<nav_msgs::Odometry> > odom_sub_ ;
    typedef message_filters::sync_policies::ApproximateTime< geometry_msgs::TwistStamped
                                                                      , sensor_msgs::LaserScan
                                                                      , nav_msgs::OccupancyGrid
                                                                      , nav_msgs::Odometry> mySyncPolicy ;
    typedef message_filters::Synchronizer<mySyncPolicy> Sync ;
    std::shared_ptr<Sync> sync_ ;

    void sync_callback( const geometry_msgs::TwistStamped::ConstPtr& twistStampedMsg
                           , const sensor_msgs::LaserScan::ConstPtr& scanMsg
                           , const nav_msgs::OccupancyGrid::ConstPtr mapMsg
                           , const nav_msgs::Odometry::ConstPtr& odomMsg) ;

} ; /* End of class A */

} /* End of namespace */

While in my cpp file

#include <my_project/my_header.h>

namespace alpha {

A::A() {
    twistStamped_sub_.reset(new message_filters::Subscriber<geometry_msgs::TwistStamped>(node_, "internal_use_twist_stamped", 10) ) ;
    scan_sub_.reset(new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 10) ) ;
    map_sub_.reset(new message_filters::Subscriber<nav_msgs::OccupancyGrid>(node_, "map", 10) ) ;
    odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "odom", 10) ) ;

    sync_.reset( new Sync(mySyncPolicy(10), *twistStamped_sub_, *scan_sub_, *map_sub_, *odom_sub_) ) ;
    sync_->registerCallback( boost::bind(&safe_cmd_vel_governor::sync_callback, this, _1, _2, _3, _4) ) ;
} /* End of constructor */


void 
A::sync_callback( const geometry_msgs::TwistStamped::ConstPtr& twistStampedMsg
                    , const sensor_msgs::LaserScan::ConstPtr& scanMsg
                    , const nav_msgs::OccupancyGrid::ConstPtr mapMsg
                    , const nav_msgs::Odometry::ConstPtr& odomMsg) {
    // Do something ...
} /* End of sync_callback() */
} /* End of namespace */



int main(int argc, char **argv){
    ros::init(argc, argv, "my_node") ;

    alpha::A a ;

    ros::AsyncSpinner spinner(4);
    spinner.start() ;
    ros::waitForShutdown() ;

    return EXIT_SUCCESS ;
} /* End of main */
edit retag flag offensive close merge delete

Comments

hi. I also meet the same situation, would you like to share the solution to this question to me?thx

GFZRZK gravatar image GFZRZK  ( 2018-12-27 02:16:44 -0500 )edit

Hi. What are these four topics publishing frequency?

HankYuan gravatar image HankYuan  ( 2019-02-12 03:54:37 -0500 )edit