导入所需功能包

初始化ros节点

创建静态坐标广播器

编写静态坐标信息

发送消息

spin()

这里是接收方的逻辑：

导入所需要的功能包

初始化ros节点

创建TF订阅对象

创建lase的坐标点

坐标转换

spin（）

1.1导入所需功能包

在这个案例中，需要:rospy,std_msgs 这两个标准件
还需要：
tf2:封装了坐标变换的常用消息。
tf2_ros: 为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。

1.2发布方实现

"""导入功能包""" import tf2_rosfrom geometry_msgs.msg import TransformStampedimport tfimport rospy"""初始化节点信息创建发布对象组织发布数据发布数据spin()"""#初始化ros节点rospy.init_node("static_pub")#创建静态发布对象pub=tf2_ros.StaticTransformBroadcaster()#组织消息类型ts=TransformStamped()ts.header.seq=123ts.header.stamp=rospy.Time.now()ts.child_frame_id="laser"ts.header.frame_id="frame_id"ts.transform.translation.x=0.2ts.transform.translation.y=0ts.transform.translation.z=0.5"""将欧拉角放到四元数中进行转换用到了tf中的transformation.quaternion_from_euler"""qtn=tf.transformations.quaternion_from_euler(0,0,0)ts.transform.rotation.x=qtn[0]ts.transform.rotation.y=qtn[1]ts.transform.rotation.z=qtn[2]ts.transform.rotation.w=qtn[3]#发布消息pub.sendTransform(tf)rospy.spin()

1.3 订阅方实现

"""导入功能包"""import rospyfrom tf2_geometry_msgs import tf2_geometry_msgsimport tf2_ros#初始化节点rospy.init_node("static_sub")#创建缓存对象buffer=tf2_ros.Buffer()"""调用tf2_ros.Buffer()创建一个buffer用来存储坐标消息"""tf2_ros.TransformListener(buffer)"""监听tf坐标变换,将值存入buffer中""""""创建点坐标信息"""ps=tf2_geometry_msgs.PointStamped()ps.header.stamp=rospy.Time.now()ps.header.frame_id="laser"ps.point.x=2.0ps.point.y=3.0ps.point.z=5.0rate=rospy.Rate(10)while not rospy.is_shutdown():try:"""调用buffer.transform 将点坐标与原始坐标进行转换"""ps_out=buffer.transform(ps,"frame_id")rospy.loginfo("转换后的坐标：（%.2f,%.2f,%.2f）,参考坐标系：%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id)except Exception as ee:rospy.logwarn("错误提示%s",ee)rate.sleep()

1.4 tf2_ros实现静态坐标转换

由于静态坐标转换中的整体逻辑大致相同,所以tf2_ros提供了一个功能包来直接实现坐标转换,不需要每次都使用编写代码

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

2.动态坐标转换

在现实生活中，我们面对的不仅有点对点的坐标转换，还动态的坐标转换。
我们以乌龟为例来实现一下动态坐标转换

先来组织下发布方的逻辑

导包 rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim

初始化ros节点

订阅 /turtle1/pose 话题消息

回调函数

创建TF广播器

组织广播数据

广播器发布数据

spin
接收方的逻辑

导包

初始化ros节点

创建TF对象

处理订阅数据

2.1发布方实现

import rospyfrom turtlesim.msg import Poseimport tf2_rosfrom geometry_msgs.msg import TransformStampedimport tf"""订阅乌龟的位姿信息"""def doPose(pose):#创建动态坐标发布对象pub=tf2_ros.TransformBroadcaster()#组织点坐标消息类型ts=TransformStamped()ts.header.frame_id="world"ts.child_frame_id="turtle1"ts.header.stamp=rospy.Time.now()#坐标系相对于子集坐标系ts.transform.translation.x=pose.xts.transform.translation.y=pose.yts.transform.translation.z=0#四元数转换qtn=tf.transformations.quaternion_from_euler(0,0,pose.theta)ts.transform.rotation.x=qtn[0]ts.transform.rotation.y=qtn[1]ts.transform.rotation.z=qtn[2]ts.transform.rotation.w=qtn[3]pub.sendTransform(ts)#初始化ROS节点rospy.init_node("tf02_pub")#订阅消息位姿信息，创建回调函数sub=rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)rospy.spin()

2.2订阅方逻辑

import rospyimport 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()# 监听坐标变换存入buffer中tf2_ros.TransformListener(buffer)rate = rospy.Rate(1)while not rospy.is_shutdown(): # 4.创建坐标点信息# 仅需提供目标坐标系point_source = PointStamped()point_source.header.frame_id = "turtle1"point_source.header.stamp = rospy.Time.now()try:# 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()

2.3实现效果

首先启动turtlesim的键盘控制节点与GUI

rosrun turtlesim turtlesim_noderosrun turtlesim turtle_teleop_key

接着启动发布方与接收方 之后就可以在屏幕上看到转换后的坐标系

rosrun tf02_dynamic demo01_tf02_pub.pyrosrun tf02_dynamic demo01_tf02_sub.py

3.0多坐标转换

