How to merge 3 ROS2 topics, namely attitude, angular velocity and linear acceleration into one IMU topic?

asked 2023-07-27 01:09:45 -0500

Astronaut gravatar image

updated 2023-07-27 02:48:52 -0500

Hi

I have 3 topics that been published in ROS2 humble. Namely my robot publish these 3 topics: attitude, linear acceleration and angular velocity.

/robot/attitude which is type of geometry_msgs/msg/QuaternionStamped

/robot/linear_acceleration which is type of geometry_msgs/msg/Vector3Stamped

/robot/angular_velocity which is type of geometry_msgs/msg/Vector3Stamped

I would like to merge all of them into only one IMU topic. Is the ROS2 message filter the best solution for that? I done a code so not sure if that is ok. Here the code in C++

#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"

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

class ExactTimeSubscriber : public rclcpp::Node
{
public:
  ExactTimeSubscriber()
      : Node("exact_time_subscriber")
  {
    subscription_imu_1_.subscribe(this, "imu_1");
    subscription_imu_2_.subscribe(this, "imu_2");
    subscription_imu_3_.subscribe(this, "imu_3");

    sync_ = std::make_shared<message_filters::TimeSynchronizer<geometry_msgs::msg::Quaternion, geometry_msgs::msg::Vector3, geometry_msgs::msg::Vector3>>(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::Quaternion::ConstSharedPtr& imu_1, const geometry_msgs::msg::Vector3::ConstSharedPtr& imu_2, const geometry_msgs::msg::Vector3::ConstSharedPtr& imu_3) const
  {
    const char *imu_1 = std::to_string(imu_1->imu).c_str();
    const char *imu_2 = std::to_string(imu_2->imu).c_str();
    const char *imu_3 = std::to_string(imu_3->imu).c_str();

    RCLCPP_INFO(this->get_logger(), "I heard: '%s' and '%s'", imu_1, imu_2, imu_3);
  }
  message_filters::Subscriber<geometry_msgs::msg::Quaternion> subscription_imu_1_;
  message_filters::Subscriber<geometry_msgs::msg::Vector3> subscription_imu_2_;
  message_filters::Subscriber<geometry_msgs::msg::Vector3> subscription_imu_3_;
  std::shared_ptr<message_filters::TimeSynchronizer<geometry_msgs::msg::Quaternion, geometry_msgs::msg::Vector3, geometry_msgs::msg::Vector3>> sync_;
};

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

  return 0;
}

So is this ok? So is this correct implementation? Means I have IMU subscriber to those 3 topics?

edit retag flag offensive close merge delete