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

news2024/11/16 10:51:27

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

文章目录

  • 0.ROS中的坐标转换消息包
  • 0.1 geometry_msgs/TransformStamped
  • 0.2 geometry_msgs/PointStamped
  • 1.静态坐标转换
  • 1.1导入所需功能包
  • 1.2发布方实现
  • 1.3 订阅方实现
  • 1.4 tf2_ros实现静态坐标转换
  • 2.动态坐标转换
  • 2.1发布方实现
  • 2.2订阅方逻辑
  • 2.3实现效果
  • 3.0多坐标转换
  • 3.1发布方实现
  • 3.2订阅方实现
  • 3.3 view_frames查看当前坐标系
  • 4.0 tf坐标变换实操
  • 4.1乌龟位姿信息发布
  • 4.2 控制乌龟进行跟随运动
  • 4.3查看当前坐标关系

在这里插入图片描述

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

在日常生活中,特别是对于机器人来说,各个目标系中的坐标转换是很关键的,通过右手系来标注坐标。
ROS中提供了坐标转换的软件包 Transform Frame TF的作用是ROS中实现不同坐标点/向量的转换。
在这里插入图片描述

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

  1. tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
  2. tf2: 封装了坐标变换的常用消息。
  3. tf2_ros:为tf2提供了roscpp和rospy绑定,封装了坐标变换常用的API。
    请添加图片描述

在坐标系转换中,在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

可以查看该消息中的具体信息
在这里插入图片描述

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

可以看出 PointStamped消息是由:
std_msgs/Headergeometry_msgs/Point封装在一起,组成的新消息类型。

1.静态坐标转换

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

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

  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

在这里插入图片描述

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: ''" 

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

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

在这里插入图片描述

4.3查看当前坐标关系

rosrun tf2_tools view_frames.py

在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/690003.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

多元分类预测 | Matlab粒子群算法(PSO)优化极限学习机(ELM)的分类预测,多特征输入模型。PSO-ELM分类预测模型

文章目录 效果一览文章概述部分源码参考资料效果一览 文章概述 多元分类预测 | Matlab粒子群算法(PSO)优化极限学习机(ELM)的分类预测,多特征输入模型。PSO-ELM分类预测模型 多特征输入单输出的二分类及多分类模型。程序内注释详细,直接替换数据就可以用。程序语言为matlab,…

DALL-E2原理解读——大模型论文阅读笔记五

论文&#xff1a;https://cdn.openai.com/papers/dall-e-2.pdf 项目&#xff1a;https://openai.com/dall-e-2 一. 主要思想 利用CLIP提取的文本特征&#xff0c;级联式的生成图片。第一阶段通过prior将文本特征与图像特征进行对齐&#xff0c;第二阶段用扩散模型将视觉特征转…

简单demo演示Tomcat中Servlet

