ros2 如何在TF树中，添加1个传感器

在ROS 2中，要在TF树中添加一个传感器，你需要首先发布该传感器的数据，并且这些数据通常包含它们的位置和姿态信息。然后，你可以使用`tf2_ros`包提供的服务或节点来手动或自动地将这些传感器数据转换成TF变换并广播到TF树中。

假设你有一个名为`sensor_data`的话题，它发布了一些包含位置（position XYZ）和方向（quaternion XYZW）的传感器消息，如`sensor_msgs::msg::PoseWithCovarianceStamped`。首先，你需要创建一个节点来接收这些数据：

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>

class TFSensorPublisher : public rclcpp::Node
{
public:
    TFSensorPublisher()
        : node_(rclcpp::node::Node::make_shared("tf_sensor_publisher"))
    {
        // 创建静态变换广播者
        broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
        
        // 创建监听器订阅传感器数据
        sub_ = node_->create_subscription<sensor_msgs::msg::PoseWithCovarianceStamped>(
            "sensor_data", 10, [this](const sensor_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) -> void {
                // 在这里处理传感器数据并生成变换
                geometry_msgs::msg::TransformStamped transform;
                generateTransform(msg->pose.pose);
                broadcaster_->sendTransform(transform);
            });
    }

private:
    void generateTransform(const geometry_msgs::msg::Pose &pose)
    {
        // 根据收到的传感器数据生成transform对象
        // 假设你的传感器坐标系是/sensor_frame，相对于全局坐标系/base_link
        tf2::Transform t;
        t.setOrigin(tf2::Vector3(pose.position.x, pose.position.y, pose.position.z));
        t.setRotation(pose.orientation);

        tf2::StampedTransform stamped_t;
        stamped_t.header.frame_id = "/base_link";
        stamped_t.child_frame_id = "/sensor_frame";
        stamped_t.transform = t;

        broadcaster_->sendTransform(stamped_t);
    }

    rclcpp::Node::SharedPtr node_;
    rclcpp::Subscription<sensor_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_;
    tf2_ros::StaticTransformBroadcaster broadcaster_;
};

在这个例子中，`generateTransform`函数负责计算变换并将它广播出去。记得替换 `/sensor_data` 和 `/sensor_frame` 为你实际的传感器主题和帧名。