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

Is that possible to synchronize an average 5 fps topic with 100 fps topic? Did I init subscriber in corret place?

asked 2018-03-23 10:11:22 -0500

waschbaer00 gravatar image

updated 2018-03-23 11:45:44 -0500

Hi there,

I have a YOLO detector topic which produces Boundingboxes with average rate 2-5.

My another topic has average 100 rate.

Is that possible to synchronize both topics? And how to set the queue size or any other arguments for sync CallBack?

Right now thw ROS_INFO_STREAM inside synCB does not print onto screen, i.e. synCB is not reacted.

In pupildarknet.h I declare a class and initialize message_filters.Subscriber in the class constructor. Did I initialize in wrong place?

#ifndef PUPILDARKNET_H
#define PUPILDARKNET_H

#include <boost/bind.hpp>
#include <ros/ros.h>
#include <std_msgs/Int8.h>
#include <darknet_ros_msgs/BoundingBox.h>
#include <darknet_ros_msgs/BoundingBoxes.h>
#include <sensor_msgs/Image.h>
#include <pupil_ros_plugin/gaze_positions.h>

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

#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Image.h>

#include <cv_bridge/cv_bridge.h>

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

#include <iostream>
using namespace cv;
using namespace std;

class PupilDarknet {
private:
  ros::NodeHandle nh_;
  ros::Subscriber sub_numDetObj;

  pupil_ros_plugin::gaze_positions in_gaze;
  cv_bridge::CvImagePtr in_cv_ptr;
  darknet_ros_msgs::BoundingBoxes in_bbes;
  std_msgs::Int8 numDetObj;

  bool verbose;
  /// TODO: CHECK DIFF BETWEEN DECLARATION AS PRIVATE MEMBERS AND IN CONSTRUCTOR.
  //  message_filters::Subscriber<pupil_ros_plugin::gaze_positions> sub_gazePose;
  //  message_filters::Subscriber<sensor_msgs::Image> sub_image;
  //  message_filters::Subscriber<darknet_ros_msgs::BoundingBoxes> sub_DarkentBBes;

public:
  PupilDarknet(ros::NodeHandle nh) : nh_(nh)
  {
      numDetObj.data = 0;

      message_filters::Subscriber<sensor_msgs::Image> sub_image(nh_,"/camera/rgb/image_raw",30);
      message_filters::Subscriber<darknet_ros_msgs::BoundingBoxes> sub_DarkentBBes(nh_,"/darknet_ros/bounding_boxes",30);
      message_filters::Subscriber<pupil_ros_plugin::gaze_positions> sub_gazePose(nh_,"/pupil_capture/gaze",30);

      typedef message_filters::sync_policies::ApproximateTime<
          sensor_msgs::Image,
          darknet_ros_msgs::BoundingBoxes,
          pupil_ros_plugin::gaze_positions> syncPolicy;

      message_filters::Synchronizer<syncPolicy> sync(
            syncPolicy(1),
            sub_image,
            sub_DarkentBBes,
            sub_gazePose);

      sync.registerCallback(boost::bind(&PupilDarknet::synCB,this, _1,_2,_3));

      sub_numDetObj = nh_.subscribe("/darknet_ros/found_object", 1, &PupilDarknet::getNumDetObj, this);

  }
  ~PupilDarknet(){}

  void getNumDetObj(std_msgs::Int8 msg);
  void synCB(const sensor_msgs::ImageConstPtr& img,
             const darknet_ros_msgs::BoundingBoxesConstPtr& BBes,
             const pupil_ros_plugin::gaze_positionsConstPtr& gaze);

};

#endif // PUPILDARKNET_H

My cpp file:

void PupilDarknet::synCB(const sensor_msgs::ImageConstPtr& img, const darknet_ros_msgs::BoundingBoxesConstPtr& BBes, const pupil_ros_plugin::gaze_positionsConstPtr& gaze)
{
  ROS_INFO_STREAM("Hello This is pupil_darknet synCB...");
}
edit retag flag offensive close merge delete

Comments

Yes, the ApproximateTimeSynchronizer will do this.

ahendrix gravatar image ahendrix  ( 2018-03-23 10:16:07 -0500 )edit

I would say this is the answer @ahendrix. Did you have a particular reason for not posting it as one?

gvdhoorn gravatar image gvdhoorn  ( 2018-03-23 10:55:35 -0500 )edit

My synCB is not working, and there is no compile error, so I guess it is the rate differs too much and therefore can not be synchronized.

waschbaer00 gravatar image waschbaer00  ( 2018-03-23 11:27:13 -0500 )edit

Or it could because I init subscribers in wrong place.

waschbaer00 gravatar image waschbaer00  ( 2018-03-23 11:46:15 -0500 )edit

Try syncPolicy(10) or even higher- 100 or more? I wonder if the queue size for the policy needs to be big enough to hold 50 messages each for the 2 topics + 1 message from the low frequency topic to have a chance at working. (The individual queue sizes for the subscribers are irrelevant?)

lucasw gravatar image lucasw  ( 2018-03-23 12:00:27 -0500 )edit

And/or the individual subscribers need bigger queue sizes also?http://wiki.ros.org/message_filters/ApproximateTime

lucasw gravatar image lucasw  ( 2018-03-23 12:04:28 -0500 )edit

I tried out queue_size=1 for everything in rospy and it worked fine with 100:1 ratios between topic frequencies and it works fine. C++ is possibly different.

lucasw gravatar image lucasw  ( 2018-03-23 12:30:47 -0500 )edit

Thank you @lucasw, I will check this method later.

waschbaer00 gravatar image waschbaer00  ( 2018-03-24 11:21:00 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-03-24 11:31:25 -0500

waschbaer00 gravatar image

updated 2018-03-24 11:44:19 -0500

Actually my solution is to give up the synch because of lessing time to wait answer here, but the above answer from lucasw seems should work well.

My solution:

Improve the rate of topic through checking whether there is a IF ELSE statement before the msg publishing.

If there is a IF ELSE and which is the core reason to reduce the publishing rate, then publish those messages always, but with empty content if rules are not satisfied.

More details about implementation(For ROS beginner):

  1. You need to modify the src code of author's node, by IF ELSE statement to improve rate as above I explained.

  2. Look at which topic has the most lowest hz. This message will be the critical one.

  3. write callback functions individually for each topic. Store these messages, i.e. update messages as member variables of your node class.

  4. In the critical callback function: I get all other member variables(messages) to deal with whatever I want to do. i.e. You don't manipulate other messages in their callbacks, you only update them.

This solution works if you need for example imshow, since the critical's callback will be called every time when message received , if the node which publish the critical message with empty content, this will help the callback will be called more soon. This is important because this will keep other messages you subscribed could be used inside this critical callback, whenever there is an critical empty message received, you could write a IF ELSE to control your working flow.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-03-23 10:11:22 -0500

Seen: 843 times

Last updated: Mar 24 '18