Robotics StackExchange | Archived questions

ROS2 message filter approximate time policy callback doesn't run

I am trying to sync two message streams of 2 image topics. So I am using message filters. I was using a class to do all this, however, the callback doesnt seem to be running. I have echoed the rostopics in the terminal and they seem to be outputting the messages. What could be happening here?

What is allowed time delay in the approximate policy in receiving the two topic streams(I atleast have topics matching every second right now)?

I guess I may be doing something wrong there. (Things do work if i create two different subscribers with their own callbacks)

This is my code-

   class MultiSubscriber : public rclcpp::Node
    {
      public:
      message_filters::Subscriber<sensor_msgs::msg::Image> image_sub_;
          message_filters::Subscriber<sensor_msgs::msg::Image> disparity_sub_;              
        MultiSubscriber(const std::string& name)
        : Node(name)
        {
          image_sub_.subscribe(this, "/image_color");
          disparity_sub_.subscribe(this, "/image_disparity");
          typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> approximate_policy;
          message_filters::Synchronizer<approximate_policy>syncApproximate(approximate_policy(10), image_sub_, disparity_sub_);
             syncApproximate.setMaxIntervalDuration(rclcpp::Duration(3,0)); // Added after a comment
          syncApproximate.registerCallback(&MultiSubscriber::disparityCb,this);

        }
        private:
        void disparityCb(const sensor_msgs::msg::Image::SharedPtr disparity_msg, const sensor_msgs::msg::Image::SharedPtr color_msg)
        {

            std::cout<<"Hello messages are being received";

        }};

Just to add to this. This is the output of -

ros2 node info /obstacle_detection 

/obstacle_detection
  Subscribers:
    /image_disparity: sensor_msgs/msg/Image
    /image_color: sensor_msgs/msg/Image
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
  Service Servers:
    /obstacle_detection/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /obstacle_detection/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /obstacle_detection/get_parameters: rcl_interfaces/srv/GetParameters
    /obstacle_detection/list_parameters: rcl_interfaces/srv/ListParameters
    /obstacle_detection/set_parameters: rcl_interfaces/srv/SetParameters
    /obstacle_detection/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

Edit- Error Message on changing the registerCallback call to syncApproximate.registerCallback(std::bind (&MultiSubscriber::disparityCb, this,_1,_2)); image description

Edit 2- This github issue shows how the registercallback is to be called.

Asked by Harsh2308 on 2020-08-16 12:10:31 UTC

Comments

I am assuming that the headers for each image message has been populated correctly. Try outputting the stamps msg->header.stamp.sec and msg->header.stamp.nsec from each message to debug if the times are syncing.

Asked by surfertas on 2020-08-16 17:31:45 UTC

Also try using ros2 topic info <TOPIC> for each topic, and confirm that you observe expected values for Publisher count and Subscriber count.

Asked by surfertas on 2020-08-16 17:38:21 UTC

Answers

First, check if the signature of the callback is correct. (e.g. void callback(const std::shared_ptr<M const>&, const std::shared_ptr<M const>&) for 2 filters). If yes, proceed below.

  1. Check if the Quality of Service (QoS) is compatible between the subscriber and publisher. You can specify a QoS by passing a rmw quality of service profile.

  2. Use ros2 topic info <TOPIC> to confirm the expected number of publisher count and subscriber count.

If you are still having issues, you might be facing time stamps that doesn't meet the duration threshold.

  1. Output the time stamps to visually compare.

  2. Adjust the duration threshold by calling the method setMaxIntervalDuration() that takes as input a rclcpp::Duration() representation of time. (e.g. sync_policy.setMaxIntervalDuration(rclcpp::Duration(1,0)))

Asked by surfertas on 2020-08-17 00:22:39 UTC

Comments

