【ROS学习笔记13】ROS中的TF坐标变换

文章目录

• 【ROS学习笔记13】ROS中的TF坐标变换
• • 前言
  • 1. 静态坐标变换
  • 2. 动态坐标变换
  • 3. 多坐标变换
  • 4. 坐标系关系查看
  • 5. TF坐标变换实操
  • Reference

写在前面，本系列笔记参考的是AutoLabor的教程，具体项目地址在 这里

前言

在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率，本章主要介绍ROS中内置的如下组件:

• TF坐标变换，实现不同类型的坐标系之间的转换；
• rosbag 用于录制ROS节点的执行过程并可以重放该过程；
• rqt 工具箱，集成了多款图形化的调试工具。

本章预期达成的学习目标:

• 了解 TF 坐标变换的概念以及应用场景；
• 能够独立完成TF案例:小乌龟跟随；
• 可以使用 rosbag 命令或编码的形式实现录制与回放；
• 能够熟练使用rqt中的图形化工具。

案例演示: 小乌龟跟随实现，该案例是ros中内置案例，终端下键入启动命令

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch

roslaunch turtle_tf2 turtle_tf2_demo.launch

键盘可以控制一只乌龟运动，另一只跟随运动。

示例的结果：

1. 静态坐标变换

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

需求描述:

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

结果演示:

实现分析:

1. 坐标系相对关系，可以通过发布方发布
2. 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出

**实现流程:**C++ 与 Python 实现流程一致

1. 新建功能包，添加依赖
2. 编写发布方实现
3. 编写订阅方实现
4. 执行并查看结果

方案A:C++实现

1.创建功能包

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

2.发布方

/* 静态坐标变换发布方:发布关于 laser 坐标系的位置信息 实现流程:1.包含头文件2.初始化 ROS 节点3.创建静态坐标转换广播器4.创建坐标系信息5.广播器发布坐标系信息6.spin()
*/// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ROS 节点ros::init(argc,argv,"static_brocast");// 3.创建静态坐标转换广播器tf2_ros::StaticTransformBroadcaster broadcaster;// 4.创建坐标系信息geometry_msgs::TransformStamped ts;//----设置头信息ts.header.seq = 100;ts.header.stamp = ros::Time::now();ts.header.frame_id = "base_link";//----设置子级坐标系ts.child_frame_id = "laser";//----设置子级相对于父级的偏移量ts.transform.translation.x = 0.2;ts.transform.translation.y = 0.0;ts.transform.translation.z = 0.5;//----设置四元数:将 欧拉角数据转换成四元数tf2::Quaternion qtn;qtn.setRPY(0,0,0);ts.transform.rotation.x = qtn.getX();ts.transform.rotation.y = qtn.getY();ts.transform.rotation.z = qtn.getZ();ts.transform.rotation.w = qtn.getW();// 5.广播器发布坐标系信息broadcaster.sendTransform(ts);ros::spin();return 0;
}

配置文件此处略。

示例效果：

使用

rviz

启动图形化界面，然后可以查看结果：

3.订阅方