将多个坐标先相对于世界坐标系进行转换,然后在调用api将转换后的数据进行相互转换

3.1发布方实现

直接调用静态坐标转换的ros包,写成launch文件

<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>

3.2订阅方实现

订阅方逻辑实现

导入包 rospy std_msgs tf2_ros geometry_msgs(TransformStamped 坐标系转换) tf2_geomerty_msgs(PointStamped 坐标点转换)

初始化ros节点

创建TF订阅对象,实现两个坐标系之间相互转换

import rospyfrom tf2_geometry_msgs import tf2_geometry_msgsimport tf2_rosfrom geometry_msgs.msg import TransformStampedrospy.init_node("static_sub")#创建缓存对象buffer=tf2_ros.Buffer()sub=tf2_ros.TransformListener(buffer)rate=rospy.Rate(10)while not rospy.is_shutdown():try:"""计算son1相对于son2的坐标关系lookup_transform(父级坐标系，子级坐标系，取坐标的时间，时间间隔)"""ts=buffer.lookup_transform("son2","son1",rospy.Time(0))rospy.loginfo("父级坐标系：%s，子级坐标系：%s,%.2f,%.2f,%.2f",ts.header.frame_id,ts.child_frame_id,ts.transform.translation.x,ts.transform.translation.y,ts.transform.translation.z)except Exception as ee:passrospy.logwarn("错误提示%s",ee)rate.sleep()

3.3 view_frames查看当前坐标系

运行以上节点后，在任意工作目录下输入

rosrun tf2_tools view_frames.py

会在当前目录下生成一个可以坐标关系的pdf，可以利用此工具查看坐标关系
请添加图片描述

4.0 tf坐标变换实操

我们先来创建turtle，运行turtlesim这个节点

rosrun turtlesim turtlesim_node

通过rosservice的/spawn服务来多生成一只turtle来完成我们的多坐标转换,生成一只名为H的乌龟

rosservice call /spawn "x: 0.0 y: 0.0theta: 0.0name: ''"

若返回输入的名字，此时就能在屏幕上看到刚刚生成的那只乌龟
准备工作都做完了，现在开始创建坐标系

4.1乌龟位姿信息发布

先来理清整个跟随的逻辑：

在坐标系中发布两只乌龟的信息

将第二只乌龟的位姿信息相对第一只乌龟作转换

控制cmd发布速度信息

import rospyimport sysfrom turtlesim.msg import Poseimport tf2_rosfrom geometry_msgs.msg import TransformStampedimport tfdef doPose(pose):pub=tf2_ros.TransformBroadcaster()ts=TransformStamped()ts.header.frame_id="world"ts.header.stamp=rospy.Time.now()ts.child_frame_id=turtle_namets.transform.translation.x=pose.xts.transform.translation.y=pose.yqtn=tf.transformations.quaternion_from_euler(0,0,pose.theta)ts.transform.rotation.x=qtn[0]ts.transform.rotation.y=qtn[1]ts.transform.rotation.z=qtn[2]ts.transform.rotation.w=qtn[3]pub.sendTransform(ts)rospy.init_node("dynamic_pub",anonymous=True)if len(sys.argv)>=2: turtle_name=sys.argv[1]sub=rospy.Subscriber(turtle_name+"/pose",Pose,doPose,queue_size=10)rospy.spin()else:print(sys.argv[1])rospy.loginfo("请输入坐标名称")sys.exit()

这份代码出现过很多次了，这里就不过多赘述。注意：sys.argv的第一个参数为文件名 之后的为传入参数

4.2 控制乌龟进行跟随运动

总体逻辑：

计算两个乌龟之间的相对坐标

控制乌龟的线速度与角速度

发布

import rospyfrom tf2_geometry_msgs import tf2_geometry_msgsimport tf2_rosfrom geometry_msgs.msg import TransformStamped,Twistimport mathimport sys"""创建订阅对象组织被转换的坐标点转换逻辑实现调用tf封装的算法输出结果"""rospy.init_node("static_sub")#创建缓存对象buffer=tf2_ros.Buffer()sub=tf2_ros.TransformListener(buffer)pub=rospy.Publisher("/H/cmd_vel",Twist,queue_size=10)rate=rospy.Rate(10)while not rospy.is_shutdown():try:"""计算son1相对于son2的坐标关系直接监听整个坐标系，不需要订阅话题"""ts=buffer.lookup_transform("H","turtle1",rospy.Time(0))rospy.loginfo("父级坐标系：%s，子级坐标系：%s,%.2f,%.2f,%.2f",ts.header.frame_id,ts.child_frame_id,ts.transform.translation.x,ts.transform.translation.y,ts.transform.translation.z)twist=Twist()twist.linear.x=0.5*math.sqrt(math.pow(ts.transform.translation.x,2)+math.pow(ts.transform.translation.y,2))twist.angular.z=4*math.atan2(ts.transform.translation.y,ts.transform.translation.x)pub.publish(twist)except Exception as ee:passrospy.logwarn("错误提示%s",ee)rate.sleep()

4.3查看当前坐标关系

rosrun tf2_tools view_frames.py