How to use QoS in Subscriber with message_filters in ROS2 and C++?

asked 2023-07-28 03:41:37 -0500

Astronaut gravatar image

updated 2023-07-28 06:38:45 -0500

Hi I make a use of message-filters to merge 3 ROS2 topic into one and publish it. Im using ROS2 and humble and C++. So in the ROS2 need to declare QoS when subscribe to the topics. Here the code that works but without QoS.

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

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "geometry_msgs/msg/vector3.hpp"
#include "message_filters/subscriber.h"
#include "message_filters/time_synchronizer.h"
#include "geometry_msgs/msg/quaternion_stamped.hpp"
#include "geometry_msgs/msg/vector3_stamped.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rmw/types.h"
#include "rclcpp/qos.hpp"

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


class ExactTimeSubscriber : public rclcpp::Node
{
public:
  ExactTimeSubscriber()
      : Node("exact_time_subscriber")
  {


     // Create publishers for the merged IMU topic`
    imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/robot/imu", 10);

    subscription_imu_1_.subscribe(this, "/robot/attitude",);
    subscription_imu_2_.subscribe(this, "/robot/angular_velocity");
    subscription_imu_3_.subscribe(this, "/robot/linear_acceleration");

    sync_ = std::make_shared<message_filters::TimeSynchronizer<geometry_msgs::msg::QuaternionStamped, geometry_msgs::msg::Vector3Stamped, geometry_msgs::msg::Vector3Stamped>>(subscription_imu_1_, subscription_imu_2_, subscription_imu_3_, 3);
    sync_->registerCallback(std::bind(&ExactTimeSubscriber::topic_callback, this,  _1, _2, _3));

  }


private:  
  void topic_callback(const geometry_msgs::msg::QuaternionStamped::ConstSharedPtr& msg1, const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr& msg2, const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr& msg3) const
  {
    // Create the IMU message and populate its fields
    sensor_msgs::msg::Imu imu_msg;
    imu_msg.header = msg1->header;
    imu_msg.orientation = msg1->quaternion;
    imu_msg.linear_acceleration = msg2->vector;
    imu_msg.angular_velocity = msg3->vector;

    // Publish the merged IMU message
    RCLCPP_INFO(this->get_logger(), "Mono_Inertial node started");
    imu_pub_->publish(imu_msg);

  }

  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
  message_filters::Subscriber<geometry_msgs::msg::QuaternionStamped> subscription_imu_1_;
  message_filters::Subscriber<geometry_msgs::msg::Vector3Stamped> subscription_imu_2_;
  message_filters::Subscriber<geometry_msgs::msg::Vector3Stamped> subscription_imu_3_;

  std::shared_ptr<message_filters::TimeSynchronizer<geometry_msgs::msg::QuaternionStamped  , geometry_msgs::msg::Vector3Stamped, geometry_msgs::msg::Vector3Stamped>> sync_;

};

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

  return 0;
}

Im not familiar with QoS. So not sure how to declare when subscribe to ROS2 topic. So my question is how to use it the QoS in this code?

edit retag flag offensive close merge delete