【ROS】TF2坐标转换及实战示例

本文涉及的产品
资源编排,不限时长
简介: ROS中提供了坐标转换的软件包 Transform Frame TF的作用是ROS中实现不同坐标点/向量的转换。

Halo,这里是Ppeua。平时主要更新C++,数据结构算法…感兴趣就关注我吧!你定不会失望。


3f46226b7d4d4fe79a68e8be70d91454.jpg


0.ROS中的坐标转换消息包


在日常生活中,特别是对于机器人来说,各个目标系中的坐标转换是很关键的,通过右手系来标注坐标。

ROS中提供了坐标转换的软件包 Transform Frame TF的作用是ROS中实现不同坐标点/向量的转换。


562690e55f64450ab6eb36bc161d1e73.png


不过TF在若干个版本前已经弃用,现在使用的是全新的版本:TF2,其有几个相关的功能包:


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

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

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


cc6b1399c49b4b779ef63d275d4c3801.png


在坐标系转换中,在geometry下有两个重要的消息类型:TransformStamped、PointStamped,前者用于坐标系间的转换,后者用于点之间的坐标转换,这对我们之后的使用很重要。先来了解下这两种消息类型中的内容。


0.1 geometry_msgs/TransformStamped


该消息类型表示坐标系之间的关系

在终端中输入


rosmsg info geometry_msgs/TransformStamped


查看该消息类型的具体信息:


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


可以看出 PointStamped消息是由:

std_msgs/Header,string,geometry_msgs/Transform封装在一起,组成的新消息类型。

其中Transform又是由geometry_msgs/Vector3,geometry_msgs/Quaternion进行封装的。


0.2 geometry_msgs/PointStamped


该消息类型表示坐标点之间的转换

在终端中输入


rosmsg info geometry_msgs/PointStamped


可以查看该消息中的具体信息


ea5877fe3ce94c5a87856010a1569ddb.png


std_msgs/Header header      #头信息
    uint32 seq                  ##序列号
    time stamp                  ##时间戳
    string frame_id             ##坐标系
geometry_msgs/Point point   #点坐标
    float64 x                   
    float64 y
    float64 z


可以看出 PointStamped消息是由:

std_msgs/Header与geometry_msgs/Point封装在一起,组成的新消息类型。


1.静态坐标转换


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


4b44ef8844764516a3d00ed5458a006f.png


组织下我们发布方的整体逻辑:


  1. 导入所需功能包
  2. 初始化ros节点
  3. 创建静态坐标广播器
  4. 编写静态坐标信息
  5. 发送消息
  6. spin()


这里是接收方的逻辑:


  1. 导入所需要的功能包
  2. 初始化ros节点
  3. 创建TF订阅对象
  4. 创建lase的坐标点
  5. 坐标转换
  6. spin()


1.1导入所需功能包


在这个案例中,需要:rospy,std_msgs 这两个标准件

还需要:

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

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

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

1.2发布方实现


"""
导入功能包
""" 
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
import rospy
"""
初始化节点信息
创建发布对象
组织发布数据
发布数据
spin()
"""
#初始化ros节点
rospy.init_node("static_pub")
#创建静态发布对象
pub=tf2_ros.StaticTransformBroadcaster()
#组织消息类型
ts=TransformStamped()
ts.header.seq=123
ts.header.stamp=rospy.Time.now()
ts.child_frame_id="laser"
ts.header.frame_id="frame_id"
ts.transform.translation.x=0.2
ts.transform.translation.y=0
ts.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 rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import 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.0
ps.point.y=3.0
ps.point.z=5.0
rate=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.动态坐标转换


在现实生活中,我们面对的不仅有点对点的坐标转换,还动态的坐标转换。

我们以乌龟为例来实现一下动态坐标转换

先来组织下发布方的逻辑


1.导包 rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim

2.初始化ros节点

3.订阅 /turtle1/pose 话题消息

4.回调函数

  1.创建TF广播器

  2.组织广播数据

  3.广播器发布数据

5.spin

接收方的逻辑

6.导包

7.初始化ros节点

8.创建TF对象

9.处理订阅数据


2.1发布方实现


