ros c++ 订阅tf话题并转换成pose

在ROS中，使用TF库来处理坐标系变换。首先，在你的ROS包中创建一个订阅器（Subscriber）来订阅TF话题，并在回调函数中将TF消息转换为Pose消息。

以下是一个示例代码：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>

void tfCallback(const tf::tfMessage::ConstPtr& tfMsg)
{
    // 创建一个TransformListener对象
    tf::TransformListener listener;

    // 获取两个坐标系之间的变换
    tf::StampedTransform transform;
    try{
        listener.lookupTransform("map", "base_link", ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
        return;
    }

    // 将变换转换为Pose消息
    geometry_msgs::PoseStamped poseMsg;
    poseMsg.header.stamp = ros::Time::now();
    poseMsg.header.frame_id = "map";
    poseMsg.pose.position.x = transform.getOrigin().x();
    poseMsg.pose.position.y = transform.getOrigin().y();
    poseMsg.pose.position.z = transform.getOrigin().z();
    poseMsg.pose.orientation.x = transform.getRotation().x();
    poseMsg.pose.orientation.y = transform.getRotation().y();
    poseMsg.pose.orientation.z = transform.getRotation().z();
    poseMsg.pose.orientation.w = transform.getRotation().w();

    // 发布Pose消息
    // ...

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tf_to_pose");
    ros::NodeHandle nh;

    // 创建一个订阅器来订阅TF话题
    ros::Subscriber sub = nh.subscribe("/tf", 10, tfCallback);

    // 创建一个发布器来发布Pose消息
    // ...

    ros::spin();

    return 0;
}

在回调函数中，我们首先创建了一个TransformListener对象，然后调用`lookupTransform()`方法获取两个坐标系之间的变换。注意，`lookupTransform()`方法的第一个参数是目标坐标系，第二个参数是源坐标系。在这个例子中，我们将获取从`base_link`坐标系到`map`坐标系的变换。

一旦我们获取了变换，我们就可以将其转换为Pose消息，并发布出去。