/*  订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点实现流程:1.包含头文件2.初始化 ROS 节点3.创建 TF 订阅节点4.生成一个坐标点(相对于子级坐标系)5.转换坐标点(相对于父级坐标系)6.spin()
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ROS 节点ros::init(argc,argv,"tf_sub");ros::NodeHandle nh;// 3.创建 TF 订阅节点tf2_ros::Buffer buffer;tf2_ros::TransformListener listener(buffer);ros::Rate r(1);while (ros::ok()){// 4.生成一个坐标点(相对于子级坐标系)geometry_msgs::PointStamped point_laser;point_laser.header.frame_id = "laser";point_laser.header.stamp = ros::Time::now();point_laser.point.x = 2.0;point_laser.point.y = 3.0;point_laser.point.y = 5.0;// 5.转换坐标点(相对于父级坐标系)//新建一个坐标点，用于接收转换结果  //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------try{geometry_msgs::PointStamped point_base;point_base = buffer.transform(point_laser,"base_link");ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:%s",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());}catch(const std::exception& e){// std::cerr << e.what() << '\n';ROS_INFO("程序异常.....");}r.sleep();  ros::spinOnce();}return 0;
}

配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，控制台将输出，坐标转换后的结果。

执行的结果：

方案B:Python实现

1.创建功能包

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

2.发布方

#! /usr/bin/env python
"""  静态坐标变换发布方:发布关于 laser 坐标系的位置信息 实现流程:1.导包2.初始化 ROS 节点3.创建 静态坐标广播器4.创建并组织被广播的消息5.广播器发送消息6.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStampedif __name__ == "__main__":# 2.初始化 ROS 节点rospy.init_node("static_tf_pub_p")# 3.创建 静态坐标广播器broadcaster = tf2_ros.StaticTransformBroadcaster()# 4.创建并组织被广播的消息tfs = TransformStamped()# --- 头信息tfs.header.frame_id = "world"tfs.header.stamp = rospy.Time.now()tfs.header.seq = 101# --- 子坐标系tfs.child_frame_id = "radar"# --- 坐标系相对信息# ------ 偏移量tfs.transform.translation.x = 0.2tfs.transform.translation.y = 0.0tfs.transform.translation.z = 0.5# ------ 四元数qtn = tf.transformations.quaternion_from_euler(0,0,0)tfs.transform.rotation.x = qtn[0]tfs.transform.rotation.y = qtn[1]tfs.transform.rotation.z = qtn[2]tfs.transform.rotation.w = qtn[3]# 5.广播器发送消息broadcaster.sendTransform(tfs)# 6.spinrospy.spin()

权限设置以及配置文件此处略。

示例结果：

3.订阅方

#! /usr/bin/env python
"""  订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点实现流程:1.导包2.初始化 ROS 节点3.创建 TF 订阅对象4.创建一个 radar 坐标系中的坐标点5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标6.spin"""
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStampedif __name__ == "__main__":# 2.初始化 ROS 节点rospy.init_node("static_sub_tf_p")# 3.创建 TF 订阅对象buffer = tf2_ros.Buffer()listener = tf2_ros.TransformListener(buffer)rate = rospy.Rate(1)while not rospy.is_shutdown():    # 4.创建一个 radar 坐标系中的坐标点point_source = PointStamped()point_source.header.frame_id = "radar"point_source.header.stamp = rospy.Time.now()point_source.point.x = 10point_source.point.y = 2point_source.point.z = 3try:#     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标point_target = buffer.transform(point_source,"world")rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",point_target.point.x,point_target.point.y,point_target.point.z)except Exception as e:rospy.logerr("异常:%s",e)#     6.spinrate.sleep()

权限设置以及配置文件此处略。

PS: 在 tf2 的 python 实现中，tf2 已经封装了一些消息类型，不可以使用 geometry_msgs.msg 中的类型

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，控制台将输出，坐标转换后的结果。

示例的结果:

补充1:

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser

也建议使用该种方式直接实现静态坐标系相对信息发布。

补充2:

可以借助于rviz显示坐标系关系，具体操作:

• 新建窗口输入命令:rviz;
• 在启动的 rviz 中设置Fixed Frame 为 base_link;
• 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系。

2. 动态坐标变换

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

需求描述:

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

结果演示:

实现分析:

1. 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2. 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3. 将 pose 信息转换成 坐标系相对信息并发布

实现流程: C++ 与 Python 实现流程一致

1. 新建功能包，添加依赖
2. 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
3. 创建坐标相对关系订阅方
4. 执行

方案A:C++实现

1.创建功能包

创建项目功能包依赖于

 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

/*  动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布实现分析:1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度3.将 pose 信息转换成 坐标系相对信息并发布实现流程:1.包含头文件2.初始化 ROS 节点3.创建 ROS 句柄4.创建订阅对象5.回调函数处理订阅到的数据(实现TF广播)5-1.创建 TF 广播器5-2.创建 广播的数据(通过 pose 设置)5-3.广播器发布数据6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"void doPose(const turtlesim::Pose::ConstPtr& pose){//  5-1.创建 TF 广播器static tf2_ros::TransformBroadcaster broadcaster;//  5-2.创建 广播的数据(通过 pose 设置)geometry_msgs::TransformStamped tfs;//  |----头设置tfs.header.frame_id = "world";tfs.header.stamp = ros::Time::now();//  |----坐标系 IDtfs.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//  |--------- 四元数设置tf2::Quaternion qtn;qtn.setRPY(0,0,pose->theta);tfs.transform.rotation.x = qtn.getX();tfs.transform.rotation.y = qtn.getY();tfs.transform.rotation.z = qtn.getZ();tfs.transform.rotation.w = qtn.getW();//  5-3.广播器发布数据broadcaster.sendTransform(tfs);
}int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ROS 节点ros::init(argc,argv,"dynamic_tf_pub");// 3.创建 ROS 句柄ros::NodeHandle nh;// 4.创建订阅对象ros::Subscriber sub = nh.subscribe("/turtle1/pose",1000,doPose);//     5.回调函数处理订阅到的数据(实现TF广播)//        // 6.spinros::spin();return 0;
}

实现效果：

配置文件此处略。

3.订阅方

//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ROS 节点ros::init(argc,argv,"dynamic_tf_sub");ros::NodeHandle nh;// 3.创建 TF 订阅节点tf2_ros::Buffer buffer;tf2_ros::TransformListener listener(buffer);ros::Rate r(1);while (ros::ok()){// 4.生成一个坐标点(相对于子级坐标系)geometry_msgs::PointStamped point_laser;point_laser.header.frame_id = "turtle1";point_laser.header.stamp = ros::Time();point_laser.point.x = 1;point_laser.point.y = 1;point_laser.point.z = 0;// 5.转换坐标点(相对于父级坐标系)//新建一个坐标点，用于接收转换结果  //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------try{geometry_msgs::PointStamped point_base;point_base = buffer.transform(point_laser,"world");ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);}catch(const std::exception& e){// std::cerr << e.what() << '\n';ROS_INFO("程序异常:%s",e.what());}r.sleep();  ros::spinOnce();}return 0;
}

配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。

可以使用 rviz 查看坐标系相对关系。

示例的结果：

方案B:Python实现

1.创建功能包

创建项目功能包依赖于

 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

#! /usr/bin/env python
"""  动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布实现分析:1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度3.将 pose 信息转换成 坐标系相对信息并发布实现流程:1.导包2.初始化 ROS 节点3.订阅 /turtle1/pose 话题消息4.回调函数处理4-1.创建 TF 广播器4-2.创建 广播的数据(通过 pose 设置)4-3.广播器发布数据5.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped#     4.回调函数处理
def doPose(pose):#         4-1.创建 TF 广播器broadcaster = tf2_ros.TransformBroadcaster()#         4-2.创建 广播的数据(通过 pose 设置)tfs = TransformStamped()tfs.header.frame_id = "world"tfs.header.stamp = rospy.Time.now()tfs.child_frame_id = "turtle1"tfs.transform.translation.x = pose.xtfs.transform.translation.y = pose.ytfs.transform.translation.z = 0.0qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)tfs.transform.rotation.x = qtn[0]tfs.transform.rotation.y = qtn[1]tfs.transform.rotation.z = qtn[2]tfs.transform.rotation.w = qtn[3]#         4-3.广播器发布数据broadcaster.sendTransform(tfs)if __name__ == "__main__":# 2.初始化 ROS 节点rospy.init_node("dynamic_tf_pub_p")# 3.订阅 /turtle1/pose 话题消息sub = rospy.Subscriber("/turtle1/pose",Pose,doPose)#     4.回调函数处理#         4-1.创建 TF 广播器#         4-2.创建 广播的数据(通过 pose 设置)#         4-3.广播器发布数据#     5.spinrospy.spin()

权限设置以及配置文件此处略。

示例的效果如下：

3.订阅方

#! /usr/bin/env python
"""  动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布实现分析:1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度3.将 pose 信息转换成 坐标系相对信息并发布实现流程:1.导包2.初始化 ROS 节点3.创建 TF 订阅对象4.处理订阅的数据"""
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStampedif __name__ == "__main__":# 2.初始化 ROS 节点rospy.init_node("static_sub_tf_p")# 3.创建 TF 订阅对象buffer = tf2_ros.Buffer()listener = tf2_ros.TransformListener(buffer)rate = rospy.Rate(1)while not rospy.is_shutdown():    # 4.创建一个 radar 坐标系中的坐标点point_source = PointStamped()point_source.header.frame_id = "turtle1"point_source.header.stamp = rospy.Time.now()point_source.point.x = 10point_source.point.y = 2point_source.point.z = 3try:#     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标point_target = buffer.transform(point_source,"world",rospy.Duration(1))rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",point_target.point.x,point_target.point.y,point_target.point.z)except Exception as e:rospy.logerr("异常:%s",e)#     6.spinrate.sleep()

权限设置以及配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。

可以使用 rviz 查看坐标系相对关系。

示例结果如下：

3. 多坐标变换

需求描述:

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

实现分析:

1. 首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
2. 然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
3. 最后，还要实现坐标点的转换

**实现流程:**C++ 与 Python 实现流程一致

1. 新建功能包，添加依赖
2. 创建坐标相对关系发布方(需要发布两个坐标相对关系)
3. 创建坐标相对关系订阅方
4. 执行

方案A:C++实现

1.创建功能包

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

2.发布方

为了方便，使用静态坐标变换发布

示例结果：

3.订阅方

/*需求:现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:1.包含头文件2.初始化 ros 节点3.创建 ros 句柄4.创建 TF 订阅对象5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标解析 son1 中的点相对于 son2 的坐标6.spin*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"int main(int argc, char *argv[])
{   setlocale(LC_ALL,"");// 2.初始化 ros 节点ros::init(argc,argv,"sub_frames");// 3.创建 ros 句柄ros::NodeHandle nh;// 4.创建 TF 订阅对象tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer);// 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标ros::Rate r(1);while (ros::ok()){try{//   解析 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,tfs.transform.translation.z);// 坐标点解析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);}catch(const std::exception& e){// std::cerr << e.what() << '\n';ROS_INFO("异常信息:%s",e.what());}r.sleep();// 6.spinros::spinOnce();}return 0;
}

配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，将输出换算后的结果。

示例结果：

方案B:Python实现

1.创建功能包

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

2.发布方

为了方便，使用静态坐标变换发布

3.订阅方

#!/usr/bin/env python
"""  需求:现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标实现流程:   1.导包2.初始化 ROS 节点3.创建 TF 订阅对象4.调用 API 求出 son1 相对于 son2 的坐标关系5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标6.spin"""
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf2_geometry_msgs import PointStampedif __name__ == "__main__":# 2.初始化 ROS 节点rospy.init_node("frames_sub_p")# 3.创建 TF 订阅对象buffer = tf2_ros.Buffer()listener = tf2_ros.TransformListener(buffer)rate = rospy.Rate(1)while not rospy.is_shutdown():try:# 4.调用 API 求出 son1 相对于 son2 的坐标关系#lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):tfs = buffer.lookup_transform("son2","son1",rospy.Time(0))rospy.loginfo("son1 与 son2 相对关系:")rospy.loginfo("父级坐标系:%s",tfs.header.frame_id)rospy.loginfo("子级坐标系:%s",tfs.child_frame_id)rospy.loginfo("相对坐标:x=%.2f, y=%.2f, z=%.2f",tfs.transform.translation.x,tfs.transform.translation.y,tfs.transform.translation.z,)# 5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标point_source = PointStamped()point_source.header.frame_id = "son1"point_source.header.stamp = rospy.Time.now()point_source.point.x = 1point_source.point.y = 1point_source.point.z = 1point_target = buffer.transform(point_source,"son2",rospy.Duration(0.5))rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id)rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)",point_target.point.x,point_target.point.y,point_target.point.z)except Exception as e:rospy.logerr("错误提示:%s",e)rate.sleep()# 6.spin    # rospy.spin()

权限设置以及配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，将输出换算后的结果。

示例结果：

4. 坐标系关系查看

在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。

1.准备

首先调用rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装:

sudo apt install ros-noetic-tf2-tools

2.使用

启动坐标系广播程序之后，运行如下命令，生成 pdf 文件

rosrun tf2_tools view_frames.py

会产生类似于下面的日志信息:

[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...

查看当前目录会生成一个 frames.pdf 文件

可以直接进入目录打开文件，或者调用命令查看文件:evince frames.pdf，查看文件

内如如图所示:

5. TF坐标变换实操

需求描述:

程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

结果演示:

实现分析:

乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

1. 启动乌龟显示节点
2. 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
3. 编写两只乌龟发布坐标信息的节点
4. 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

**实现流程:**C++ 与 Python 实现流程一致

1. 新建功能包，添加依赖
2. 编写服务客户端，用于生成一只新的乌龟
3. 编写发布方，发布两只乌龟的坐标信息
4. 编写订阅方，订阅两只乌龟信息，生成速度信息并发布
5. 运行

准备工作:

1.了解如何创建第二只乌龟，且不受键盘控制

创建第二只乌龟需要使用rosservice,话题使用的是 spawn

rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'" 
name: "turtle_flow"

键盘是无法控制第二只乌龟运动的，因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息

2.了解如何获取两只乌龟的坐标

是通过话题 /乌龟名称/pose 来获取的

x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度

方案A:C++实现

1.创建功能包

创建项目功能包依赖于

tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.服务客户端(生成乌龟)

/* 创建第二只小乌龟*/
#include "ros/ros.h"
#include "turtlesim/Spawn.h"int main(int argc, char *argv[])
{setlocale(LC_ALL,"");//执行初始化ros::init(argc,argv,"create_turtle");//创建节点ros::NodeHandle nh;//创建服务客户端ros::ServiceClient client = nh.serviceClient("/spawn");ros::service::waitForService("/spawn");turtlesim::Spawn spawn;spawn.request.name = "turtle2";spawn.request.x = 1.0;spawn.request.y = 2.0;spawn.request.theta = 3.12415926;bool flag = client.call(spawn);if (flag){ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());}else{ROS_INFO("乌龟2创建失败!");}ros::spin();return 0;
}

