How to use QoS in Subscriber with message_filters in ROS2 and C++?
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?