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

ROS 2 message_filters TimeSynchronizer minimal example does not reach callback function

asked 2020-11-26 10:34:14 -0500

Description

I am trying to get the message_filters::TimeSynchronizer to work on my Linux Ubuntu 18.04 (Linux version 5.4.0-54-generic) and wrote a minimal node, according to other entries in this forum and the ros2 tutorials. This node simply publishes two Temperature messages with the exact same timestamp and then subscribes to these topics to an instance of message_filters::TimeSynchronizer. Unfortunately, the Synchronization does not work as expected and at the same time does not output any Warnings, Errors or similar, that would help debugging. The issue is the following:

When testing the plain message_filters::Subscriber instances for the two topics, I could verify, that the messages actually do come in and have the exact same timestamp. But still after adding them to the TimeSynchonizer instance, the according callback function is never called somehow. I also tested it with a Policy (ApproximateTime), but also there, the callback was never called. I even made sure, after reading through similar issues in here, to use the same QoS in both the publishers and subscribers without any success...

Platform: Tested both in Ubuntu 20.04 with Foxy, as well as in Ubuntu 18.04 with Eloquent with the same outcome.

Code: The test node I wrote for this to reproduce the error looks the following:

Package XML

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema" ?>
<package format="3">
  <name>temp_sync</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="tobias@mantistechnologies.ch">tobias</maintainer>
  <license>TODO: License declaration</license>

  <depend>sensor_msgs</depend>
  <depend>message_filters</depend>
  <depend>rclcpp</depend>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

CmakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(temp_sync)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(message_filters REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

add_executable(syncer src/temp_sync.cpp)
ament_target_dependencies(syncer rclcpp sensor_msgs message_filters)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

install(TARGETS syncer
  EXPORT export_${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME})

ament_package()

Cpp Node

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

#include <chrono>
#include <rclcpp/rclcpp.hpp>

#include "sensor_msgs/msg/temperature.hpp"

using namespace std::chrono_literals;

class SyncerNode : public rclcpp::Node {
 public:
  SyncerNode() : Node("syncer") {
    rclcpp::QoS qos(10);
    auto rmw_qos_profile = qos.get_rmw_qos_profile();

    publisher_temp1_ =
        this->create_publisher<sensor_msgs::msg::Temperature>("temp_1", qos);
    publisher_temp2_ =
        this->create_publisher<sensor_msgs::msg::Temperature>("temp_2", qos);

    timer_ = this->create_wall_timer(
        500ms, std::bind(&SyncerNode::TimerCallback, this));

    subscriber_temp1_.subscribe(this, "temp_1 ...
(more)
edit retag flag offensive close merge delete

Comments

UPDATE The issue still persists, but I just implemented and tested the same nodes in python, where the synchronization seems to work flawlessly. The synced callback function was being reached succesfully, leaving me baffled with why the c++ counterpart just won't work, since I would expect the python part to be just a wrapper of the c++ package... Is anyone able to recreate the issue with the c++ synchronizer? I would appreciate any help!

Both the code for c++ and python nodes can be found on my git: https://github.com/sttobia/ros2_eloqu... Just build directly in the repo using colcon build and then run the talker and listener nodes of both packages to test and reproduce the issue...

sttobia gravatar image sttobia  ( 2021-01-26 12:06:29 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
4

answered 2021-04-07 03:11:12 -0500

kas gravatar image

Came here from your comment on a related question.

You have the same issue as I mentioned in this answer to the same question.

Declare the local variable temp_sync as a member variable of SyncerNode, i.e.

#include <chrono>
#include <memory>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>    
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/temperature.hpp>

using namespace std::chrono_literals;

class SyncerNode : public rclcpp::Node {
 public:
  SyncerNode() : Node("syncer") {
    rclcpp::QoS qos(10);
    auto rmw_qos_profile = qos.get_rmw_qos_profile();

    publisher_temp1_ =
        this->create_publisher<sensor_msgs::msg::Temperature>("temp_1", qos);
    publisher_temp2_ =
        this->create_publisher<sensor_msgs::msg::Temperature>("temp_2", qos);

    timer_ = this->create_wall_timer(
        500ms, std::bind(&SyncerNode::TimerCallback, this));

    subscriber_temp1_.subscribe(this, "temp_1", rmw_qos_profile);
    subscriber_temp2_.subscribe(this, "temp_2", rmw_qos_profile);

    // // Uncomment this to verify that the messages indeed reach the
    // subscriber_temp1_.registerCallback(
    //     std::bind(&SyncerNode::Tmp1Callback, this, std::placeholders::_1));
    // subscriber_temp2_.registerCallback(
    //     std::bind(&SyncerNode::Tmp2Callback, this, std::placeholders::_1));

    temp_sync_ = std::make_shared<message_filters::TimeSynchronizer<sensor_msgs::msg::Temperature, sensor_msgs::msg::Temperature>>(subscriber_temp1_, subscriber_temp2_, 10);
    temp_sync_->registerCallback(std::bind(&SyncerNode::TempSyncCallback, this, std::placeholders::_1, std::placeholders::_2));
  }

 private:
  void TimerCallback() {
    rclcpp::Time now = this->get_clock()->now();

    auto msg_tmp1 = sensor_msgs::msg::Temperature();
    msg_tmp1.header.stamp = now;
    msg_tmp1.header.frame_id = "test";
    msg_tmp1.temperature = 1.0;

    auto msg_tmp2 = sensor_msgs::msg::Temperature();
    msg_tmp2.header.stamp = now;
    msg_tmp2.header.frame_id = "test";
    msg_tmp2.temperature = 2.0;

    publisher_temp1_->publish(msg_tmp1);
    publisher_temp2_->publish(msg_tmp2);

    RCLCPP_INFO(this->get_logger(), "Published two temperatures.");
  }

  // For veryfing the single subscriber instances: Uncomment line 26-28.
  void Tmp1Callback(const sensor_msgs::msg::Temperature::ConstSharedPtr& msg) {
    RCLCPP_INFO(this->get_logger(), "Frame '%s', temp %f with ts %u.%u sec ",
                msg->header.frame_id.c_str(), msg->temperature,
                msg->header.stamp.sec, msg->header.stamp.nanosec);
  }

  // For veryfing the single subscriber instances: Uncomment line 29-31.
  void Tmp2Callback(const sensor_msgs::msg::Temperature::ConstSharedPtr& msg) {
    RCLCPP_INFO(this->get_logger(), "Frame '%s', temp %f with ts %u.%u sec ",
                msg->header.frame_id.c_str(), msg->temperature,
                msg->header.stamp.sec, msg->header.stamp.nanosec);
  }

  // This callback is never being called.
  void TempSyncCallback(
      const sensor_msgs::msg::Temperature::ConstSharedPtr& msg_1,
      const sensor_msgs::msg::Temperature::ConstSharedPtr& msg_2) {
    RCLCPP_INFO(this->get_logger(),
                "I heard and synchronized the following timestamps: %u, %u",
                msg_1->header.stamp.sec, msg_2->header.stamp.sec);
  }

  rclcpp::Publisher<sensor_msgs::msg::Temperature>::SharedPtr publisher_temp1_;
  rclcpp::Publisher<sensor_msgs::msg::Temperature>::SharedPtr publisher_temp2_;
  message_filters::Subscriber<sensor_msgs::msg::Temperature> subscriber_temp1_;
  message_filters::Subscriber<sensor_msgs::msg::Temperature> subscriber_temp2_;
  std::shared_ptr<message_filters::TimeSynchronizer<sensor_msgs::msg::Temperature, sensor_msgs::msg::Temperature>> temp_sync_;

  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char* argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<SyncerNode>());
  rclcpp::shutdown();
  return 0;
}
edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2020-11-26 09:35:28 -0500

Seen: 4,317 times

Last updated: Apr 07 '21