Thanks for such an informative answer.

  1. ros2 topic info results seem to be correct. (When I start the node, the topics' subscriber count does increase.)

  2. Output time stamps visually I see both images coming in at least a second.

  3. I used this function and set the duration to be 3 seconds (syncApproximate.setMaxIntervalDuration(rclcpp::Duration(3,0));)

I don't know how to check the compatibility between the publisher and subscriber . Btw I am using a bagfile (which publishes the messages, I am trying to subscribe to )

Asked by Harsh2308 on 2020-08-20 02:01:18 UTC

Ok assuming everything compiles, seems like there might be QoS compatibility issue. Its recommended to use rmw_qos_profile_sensor_data with image topics, so it's possible that the creator of the bag file has used the sensor data profile.

Can you pass the QoS profile to the call to subscribe()?

.subscribe(this, "/some_topic", rmw_qos_profile_sensor_data);

Asked by surfertas on 2020-08-20 07:32:38 UTC

Also as @KenYN has mentioned, my signature is consistent with his suggestion. Did you try that as well?

Asked by surfertas on 2020-08-20 07:36:05 UTC

  1. Modified to this but no change.

image_sub_.subscribe(this, "/image_color",rmw_qos_profile_sensor_data) disparity_sub_.subscribe(this, "/image_disparity", rmw_qos_profile_sensor_data);

2. syncApproximate.registerCallback(std::bind (&MultiSubscriber::disparityCb, this,_1,_2_)); is giving a compilation error. My callback signature is same as in the answer.

There is a huge error

Asked by Harsh2308 on 2020-08-20 09:03:29 UTC

error: no matching function for call to ‘message_filters::Signal9 >, sensor_msgs::msg::Image_std::allocator<void >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType>::addCallback(std::Bind_helper, std::_Placeholder<2>))(std::shared_ptr<sensor_msgs::msg::Imagestd::allocator<void > >, std::shared_ptr > >)>&, const std::Placeholder<1>&, const std::_Placeholder<2>&, const std::_Placeholder<3>&, const std::_Placeholder<4>&, const std::_Placeholder<5>&, const std::_Placeholder<6>&, const std::

Asked by Harsh2308 on 2020-08-20 09:22:24 UTC

The callback needs to be of signature void callback(const std::shared_ptr<M const>&) as detailed here

Change this void disparityCb(const sensor_msgs::msg::Image::SharedPtr disparity_msg, const sensor_msgs::msg::Image::SharedPtr color_msg)

to

void disparityCb(const sensor_msgs::ImageConstPtr disparity_msg, const sensor_msgs::ImageConstPtr & color_msg)

Asked by surfertas on 2020-08-20 09:26:27 UTC

I will leave this stack overflow discussion on differences for reference.

Asked by surfertas on 2020-08-20 09:31:46 UTC

I have added the relevant part of the error message in the answer. I had to change the callback to -

void MultiSenseSubscriber::disparityCb(const boost::shared_ptr<sensor_msgs::msg::Image const>& disparity_msg, const boost::shared_ptr<sensor_msgs::msg::Image const> & color_msg)

Asked by Harsh2308 on 2020-08-20 10:22:22 UTC

So this issue shows how the registercallback is to be called. I believe my signature is correct

Asked by Harsh2308 on 2020-08-20 11:07:50 UTC

Your callback signature two comments above uses boost - you should be using std, or even better the ::ConstSharedPtr typedef.

Asked by KenYN on 2020-08-20 23:55:52 UTC

Were you able to make it run? I had similar issues but with the python API. It was solved by ensuring the subscriber had the same qos_profile with the publisher, in my case rclcpp::SensorDataQoS

Asked by charlie92 on 2020-12-02 17:21:06 UTC

I think your registerCallback() looks odd; this is what I use on Foxy:

  syncApproximate.registerCallback(std::bind(&MultiSubscriber::disparityCb,
                                             this,
                                             std::placeholders::_1,
                                             std::placeholders::_2));

Also, my callback uses ConstSharedPtr, but the compiler should catch a mismatch.

Asked by KenYN on 2020-08-17 01:13:02 UTC

Comments

Could you share how your callback looks like?

Asked by Harsh2308 on 2020-08-20 01:24:53 UTC

I looked at this issue for the way the registerCallback is to be called

Asked by Harsh2308 on 2020-08-20 11:04:57 UTC

Hi,

I had the same issue as you and it is fixed when I define synchronizer as a static variable to keep it out of the constructor function.

If you still need it this is the change for you, I did similarly in my code.

static message_filters::Synchronizer<approximate_policy>syncApproximate(approximate_policy(10), image_sub_, disparity_sub_);

It works and it calls the callback function. However, I have the termination error because I define a variable as static in the function. I am looking solution for it.

I can use the time synchronizer and the application prints errors when I terminate it.

Asked by tgulbudak on 2023-02-22 05:09:30 UTC

Comments