ROS学习笔记8:TF坐标变换

前言

本人ROS小白,利用寒假时间学习ROS,在此以笔记的方式记录自己每天的学习过程。争取写满20篇(8/20)。
环境:Ubuntu20.04、ROS1:noetic
环境配置:严格按照下方学习链接的教程配置,基本一次成功。
学习链接:【Autolabor初级教程】ROS机器人入门
对应链接文档:ROS机器人入门课程《ROS理论与实践》
笔记绝大部分代码使用Python语言编写。
本期关键词:静态变换,动态变换,多坐标变换

准备工作

了解坐标变换中常用的消息载体:geometry_msgs/TransformStampedgeometry_msgs/PointStamped

geometry_msgs/TransformStamped
  1. geometry_msgs/TransformStamped用于传输坐标系相关位置信息
  2. 先运行一下ROS自带的乌龟跟随指令:roslaunch turtle_tf2 turtle_tf2_demo.launch。如果运行时报错,可执行:sudo ln -s /usr/bin/python3 /usr/bin/python ,这个命令在Linux系统中创建了一个符号链接(也称为软链接),它将/usr/bin/python链接到/usr/bin/python3。这个操作通常用于解决Python版本冲突或者指定系统默认使用的Python版本。然后新开终端运行rostopic list,得到以下结果:(我们主要关注/tf_static。)
/rosout
/rosout_agg
/tf
/tf_static
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
/turtle2/cmd_vel
/turtle2/color_sensor
/turtle2/pose
  1. 依次执行以下指令:(或者也可以直接执行最后一条指令)
