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

Revision history [back]

click to hide/show revision 1
initial version

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;
}