ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
As far as I know you should specify the sychronization policy, i.e.
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/time_synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
#include <ros/callback_queue.h>
#include <boost/bind.hpp>
Furthermore you should set a callback queue.
Example: First:
ros::CallbackQueue my_callback_queue_;
message_filters::Subscriber<nav_msgs::Odometry> poseSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> laserSub_;
typedef sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::LaserScan> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Synchronizer;
boost::shared_ptr<Synchronizer> sync_;
Specify the message_filter subscriber:
ros::NodeHandle nh_;
poseSub_.subscribe(nh_, "/robot_pose", 1);
laserSub_.subscribe(nh_, "/scan", 1);
Then set the synchronizer:
sync_.reset(new Synchronizer(MySyncPolicy(10), poseSub_, laserSub_));
sync_->registerCallback(boost::bind(&emergency_handler::avoid, this, _1, _2));
nh_.setCallbackQueue(&my_callback_queue_);
Then, in order to spin the callback:
my_callback_queue_.callOne();
The code I posted is intended for a class use.I hope this helped...
PS: Sorry for my poor English...
2 | No.2 Revision |
As far as I know you should specify the sychronization policy, i.e.
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/time_synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
#include <ros/callback_queue.h>
#include <boost/bind.hpp>
Furthermore you should set a callback queue.
Example: First:
using namespace message_filters;
ros::CallbackQueue my_callback_queue_;
message_filters::Subscriber<nav_msgs::Odometry> poseSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> laserSub_;
typedef sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::LaserScan> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Synchronizer;
boost::shared_ptr<Synchronizer> sync_;
Specify the message_filter subscriber:
ros::NodeHandle nh_;
poseSub_.subscribe(nh_, "/robot_pose", 1);
laserSub_.subscribe(nh_, "/scan", 1);
Then set the synchronizer:
sync_.reset(new Synchronizer(MySyncPolicy(10), poseSub_, laserSub_));
sync_->registerCallback(boost::bind(&emergency_handler::avoid, this, _1, _2));
nh_.setCallbackQueue(&my_callback_queue_);
Then, in order to spin the callback:
my_callback_queue_.callOne();
The code I posted is intended for a class use.I hope this helped...
PS: Sorry for my poor English...