配置文件此处略。

3.发布方(发布两只乌龟的坐标信息)

可以订阅乌龟的位姿信息，然后再转换成坐标信息，两只乌龟的实现逻辑相同，只是订阅的话题名称，生成的坐标信息等稍有差异，可以将差异部分通过参数传入:

• 该节点需要启动两次
• 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

/*  该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,其他的话题名称和实现逻辑都是一样的，所以我们可以将所需的命名空间通过 args 动态传入实现流程:1.包含头文件2.初始化 ros 节点3.解析传入的命名空间4.创建 ros 句柄5.创建订阅对象6.回调函数处理订阅的 pose 信息6-1.创建 TF 广播器6-2.将 pose 信息转换成 TransFormStamped6-3.发布7.spin*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;void doPose(const turtlesim::Pose::ConstPtr& pose){//  6-1.创建 TF 广播器 ---------------------------------------- 注意 staticstatic tf2_ros::TransformBroadcaster broadcaster;//  6-2.将 pose 信息转换成 TransFormStampedgeometry_msgs::TransformStamped tfs;tfs.header.frame_id = "world";tfs.header.stamp = ros::Time::now();tfs.child_frame_id = turtle_name;tfs.transform.translation.x = pose->x;tfs.transform.translation.y = pose->y;tfs.transform.translation.z = 0.0;tf2::Quaternion qtn;qtn.setRPY(0,0,pose->theta);tfs.transform.rotation.x = qtn.getX();tfs.transform.rotation.y = qtn.getY();tfs.transform.rotation.z = qtn.getZ();tfs.transform.rotation.w = qtn.getW();//  6-3.发布broadcaster.sendTransform(tfs);} int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ros 节点ros::init(argc,argv,"pub_tf");// 3.解析传入的命名空间if (argc != 2){ROS_ERROR("请传入正确的参数");} else {turtle_name = argv[1];ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());}// 4.创建 ros 句柄ros::NodeHandle nh;// 5.创建订阅对象ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",1000,doPose);//     6.回调函数处理订阅的 pose 信息//         6-1.创建 TF 广播器//         6-2.将 pose 信息转换成 TransFormStamped//         6-3.发布// 7.spinros::spin();return 0;
}

配置文件此处略。

4.订阅方(解析坐标信息并生成速度信息)

/*  订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布实现流程:1.包含头文件2.初始化 ros 节点3.创建 ros 句柄4.创建 TF 订阅对象5.处理订阅到的 TF6.spin*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ros 节点ros::init(argc,argv,"sub_TF");// 3.创建 ros 句柄ros::NodeHandle nh;// 4.创建 TF 订阅对象tf2_ros::Buffer buffer;tf2_ros::TransformListener listener(buffer);// 5.处理订阅到的 TF// 需要创建发布 /turtle2/cmd_vel 的 publisher 对象ros::Publisher pub = nh.advertise("/turtle2/cmd_vel",1000);ros::Rate rate(10);while (ros::ok()){try{//5-1.先获取 turtle1 相对 turtle2 的坐标信息geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));//5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.hgeometry_msgs::Twist twist;twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);//5-3.发布速度信息 -- 需要提前创建 publish 对象pub.publish(twist);}catch(const std::exception& e){// std::cerr << e.what() << '\n';ROS_INFO("错误提示:%s",e.what());}rate.sleep();// 6.spinros::spinOnce();}return 0;
}

配置文件此处略。

5.运行

使用 launch 文件组织需要运行的节点，内容示例如下:

方案B:Python实现

1.创建功能包

创建项目功能包依赖于

tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.服务客户端(生成乌龟)

#! /usr/bin/env python
"""  调用 service 服务在窗体指定位置生成一只乌龟流程:1.导包2.初始化 ros 节点3.创建服务客户端4.等待服务启动5.创建请求数据6.发送请求并处理响应
"""
#1.导包
import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponseif __name__ == "__main__":# 2.初始化 ros 节点rospy.init_node("turtle_spawn_p")# 3.创建服务客户端client = rospy.ServiceProxy("/spawn",Spawn)# 4.等待服务启动client.wait_for_service()# 5.创建请求数据req = SpawnRequest()req.x = 1.0req.y = 1.0req.theta = 3.14req.name = "turtle2"# 6.发送请求并处理响应try:response = client.call(req)rospy.loginfo("乌龟创建成功，名字是:%s",response.name)except Exception as e:rospy.loginfo("服务调用失败....")

权限设置以及配置文件此处略。

3.发布方(发布两只乌龟的坐标信息)

#! /usr/bin/env python
"""  该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,其他的话题名称和实现逻辑都是一样的，所以我们可以将所需的命名空间通过 args 动态传入实现流程:1.导包2.初始化 ros 节点3.解析传入的命名空间4.创建订阅对象5.回调函数处理订阅的 pose 信息5-1.创建 TF 广播器5-2.将 pose 信息转换成 TransFormStamped5-3.发布6.spin
"""
# 1.导包
import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversionsturtle_name = ""def doPose(pose):# rospy.loginfo("x = %.2f",pose.x)#1.创建坐标系广播器broadcaster = tf2_ros.TransformBroadcaster()#2.将 pose 信息转换成 TransFormStampedtfs = TransformStamped()tfs.header.frame_id = "world"tfs.header.stamp = rospy.Time.now()tfs.child_frame_id = turtle_nametfs.transform.translation.x = pose.xtfs.transform.translation.y = pose.ytfs.transform.translation.z = 0.0qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)tfs.transform.rotation.x = qtn[0]tfs.transform.rotation.y = qtn[1]tfs.transform.rotation.z = qtn[2]tfs.transform.rotation.w = qtn[3]#3.广播器发布 tfsbroadcaster.sendTransform(tfs)if __name__ == "__main__":# 2.初始化 ros 节点rospy.init_node("sub_tfs_p")# 3.解析传入的命名空间rospy.loginfo("-------------------------------%d",len(sys.argv))if len(sys.argv) < 2:rospy.loginfo("请传入参数:乌龟的命名空间")else:turtle_name = sys.argv[1]rospy.loginfo("///乌龟:%s",turtle_name)rospy.Subscriber(turtle_name + "/pose",Pose,doPose)#     4.创建订阅对象#     5.回调函数处理订阅的 pose 信息#         5-1.创建 TF 广播器#         5-2.将 pose 信息转换成 TransFormStamped#         5-3.发布#     6.spinrospy.spin()

权限设置以及配置文件此处略。

4.订阅方(解析坐标信息并生成速度信息)

#! /usr/bin/env python
"""  订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布实现流程:1.导包2.初始化 ros 节点3.创建 TF 订阅对象4.处理订阅到的 TF4-1.查找坐标系的相对关系4-2.生成速度信息，然后发布
"""
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import mathif __name__ == "__main__":# 2.初始化 ros 节点rospy.init_node("sub_tfs_p")# 3.创建 TF 订阅对象buffer = tf2_ros.Buffer()listener = tf2_ros.TransformListener(buffer)# 4.处理订阅到的 TFrate = rospy.Rate(10)# 创建速度发布对象pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)while not rospy.is_shutdown():rate.sleep()try:#def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))# rospy.loginfo("相对坐标:(%.2f,%.2f,%.2f)",#             trans.transform.translation.x,#             trans.transform.translation.y,#             trans.transform.translation.z#             )   # 根据转变后的坐标计算出速度和角速度信息twist = Twist()# 间距 = x^2 + y^2  然后开方twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)pub.publish(twist)except Exception as e:rospy.logwarn("警告:%s",e)

权限设置以及配置文件此处略。

5.运行

使用 launch 文件组织需要运行的节点，内容示例如下:

实现的效果如下：

Reference

http://www.autolabor.com.cn/book/ROSTutorials/di-2-zhang-ros-jia-gou-she-ji/23-fu-wu-tong-xin/224-fu-wu-tong-xin-zi-ding-yi-srv-diao-yong-b-python.html