我有 3 个主题已在 ROS2 humility 中发布。即我的机器人发布了这3个主题:姿态、线加速度和角速度。
/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
我想将它们全部合并到一个 IMU 主题中。 ROS2 消息过滤器是最佳解决方案吗?我做了一个代码,所以不确定这是否可以。这是 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;
}
那么这是正确的实现吗?意味着我有这 3 个主题的 IMU 订阅者?