Ask Your Question
0

ApproximateTime policy not working

asked 2021-04-06 18:35:58 -0500

Ahmad Kheirandish gravatar image

Hello I'm trying to synchronize 2 topic(color/image and semantic_image) but unfortantelly the callback function doesn't work , here is my code ;

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

using namespace sensor_msgs;
using namespace message_filters;
void callback(const ImageConstPtr& image1, const ImageConstPtr& image2)
 {
   // Solve all of perception here...
   std::cout<<"CallBack works ...";
 }

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

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image1_sub(nh, "semantic_pcl/semantic_image", 1);
  message_filters::Subscriber<Image> image2_sub(nh, "camera/color/image_raw", 1);

   typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;

  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)

   Synchronizer<MySyncPolicy> sync(MySyncPolicy(30), image1_sub, image2_sub);
    sync.registerCallback(boost::bind(&callback, _1, _2));
    std::cout<<"Sync Image ..."<<std::endl;

    ros::spin();

    return 0;
    }

Here is more details of these 2 topics(when running $ rostopic hz & echo ...) ;


Image_raw ;

   $ rostopic echo /camera/color/image_raw
 header: 
   seq: 47205
   stamp: 
   secs: 1615561761
   nsecs: 128132582
   frame_id: "camera_color_optical_frame"
   height: 720
   width: 1280
   encoding: "rgb8"
   is_bigendian: 0
   step: 3840 
   data: [131, 146, 132, 132, 147, 133,

$ rostopic hz /camera/color/image_raw

subscribed to [/camera/color/image_raw]
   average rate: 30.187
min: 0.025s max: 0.044s std dev: 0.00285s window: 28
    average rate: 30.076
    min: 0.025s max: 0.044s std dev: 0.00226s

semantic_image :

$ rostopic echo /semantic_pcl/semantic_image

header: 
   seq: 18
   stamp: 
   secs: 0
   nsecs:         0
   frame_id: ''
   height: 480
   width: 640
   encoding: "bgr8"  
   is_bigendian: 0
   step: 1920
   data: [128, 192, 64, 128, 192,

$ rostopic hz /semantic_pcl/semantic_image

subscribed to [/semantic_pcl/semantic_image]
no new messages
no new messages
average rate: 0.386
min: 2.591s max: 2.591s std dev: 0.00000s window: 2
no new messages
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-04-07 01:06:15 -0500

gvdhoorn gravatar image

We see this in your rostopic echo output:

Image_raw ;

 $ rostopic echo /camera/color/image_raw
 header: 
   seq: 47205
   stamp: 
     secs: 1615561761
     nsecs: 128132582

and:

$ rostopic echo /semantic_pcl/semantic_image

header: 
   seq: 18
   stamp: 
     secs: 0
     nsecs:         0

Notice the values for the stamp.secs and stamp.nsecs fields.

While ApproximateTime allows some slop (ie: delta-t between messages which it will still consider to be published at the same time), with the values for header.stamp in your messages, there is a difference of about 2021 years between the timestamps of your two message streams.

Is the publisher of /semantic_pcl/semantic_image correctly populating the header of the messages it's publishing?

If it isn't, you need to fix that (either copy the stamp of an incoming message it's processing, or set it to some other meaningful value).

edit flag offensive delete link more

Comments

So, what was the cause?

gvdhoorn gravatar image gvdhoorn  ( 2021-04-07 07:08:40 -0500 )edit

Thanks for your help , as you said " stamp.secs and stamp.nsecs fields " were empty and by fixing that issue with the other color_image topic that has complete header field half of the problem solved.But I also should use the python version of the ApproximateTime policy and add more "slop" value .

Ahmad Kheirandish gravatar image Ahmad Kheirandish  ( 2021-04-09 11:06:18 -0500 )edit

But I also should use the python version of the ApproximateTime policy

Could you explain why?

gvdhoorn gravatar image gvdhoorn  ( 2021-04-09 11:19:47 -0500 )edit

You seem to have removed your previous post, in which you wrote:

Because it seems that there is no "delay" attribution for " ApproximateTime Policy in c++"

Aren't methods like setMaxIntervalDuration(..) on the policy the C++ equivalent?

gvdhoorn gravatar image gvdhoorn  ( 2021-04-09 11:48:51 -0500 )edit

Because it seems that for ApproximateTime Policy "ApproximateTimeSynchronizer in c++" with refer to this page; http://wiki.ros.org/message_filters doesn't have delay attribution like the one in python which is "slop" , so I used the python one.

Here is my code;

 #!/usr/bin/env python

import rospy
 from sensor_msgs.msg import Image
 import message_filters 

 rospy.init_node('myimage')
 rate = rospy.Rate(30) # 10hz


 def got_image(sem_image,image_raw):
     sync_sem=sem_image
     print "Got an Semantic_Image and Image_raw"
     pub.publish(sync_sem)

 pub = rospy.Publisher('sync_semantic_image', Image, queue_size=10)
 sem_image_sub = message_filters.Subscriber('/semantic_pcl/semantic_image', Image  )   
raw_camera_sub = message_filters.Subscriber('/camera/color/image_raw', Image )

 ats = message_filters.ApproximateTimeSynchronizer([sem_image_sub, raw_camera_sub], queue_size=1 , slop= 8.0)

 ats.registerCallback(
Ahmad Kheirandish gravatar image Ahmad Kheirandish  ( 2021-04-09 11:50:33 -0500 )edit

Yes , you are right , I didn't notice this that somehow setMaxIntervalDuration(..) is equivalent to slop in python version , in page; http://wiki.ros.org/message_filters/ApproximateTime it indicates that.

Ahmad Kheirandish gravatar image Ahmad Kheirandish  ( 2021-04-09 12:00:07 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-04-06 18:35:58 -0500

Seen: 21 times

Last updated: Apr 07