rostopic type /tf_static
rosmsg info tf2_msgs/TFMessage
rosmsg info geometry_msgs/TransformStamped
  1. 结果如下:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/Transform transform
  geometry_msgs/Vector3 translation
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation
    float64 x
    float64 y
    float64 z
    float64 w
  1. 需要注意:frame_id(父坐标系)是属于header字段的,child_frame_id(子坐标系)是单独一个字段的,写代码时不要弄混。
  2. 消息类型中的translation代表坐标系在xyz轴上的偏移,rotation代表坐标系的翻转、俯仰、偏航状态,表示方法是四元数表示法(不理解四元数也没关系,不影响写代码和理解坐标系的位置状态,如果想深入了解四元数,可参考:[四元数的可视化](【四元数的可视化】 https://www.bilibili.com/video/BV1SW411y7W1/?share_source=copy_web&vd_source=181138744abfbdc4eb6c07c38a129a45)。)
geometry_msgs/PointStamped
  1. geometry_msgs/PointStamped用于传输某个坐标系内坐标点的信息
  2. 这里我们直接用rosmsg info geometry_msgs/PointStamped命令查看:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z
  1. 坐标点的消息类型比坐标系的消息类型简单,各字段参数的含义均相同(但无四元数表示)。

一、静态变换

两个部分:发布方订阅方

代码编写

1. 发布方

代码实现:

#! usr/bin/env python

import rospy
import tf2_ros
import tf
import tf.transformations
from geometry_msgs.msg import TransformStamped
# geometry_msgs功能包下的msg文件夹,该文件夹下的TransformStamped.msg文件

if __name__=="__main__":
    rospy.init_node("static_tf_pub")
    # 和话题的发布和订阅不同,这里使用广播器广播数据,先创建一个静态坐标广播器
    broadcaster=tf2_ros.StaticTransformBroadcaster()
    # 创建要被广播的消息
    tf_pub=TransformStamped()
    # 开始设置该消息
    tf_pub.header.stamp=rospy.Time.now()
    tf_pub.header.frame_id="base_link"# 父坐标系,也可以设置为world,方便理解即可
    tf_pub.child_frame_id="radar"
    tf_pub.transform.translation.x=0.1
    tf_pub.transform.translation.y=0.2
    tf_pub.transform.translation.z=0.3
    # 以下是四元数的设置,这个我们用tf2内置的算法来求,不用自己手算
    # 如果下面的transformations不能自动补齐,可以通过import tf.transformations解决
    qua=tf.transformations.quaternion_from_euler(0,0,0)
    tf_pub.transform.rotation.x=qua[0]
    tf_pub.transform.rotation.y=qua[1]
    tf_pub.transform.rotation.z=qua[2]
    tf_pub.transform.rotation.w=qua[3]
    
    # 以上,数据已经设置好了,接下来就是广播发布
    broadcaster.sendTransform(tf_pub)
    
    rospy.spin()# 确保广播持续进行

指令测试:

rostopic echo /tf_static 

结果:

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1706501229
        nsecs: 920775890
      frame_id: "base_link"
    child_frame_id: "radar"
    transform: 
      translation: 
        x: 0.1
        y: 0.2
        z: 0.3
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

2. 订阅方

代码实现:

#! usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped

if __name__=="__main__":
    rospy.init_node("static_tf_sub")
    # 创建订阅对象,这里要用到缓冲区
    buffer1=tf2_ros.Buffer()
    # 缓冲区接收数据,这里监听缓冲区的数据
    listener=tf2_ros.TransformListener(buffer1)
    
    rate=rospy.Rate(1) # 频率设置
    # 注意rospy.is_shutdown()一定要带上小括号,不然会出bug!
    while not rospy.is_shutdown():
        point_source=PointStamped()
        point_source.header.frame_id="radar"
        point_source.header.stamp=rospy.Time.now()
        point_source.point.x=3.0
        point_source.point.y=3.0
        point_source.point.z=3.0
        rospy.loginfo("ouput4")
        
        # 避免订阅失败而导致程序中止
        try:
            point_taget=buffer1.transform(point_source,"base_link")
            rospy.loginfo("the result is: x=%.2f,y=%.2f,z=%.2f",
                          point_taget.point.x,
                          point_taget.point.y,
                          point_taget.point.z)
        except Exception as e:
            rospy.logerr("%s",e)
            
        rate.sleep()

指令实现

静态坐标变换的发布还可以用更简便的方法来实现:

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

示例:

rosrun tf2_ros static_transform_publisher 5 5 5 0 0 0 /base_link /radar

二、动态变换

这里需要借助乌龟运动控制来演示动态坐标变换。

1. 发布方

#! /usr/bin/env python

import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
import tf.transformations
from geometry_msgs.msg import TransformStamped

def doPose(pose):
    # 创建一个广播器
    broadcaster=tf2_ros.TransformBroadcaster()
    # 创建要被广播的数据
    d_pose=TransformStamped()
    d_pose.header.frame_id="world"
    # 这里要使用rospy.Time.now(),不然用rviz观察时,TF坐标系显示会有bug
    d_pose.header.stamp=rospy.Time.now()
    d_pose.child_frame_id="turtle1"
    d_pose.transform.translation.x=pose.x
    d_pose.transform.translation.y=pose.y
    d_pose.transform.translation.z=0.0
    qua=tf.transformations.quaternion_from_euler(0,0,pose.theta)
    d_pose.transform.rotation.x=qua[0]
    d_pose.transform.rotation.y=qua[1]
    d_pose.transform.rotation.z=qua[2]
    d_pose.transform.rotation.w=qua[3]
    
    broadcaster.sendTransform(d_pose)
    
    
if __name__=="__main__":
    rospy.init_node("dynamic_pub")
    # 我们要根据/turtle1/pose的数据来设置tf坐标系的数据,所以先订阅/turtle1/pose
    sub=rospy.Subscriber("/turtle1/pose",Pose,doPose)
    
    rospy.spin()

这里仍然可以使用rostopic echo指令来查看发布方发布的tf数据。

2. 订阅方

#! usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped


if __name__=="__main__":
    rospy.init_node("dynamic_sub")
    # 创建订阅对象,这里要用到缓冲区
    buffer1=tf2_ros.Buffer()
    listener=tf2_ros.TransformListener(buffer1)
    
    rate=rospy.Rate(10)
    while not rospy.is_shutdown():
	    # 创建一个坐标点
        point_source=PointStamped()
        # 设置坐标点信息
        point_source.header.frame_id="turtle1"
        
        # 这里不设置时间戳,使用rospy.Time(),防止发布方和接收方时间戳差异过大而报错
        point_source.header.stamp=rospy.Time()
        # 将“turtle1”坐标系下的点设置为原点,方便跟/turtle1/pose比对数据
        point_source.point.x=0
        point_source.point.y=0
        point_source.point.z=0
        try:
            point_taget=buffer1.transform(point_source,"world")
            rospy.loginfo("the result is: x=%.2f,y=%.2f,z=%.2f",
                          point_taget.point.x,
                          point_taget.point.y,
                          point_taget.point.z)
        except Exception as e:
            rospy.logerr("%s",e)
            
        rate.sleep()

三、多坐标变换

1. 发布方

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

2. 订阅方

#! usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped
from geometry_msgs.msg import TransformStamped


if __name__=="__main__":
    rospy.init_node("tf2_more_sub")
    # 创建订阅对象,这里要用到缓冲区
    buffer1=tf2_ros.Buffer()
    listener=tf2_ros.TransformListener(buffer1)

    rate=rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
            # 计算son1相对于son2的坐标关系(son1的值-son2的值)
            # rospy.Time(0)的作用:取时间间隔最近的两个坐标系帧结果
            tfs=buffer1.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo("父坐标系:%s,子坐标系:%s",tfs.header.frame_id,tfs.child_frame_id)            
            rospy.loginfo("the result is: x=%.2f,y=%.2f,z=%.2f",
                          tfs.transform.translation.x,
                          tfs.transform.translation.y,
                          tfs.transform.translation.z)
            
            point_source=PointStamped()
            point_source.header.frame_id="son1"
            point_source.header.stamp=rospy.Time()
            point_source.point.x = 0.1
            point_source.point.y = 0.1
            point_source.point.z = 0.1
            
            point_target = buffer1.transform(point_source,"son2")
            
            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()

输出结果为:

父坐标系:son2,子坐标系:son1
the result is: x=-0.30,y=0.00,z=0.00
point_target 所属的坐标系:son2
坐标点相对于 son2 的坐标:(-0.20,0.10,0.10)

往期内容

  1. ROS学习笔记1:基础知识
  2. ROS学习笔记2:话题通信
  3. ROS学习笔记3:服务通信与参数服务器
  4. ROS常用命令记录
  5. ROS学习笔记4:通信机制实操
  6. ROS学习笔记5:常用API和模块导入
  7. ROS学习笔记6:launch文件
  8. ROS学习笔记7:重名解决与名称设置

你可能感兴趣的:(学习,笔记,vscode,python,c++)