ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Problems publishing synchronized topics

asked 2015-03-19 07:03:39 -0600

aba92 gravatar image

I want to publish image synchronized topics using following code:

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& left_image, const CameraInfoConstPtr& left_cam_info, const ImageConstPtr& right_image, const CameraInfoConstPtr& right_cam_info)
{
  // Solve all of perception here...
    printf("-----------------------\n");
    std::cout << "Time stamp (left image): "<< left_image->header.stamp << std::endl;
    std::cout << "Time stamp (right image): "<< right_image->header.stamp << std::endl;
    std::cout << "Time stamp (left camera info): "<< left_cam_info->header.stamp << std::endl;
    std::cout << "Time stamp (right camera info): "<< right_cam_info->header.stamp << std::endl;
}

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

  ros::NodeHandle nh;

  message_filters::Subscriber<Image> left_image_sub(nh, "vizzy/left/image_raw", 1);
  message_filters::Subscriber<CameraInfo> left_info_sub(nh, "vizzy/left/camera_info", 1);
  message_filters::Subscriber<Image> right_image_sub(nh, "vizzy/right/image_raw", 1);
  message_filters::Subscriber<CameraInfo> right_info_sub(nh, "vizzy/right/camera_info", 1);

  TimeSynchronizer<Image, CameraInfo, Image, CameraInfo> sync(left_image_sub, left_info_sub, right_image_sub, right_info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4));    // cria 4 callbacks

  ros::spin();

  return 0;
}

but I see that the Callback function is not fired and don't know why. Anyone can help me and if possible tell me if in order to output the synchronized topics I can just publish them with simple advertise?

edit retag flag offensive close merge delete

Comments

Are you sure the time stamps do EXACTLY match? The TimeSynchronizer requires this for firing the callback, no epsilon allowed....

Wolf gravatar image Wolf  ( 2015-03-19 08:33:21 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-03-19 08:30:05 -0600

Chaos gravatar image

updated 2015-03-19 08:31:45 -0600

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...

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2015-03-19 07:03:39 -0600

Seen: 1,126 times

Last updated: Mar 19 '15