import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import 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.x
    ts.transform.translation.y=pose.y
    ts.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 rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
if __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.spin
        rate.sleep()


2.3实现效果


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


rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key


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


rosrun tf02_dynamic demo01_tf02_pub.py
rosrun tf02_dynamic demo01_tf02_sub.py


9909ec2bec0146d6bb8c8c5fbd60d151.png


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订阅方实现


订阅方逻辑实现


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

2.初始化ros节点

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


import rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import tf2_ros
from geometry_msgs.msg import TransformStamped
rospy.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:
        pass
        rospy.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.0
theta: 0.0
name: ''" 


若返回输入的名字,此时就能在屏幕上看到刚刚生成的那只乌龟

准备工作都做完了,现在开始创建坐标系


9d9b8b05da7f4939a679d01a3d9be2b1.png


4.1乌龟位姿信息发布


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


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

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

3.控制cmd发布速度信息


import rospy
import sys
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
def doPose(pose):
    pub=tf2_ros.TransformBroadcaster()
    ts=TransformStamped()
    ts.header.frame_id="world"
    ts.header.stamp=rospy.Time.now()
    ts.child_frame_id=turtle_name
    ts.transform.translation.x=pose.x
    ts.transform.translation.y=pose.y
    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)
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 控制乌龟进行跟随运动


总体逻辑:


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

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

3.发布


import rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import tf2_ros
from geometry_msgs.msg import TransformStamped,Twist
import math
import 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:
        pass
        rospy.logwarn("错误提示%s",ee)
    rate.sleep()


af62f9c200f74012b84b4090feec7605.png


4.3查看当前坐标关系


rosrun tf2_tools view_frames.py


a12d0027052e4ff1b6097c5ecfbc0232.png

相关实践学习
使用ROS创建VPC和VSwitch
本场景主要介绍如何利用阿里云资源编排服务,定义资源编排模板,实现自动化创建阿里云专有网络和交换机。
阿里云资源编排ROS使用教程
资源编排(Resource Orchestration)是一种简单易用的云计算资源管理和自动化运维服务。用户通过模板描述多个云计算资源的依赖关系、配置等,并自动完成所有资源的创建和配置,以达到自动化部署、运维等目的。编排模板同时也是一种标准化的资源和应用交付方式,并且可以随时编辑修改,使基础设施即代码(Infrastructure as Code)成为可能。 产品详情:https://www.aliyun.com/product/ros/
目录
相关文章
|
5月前
|
传感器 机器人 C++
ROS 2机器人编程实战:基于现代C++和Python 3实现简单机器人项目
ROS 2机器人编程实战:基于现代C++和Python 3实现简单机器人项目
558 0
|
机器人 API
[ROS基础] --- TF坐标转换
[ROS基础] --- TF坐标转换
179 0
ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换
ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换
ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换
|
数据可视化 机器人 C++
ROS TF2
turtlesim的多机器人示例中,展示了tf2的一些功能。 也介绍了使用tf2_echo,view_frames和rviz。 使对tf2可以做的事情有个很好的了解。
ROS TF2
|
传感器 C++
ROS TF2 添加一个 坐标系 附实例
ROS TF2 添加一个 坐标系 附实例
ROS TF2 添加一个 坐标系 附实例
|
传感器 缓存
ROS TF2 :通过tf2_ros::MessageFilter 将世界坐标系下的点转为期望坐标系下的点
ROS TF2 :通过tf2_ros::MessageFilter 将世界坐标系下的点转为期望坐标系下的点
|
5月前
|
传感器 人工智能 算法
ROS机器人操作系统
ROS机器人操作系统
154 1
|
4月前
|
机器学习/深度学习 传感器 算法
强化学习(RL)在机器人领域的应用,尤其是结合ROS(Robot Operating System)和Gazebo(机器人仿真环境)
强化学习(RL)在机器人领域的应用,尤其是结合ROS(Robot Operating System)和Gazebo(机器人仿真环境)
166 2
|
4月前
|
机器人 定位技术 C++
技术笔记:ROS中测试机器人里程计信息
技术笔记:ROS中测试机器人里程计信息

推荐镜像

更多