Ask Your Question
0

Using C++ message filters in ROS2

asked 2020-09-15 16:03:55 -0600

srubs_r gravatar image

I am trying to subscribe to two topics. If their header stamps are equal they should be combined into a single message and published.

I believe that an exact_time message_filter is the way to go here.

In my case the messages will be high resolution images so would like the node to be written in C++ to reduce the CPU cost.

For testing I have wrote up a Python node to to publish two messages (just sensor_msgs::msg::Temperature for this testing case) with the same header stamp. This node works with no issue.

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import Temperature


class DoublePublisher(Node):

    def __init__(self):
        super().__init__('double_publisher')

        # create publishers
        self.publisher_temp_1_ = self.create_publisher(Temperature, 'temp_1', 10)
        self.publisher_temp_2_ = self.create_publisher(Temperature, 'temp_2', 10)

        # create timer
        timer_period = 1 
        self.timer_ = self.create_timer(timer_period, self.timer_callback)

        # initialize counter
        self.i = 0

    def timer_callback(self):
        self.get_logger().info("Publishing messages")

        # get current time for headers
        msg_stamp = self._clock.now().to_msg()

        # create and publish Temperature messages
        msg_1 = Temperature()
        msg_1.header.stamp = msg_stamp
        msg_1.temperature = float(self.i)
        self.publisher_temp_1_.publish(msg_1)

        msg_2 = Temperature()
        msg_2.header.stamp = msg_stamp
        msg_2.temperature = float(self.i + 2)
        self.publisher_temp_2_.publish(msg_2)

        self.i += 1


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = DoublePublisher()

    rclpy.spin(minimal_publisher)

    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

I then tried to make a C++ node that would use message filters to subscribe to the two topics and call a callback if two messages have the same timestamp. I can't really find any solid examples on how to do this in ROS2. I tried to figure it out from the comments in the code from this repo. Below is my code and the compile errors that I get.

The code:

#include <memory>
#include <string>
#include <cstring>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/temperature.hpp"
#include "message_filters/subscriber.h"
#include "message_filters/time_synchronizer.h"

using std::placeholders::_1;
using std::placeholders::_2;

class ExactTimeSubscriber : public rclcpp::Node
{
public:
  ExactTimeSubscriber()
      : Node("exact_time_subscriber")
  {
    subscription_temp_1_.subscribe(this, "temp_1");
    subscription_temp_2_.subscribe(this, "temp_2");

    message_filters::TimeSynchronizer<sensor_msgs::msg::Temperature, sensor_msgs::msg::Temperature> sync_(subscription_temp_1_, subscription_temp_2_, 3);
    sync_.registerCallback(std::bind(&ExactTimeSubscriber::topic_callback, this, _1, _2));
  }

private:
  void topic_callback(const sensor_msgs::msg::Temperature::SharedPtr tmp_1, const sensor_msgs::msg::Temperature::SharedPtr tmp_2) const
  {
    const char *temp_1 = std::to_string(tmp_1->temperature).c_str();
    const char *temp_2 = std::to_string(tmp_2->temperature).c_str();
    RCLCPP_INFO(this->get_logger(), "I heard: '%s' and '%s'", temp_1, temp_2);
  }
  message_filters::Subscriber<sensor_msgs::msg::Temperature> subscription_temp_1_;
  message_filters::Subscriber<sensor_msgs::msg::Temperature> subscription_temp_2_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ExactTimeSubscriber>());
  rclcpp::shutdown();

  return 0;
}

Does anyone know how to use the message_filters::TimeSynchronizer (or similar) to synchronize messages in ROS2?

My system details are as follows:

  • X86 64 CPU architecture
  • Ubuntu 18.04.5
  • Eloquent

My compile error follows:

--- stderr: synchronizer                             
In file included from /opt/ros/eloquent/include/message_filters/synchronizer.h:47:0,
                 from /opt/ros/eloquent/include/message_filters/time_synchronizer.h:41,
                 from /workspaces/hfss-v3-ros2 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2020-09-15 19:18:42 -0600

Geoff gravatar image

Your callback has the wrong signature. The message filter callback must take in a reference to a const std::shared_ptr<const...>&, whereas you have just a const std::shared_ptr<> Change your callback declaration line to this:

void topic_callback(
  const sensor_msgs::msg::Temperature::ConstSharedPtr& tmp_1,
  const sensor_msgs::msg::Temperature::ConstSharedPtr& tmp_2) const

Then it will compile.

edit flag offensive delete link more

Comments

Thanks for the answer, this helped me to get started with the package and compile a similar minimal example like the one above. However, this example still does not work as expected for me in ROS2 on ubuntu. Somehow, the callback function is never reached even though I can check that the messages that reach the subscribers themselves have the exact same timestamp. This might be a problem with the message_filters package itself I fear. I posted a question about this which is awaiting moderation here:

https://answers.ros.org/question/3664...

I would be happy if you could verify that I am not a bot and make the question open for the public and maybe also help me with it by quickly reproducing the issue.

sttobia gravatar image sttobia  ( 2020-11-27 03:15:04 -0600 )edit
0

answered 2021-04-07 02:59:52 -0600

kas gravatar image

Even after changing the callback's arguments to const sensor_msgs::msg::Temperature::ConstSharedPtr&, your subscription node won't work. You declare sync_ as a local variable in ExactTimeSubscriber's constructor, but sync_ should be declared as a member variable of ExactTimeSubscriber', otherwise it goes out-of-scope.

The following works.

#include <memory>
#include <string>
#include <cstring>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/temperature.hpp"
#include "message_filters/subscriber.h"
#include "message_filters/time_synchronizer.h"

using std::placeholders::_1;
using std::placeholders::_2;

class ExactTimeSubscriber : public rclcpp::Node
{
public:
  ExactTimeSubscriber()
      : Node("exact_time_subscriber")
  {
    subscription_temp_1_.subscribe(this, "temp_1");
    subscription_temp_2_.subscribe(this, "temp_2");

    sync_ = std::make_shared<message_filters::TimeSynchronizer<sensor_msgs::msg::Temperature, sensor_msgs::msg::Temperature>>(subscription_temp_1_, subscription_temp_2_, 3);
    sync_->registerCallback(std::bind(&ExactTimeSubscriber::topic_callback, this, _1, _2));
  }

private:
  void topic_callback(const sensor_msgs::msg::Temperature::ConstSharedPtr& tmp_1, const sensor_msgs::msg::Temperature::ConstSharedPtr& tmp_2) const
  {
    const char *temp_1 = std::to_string(tmp_1->temperature).c_str();
    const char *temp_2 = std::to_string(tmp_2->temperature).c_str();
    RCLCPP_INFO(this->get_logger(), "I heard: '%s' and '%s'", temp_1, temp_2);
  }
  message_filters::Subscriber<sensor_msgs::msg::Temperature> subscription_temp_1_;
  message_filters::Subscriber<sensor_msgs::msg::Temperature> subscription_temp_2_;
  std::shared_ptr<message_filters::TimeSynchronizer<sensor_msgs::msg::Temperature, sensor_msgs::msg::Temperature>> sync_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ExactTimeSubscriber>());
  rclcpp::shutdown();

  return 0;
}
edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

4 followers

Stats

Asked: 2020-09-15 16:03:55 -0600

Seen: 864 times

Last updated: Apr 07