挺好玩的,有利于初学对容器和servlet接口规范的理解 具体代码 package org.apache;import javax.servlet.Servlet; import java.io.FileReader; import java.io.IOException; import java.util.Properties; import java.util.ResourceBundle; import java.util.Scanner;/*** a…

一文了解HTTP协议

文章目录 前言概念协议传输超文本 HTTP 协议的格式HTTP 请求HTTP 响应 总结 前言 在这之前&#xff0c;可以看看我之前的文章&#xff0c;也是关于协议的。 TCP/IP 协议详解 UDP协议详解 我们在打开一个网页的时候通常都会注意到网址的前面有一个统一的标识http://&#xf…

智慧校园电子班牌系统源码

电子班牌系统的主要功能包括&#xff1a;班级管理、学生信息管理、教师管理、课程管理、作业管理、考试管理、公告管理、评价管理、学校消息发布等。在班级管理方面&#xff0c;该系统可以实现教师对班级的整体管理以及学生个人信息的管理&#xff0c;包括个人信息、考试成绩、…

Long型参数传到前端精度丢失,后两位变为00,导致传值错误,解决方案

问题&#xff1a; 后端id字段为Long型&#xff0c;起初采用自增主键&#xff0c;没有问题&#xff1b;由于业务需要改为雪花id&#xff0c;后端可正常运行&#xff0c;传递到前端精度丢失&#xff0c;后两位变为00。 解决方案&#xff1a; 后端将属性转为字符串传递&#xff0…

Spring学习笔记---上篇

文章目录 1、Spring1.1、简介1.2、优点1.3、Spring的组成1.4、拓展 2、IOC理论推导3、IOC的本质3.1、IOC概念3.2、IoC是Spring框架的核心内容 3、HelloSpring3.1、实现3.2、思考 4、IOC创建对象的方式5、Spring配置5.1、别名&#xff08;alias&#xff09;5.2、Bean的配置5.3、…

单图换脸roop源码与环境配置

前言 1.roop是新开源了一个单图就可以进行视频换脸的项目&#xff0c;只需要一张所需面部的图像。不需要数据集&#xff0c;不需要训练。 2.大概的测试了一下&#xff0c;正脸换脸效果还不错&#xff0c;融合也比较自然。但如果人脸比较大&#xff0c;最终换出的效果可能会有…

source-map定位生产问题

source-map 定位源码错误位置 需要安装source-map库webpack配置需要配上devtool: “hidden-source-map”&#xff0c;devtool详细配置看这里devtool配置配置完webpack打包后&#xff0c;可以看到打包出来的.js.map文件 将生产包产生错误的栈赋值给stack即可&#xff0c;即设置…

前端——原生HTML猫猫max桌宠(附源码)

一、前言 看见了max大佬和狗头人大佬做的一个桌宠&#xff0c;于是就像用web简单实现一下 二、代码包 https://wwwf.lanzout.com/iWfER0ze0cqd密码:fg88 三、简单效果 简单用了随机动作&#xff08;可以进行权重设置&#xff09; 四、踩坑情况 如果不是主循环loop里&#xff0…

记录--巧用 overflow-scroll 实现丝滑轮播图

这里给大家分享我在网上总结出来的一些知识&#xff0c;希望对大家有所帮助 前言: 近期我在项目中就接到了一个完成轮播图组件的需求。最开始我也像大家一样&#xff0c;直接选择使用了知名的开源项目 "Swiper"&#xff0c;但是后来发现它在移动端项目中某些测试环境…

Go的性能优化建议

前言&#xff1a; \textcolor{Green}{前言&#xff1a;} 前言&#xff1a; &#x1f49e;这个专栏就专门来记录一下寒假参加的第五期字节跳动训练营 &#x1f49e;从这个专栏里面可以迅速获得Go的知识 Go的性能优化建议 3 性能优化建议3.1 性能优化建议 - Benchmark3.2 性能优化…

一文解答危废行业信息化平台的必要性——哲讯智能科技

危险废物物联网监管系统可以通过物联网技术实现对危险废物的精准管理&#xff0c;不仅有利于提高危险废物管理水平&#xff0c;而且能够有效防范和减少重特大事故的发生&#xff0c;保障人民群众生命财产安全。利用物联网技术&#xff0c;通过传感器、无线网络、移动终端等设备…

linux CentOS 7下载步骤

1、官网地址&#xff1a;https://www.centos.org/download/ 2、 3、下载阿里云 4、下载 或minimal - 2009

08-购物车效果

先做数据&#xff0c;再做功能&#xff0c;最后界面 var goods [{pic: ./assets/g1.png,title: 椰云拿铁,desc: 1人份【年度重磅&#xff0c;一口吞云】√原创椰云topping&#xff0c;绵密轻盈到飞起&#xff01;原创瑞幸椰云™工艺&#xff0c;使用椰浆代替常规奶盖打造丰盈…

【SAS】【01】【scsi协议族】SCSI standards family

下图显示了SAM协议与SCSI协议族中其他协议标准和相关项目的关系。 SCSI Architecture Model: 定义SCSI系统模型、SCSI标准集的功能划分以及适用于所有SCSI实现和要求。Device-Type Specific Command Sets: 定义特定设备类型的实现标准&#xff0c;包括每种设备类型的设备模型。…

leetcode1884. 鸡蛋掉落-两枚鸡蛋.动态规划-java

鸡蛋掉落-两枚鸡蛋 leetcode1884. 鸡蛋掉落-两枚鸡蛋题目描述解题思路代码演示 动态规划代码演示 动态规划专题 leetcode1884. 鸡蛋掉落-两枚鸡蛋 来源&#xff1a;力扣&#xff08;LeetCode&#xff09; 链接&#xff1a;https://leetcode.cn/problems/egg-drop-with-2-eggs-a…

基于Python和Spacy的命名实体识别

命名实体识别&#xff08;Named Entity Recognition&#xff0c;简称NER&#xff09;是一种自然语言处理&#xff08;NLP&#xff09;方法&#xff0c;用于检测和分类文本中的命名实体&#xff0c;包括人物、组织、地点、日期、数量和其他可识别的现实世界实体。 Spacy是一个基…

STM32之HAL库微妙延迟(借助Systick)

代码 void bsp_us_delay(uint32_t us) {uint32_t start, now, delta, reload, us_tick;start SysTick->VAL;reload SysTick->LOAD;us_tick SystemCoreClock / 1000000UL;do {now SysTick->VAL;delta start > now ? start - now : reload start - now;} whi…

Element el-dropdown 事件

我这里是结合el-table一起使用 设置trigger"click" 就可以加点击事件 这里我需要点击下拉选择值后&#xff0c;既要得到下拉里面的值 也要得到这一行数据的值 重要的代码 <el-dropdowntrigger"click"command"handleCommand($event,scope.row,sc…