TF坐标变换

news2024/9/8 23:55:19

ROS小乌龟跟随

5.1 TF坐标变换 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

tf模块：在 ROS 中用于实现不同坐标系之间的点或向量的转换。

在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:

tf2_geometry_msgs:可以将ROS消息转换成tf2消息。

tf2: 封装了坐标变换的常用消息。

tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

坐标系：ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。

1.坐标msg消息

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped 和 geometry_msgs/PointStamped

前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

2.静态坐标变换

所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。

创建项目功能包依赖于tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs

注意子级与父级坐标系的关系以及坐标转换函数transform()使用的方法。

3.动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

创建项目功能包依赖于 tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim

订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度。

ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,callback);


void callback(const turtlesim::Pose::ConstPtr& pose){
    
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
}

4.多坐标变换

现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标。

创建项目功能包依赖于 tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim

使用静态坐标变换发布son1 son2 分别相对于world的坐标关系

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>

解析 son1 中的点相对于 son2 的坐标

geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
         tfs.transform.translation.x,
         tfs.transform.translation.y,
         fs.transform.translation.z
         );

然后解析坐标，求出该点在 son2 中的坐标。

geometry_msgs::PointStamped ps;
ps.header.frame_id = "son1";
ps.header.stamp = ros::Time::now();
ps.point.x = 1.0;
ps.point.y = 2.0;
ps.point.z = 3.0;

geometry_msgs::PointStamped psAtSon2;
psAtSon2 = buffer.transform(ps,"son2");
ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
         psAtSon2.point.x,
         psAtSon2.point.y,
         psAtSon2.point.z
         );