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

message_filter is not working for dnn_detect

asked 2020-04-14 13:00:22 -0500

cheesee61 gravatar image

updated 2020-04-14 16:55:57 -0500

Hello guys,

I wanna sync two topics with the aid of message_filters. The topics are /scan (LaserScan) and /dnn_objects (DetectedObjectArray) from the dnn_detect package, which gives me the bounding boxes, classes and confidence of the detected objects.

My code looks like follows:

import message_filters
from sensor_msgs.msg import LaserScan
from dnn_detect.msg import DetectedObjectArray
import rospy

rospy.init_node("msg_filter_testnode")

def callback(laser, dnn_obj):
    print "first line in callback"
    print "laser min range: ", laser.range_min
    print "confidence first object: ", dnn_obj.objects[0].confidence

laser_sub = message_filters.Subscriber('/scan', LaserScan)
bounding_sub = message_filters.Subscriber('/dnn_objects', DetectedObjectArray)

ts = message_filters.TimeSynchronizer([laser_sub, bounding_sub], 10)
print "after time sync"
ts.registerCallback(callback)
print "after reg callback"

rospy.spin()

My problem is, the callback function won't be invoked and that is for sure because of the /dnn_objects topic. If I use the /dnn_images topic instead, everything works fine. I also used the ApproximateTimeSynchronizer class instead of TimeSynchronizer but nothing changed. The message type of /dnn_objects is DetectedObjectArray and has a header field, which is I guess a constraint for message_filters.

I would appreciate every help guys, thanks in advance!

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2020-04-14 17:09:53 -0500

updated 2020-04-14 17:15:17 -0500

Hello,

The message type of /dnn_objects is DetectedObjectArray and has a header field, which is I guess a constraint for message_filters.

True. However, if you try to rostopic echo /dnn_objects, you will see the timestamp is not filled in.

A simplest workaround I suppose (if you installed the dnn_detect pacakge from source) is to fill in the timestamp for /dnn_objects topic in dnn_detect.cpp.

If you installed dnn_detect from a binary package, you can use topic_tools/transform to transform /dnn_objects to a new topic /dnn_object_new (let’s say) by modifying the timestamp field in header.

So it would be something like this:

rosrun topic_tools transform /dnn_objects /dnn_objects_new dnn_detect/DetectedObjectArray 'dnn_detect.msg.DetectedObjectArray(header=std_msgs.msg.Header(seq=m.header.seq,stamp=rospy.Time.now(),frame_id=m.header.frame_id),objects=m.objects)' --import dnn_detect std_msgs rospy

I’ve tried this myself and it worked for me.

Of course, you can write a new node to achieve the same thing, but I find this topic_tools quite useful once you get the hang of it.

edit flag offensive delete link more

Comments

First of all, thank you very much, your notes are valuable. I didn't know about the topic tools. I installed dnn_detect from a binary package (sudo apt install), so I ran your command. rostopic list now shows the new topic with the time stamp fields filled as opposed to the original topic, till here everything is fine. In my source code, I just changed the name of the subscribed topic to the new one, but the callback is still not invoked. @tcchiang You wrote that it worked for you, can you tell me exactly how?

I need that for my thesis and the deadline is getting closer.

Thank you very much!

cheesee61 gravatar image cheesee61  ( 2020-04-14 19:51:09 -0500 )edit

Hi, I used the ApproximateTimeSynchronizer. I think the TimeSynchronizer used the ExactTime policy which requires messages to have exactly the same timestamp. And the possibility to have exact same timestamps for messages is low if you don’t publish them by the same node. More details can be found in #q323456.

karenchiang gravatar image karenchiang  ( 2020-04-15 07:30:27 -0500 )edit

Hi @tcchiang, I solved it already with ApproximateTimeSynchronizer as you mentioned. But I still have several questions:

1) When I run a node that publishes messages1 to topic1 and a certain time later (let's say 10 seconds) I run another node that publishes messages2 to topic2 (both with the same rate/frequency (does that even matter?)), the difference of their time field should be the time difference of starting those nodes (so time field of message2 should be 10 seconds smaller than message), right? But the time field of all those nodes are billions of seconds (approx. 50 years), that means they are sync? Im a little bit confused.

2) Let's say I wanna work with two topics, topic1 receives messages with a 50ms cycle and topic2 110ms cycle. So my callback will be called if the topic with the lowest frequency/highest cycle (topic2) receives a message ...(more)

cheesee61 gravatar image cheesee61  ( 2020-04-15 18:40:44 -0500 )edit

message and the callback will be called with the latest values of that message2, but the information of message1 is 10 ms old, is that correct? And for this test scenario, the slop argument of ApproximateTimeSynchronizer should be at least 0,01 (10ms)?

cheesee61 gravatar image cheesee61  ( 2020-04-15 18:43:01 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2020-04-14 13:00:22 -0500

Seen: 176 times

Last updated: Apr 14 '20