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

在这里插入图片描述
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 xfloat64 yfloat64 zgeometry_msgs/Quaternion rotation #四元数(欧拉角)float64 xfloat64 yfloat64 zfloat64 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 yfloat64 z

可以看出 PointStamped消息是由:
std_msgs/Header与geometry_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.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 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()# 监听坐标变换存入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_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 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.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 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 控制乌龟进行跟随运动

总体逻辑：

  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:passrospy.logwarn("错误提示%s",ee)rate.sleep()

在这里插入图片描述

4.3查看当前坐标关系

rosrun tf2_tools view_frames.py

在这里插入图片描述

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

如若内容造成侵权/违法违规/事实不符，请联系编程知识网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

【InnoDB 存储引擎】InnoDB 存储引擎的行格式，有 Compact、Redundant、Dynamic 等行格式还有它们配套实验（实验篇）

【InnoDB 存储引擎】InnoDB 存储引擎的行格式，有 Compact、Redundant、Dynamic 等行格式还有它们配套实验（实验篇）

文章目录 1 InnoDB 行记录格式&#xff08;实验&#xff09;1.1 Compact 行格式实验1.1.1 实验步骤1.1.2 分析捞出来的数据1.1.3 疑问 1.2 Redundant 行格式实验1.2.1 实验步骤1.2.2 分析捞出来的数据 1.3 CHAR 列类型的存储1.3.1 实验步骤1.3.2 分析捞出来的数据 2 参考资料 1…
阅读更多...
pnpm装包报错Run “pnpm install“ to recreate the modules directory.

pnpm装包报错Run “pnpm install“ to recreate the modules directory.

一、先看报错 执行 pnpm install 装包报错 报错信息 ERR_PNPM_PUBLIC_HOIST_PATTERN_DIFF  This modules directory was created using a different public-hoist-pattern value. Run "pnpm install" to recreate the modules directory.二、解决方法 在项目根目录中…
阅读更多...
【Zabbix 监控设置】

【Zabbix 监控设置】

目录 一、添加 zbx-agent01 客户端主机1、服务端和客户端都配置时间同步2、服务端和客户端都设置 hosts 解析3、设置 zabbix 的下载源&#xff0c;安装 zabbix-agent24、修改 agent2 配置文件5、启动 zabbix-agent26、在服务端验证 zabbix-agent2 的连通性1、常用的键值 7、在 …
阅读更多...
基于云原生网关的全链路灰度实践

基于云原生网关的全链路灰度实践

作者&#xff1a; 倪海峰&#xff08;海迩&#xff09; 前言 随着企业规模的不断扩大&#xff0c;传统单体应用已很难进一步支持业务的发展&#xff0c;业务的迭代速度已经难以满足业务的增长&#xff0c;此时企业会对应用系统做微服务化的改造&#xff0c;降低业务的耦合度&…
阅读更多...
Opencv图片样本预处理

Opencv图片样本预处理

前因 最近想学学&#xff0c;OPENCV识别物体&#xff0c;但是处理图片正样本时过于繁琐&#xff0c; 遂自己开发了工具&#xff0c;来处理样本图片&#xff0c;基于QT&#xff0c;文末附下载链接 程序 功能 一共有两个功能&#xff0c;一个是处理负样本&#xff0c;一个处理…
阅读更多...
Spring Boot源码解读与原理分析（一）：项目启动流程（上）——@SpringBootApplication

Spring Boot源码解读与原理分析（一）：项目启动流程（上）——@SpringBootApplication

文章目录 〇、准备工作一、SpringBootApplication.java源码解析1.源码2.自定义注解3.组合注解4.注解ComponentScan过滤器 5.注解SpringBootConfigurationConfiguration 6.注解EnableAutoConfiguration 本文章是Spring Boot源码解读与原理分析系列博客的第一篇&#xff0c;将会介…
阅读更多...
【js小案例】视频倍数播放、计算机、待办事项管理

【js小案例】视频倍数播放、计算机、待办事项管理

视频倍数播放示例图&#xff1a; 视频倍数播放代码&#xff1a; <!DOCTYPE html> <html> <head><meta charset"UTF-8"><title>控制视频播放速度</title> </head> <body><video id"myVideo" width&quo…
阅读更多...
10.1UEC++/UObject/UClass/UFunction/UPropret

10.1UEC++/UObject/UClass/UFunction/UPropret

1. 1.new出的对象&#xff0c;不用自己管理内存释放&#xff1b; 2.比如两个类对象指针a,b同时指向一个苹果&#xff0c;若苹果消亡&#xff0c;会将a,b同时指向空。 3.保存时&#xff0c;将工程中的actor&#xff0c;属性等能够保存在本地就是uob在发挥作用。 4.不管在ue&…
阅读更多...
机器学习25：《数据准备和特征工程-III》采样和分隔

机器学习25：《数据准备和特征工程-III》采样和分隔

目录 1.采样和分割数据 1.1 抽样简介 1.2 过滤 PII&#xff08;个人身份信息&#xff09; 2.数据不平衡 2.1 下采样和增加权重 3.数据分割示例 3.1 随机分割可能不是最好的方法 4.分割数据 5.随机化 5.1 实际考虑 5.2 散列的注意事项 6.参考文献 1.采样和分割数据-…
阅读更多...
时间序列预测 | Matlab自回归差分移动平均模型ARIMA时间序列预测

时间序列预测 | Matlab自回归差分移动平均模型ARIMA时间序列预测

文章目录 效果一览文章概述部分源码参考资料效果一览 文章概述 时间序列预测 | Matlab自回归差分移动平均模型ARIMA时间序列预测，单列数据输入模型 评价指标包括:MAE、RMSE和R2等，代码质量极高，方便学习和替换数据。要求2018版本及以上。 部分源码 %% 清空环境变量 warnin…
阅读更多...
Linux开发工具之【vim】

Linux开发工具之【vim】

Linux开发工具之【vim】 文章目录&#xff1a; Linux开发工具之【vim】1. Linux软件包管理器yum1.1 查看软件1.2. 下载软件1.3 卸载软件 2. vim编辑器的使用2.1 vim常用模式2.2 vim基本操作2.3 vim命令模式命令集2.3.1 移动光标2.3.2 删除文字2.3.3 复制文本内容2.3.4 替换文本…
阅读更多...
UE特效案例 —— 魔法翅膀

UE特效案例 —— 魔法翅膀

一&#xff0c;环境配置 创建默认地形Landscape&#xff0c;如给地形上材质需确定比例&#xff1b;添加环境主光源DirectionalLight&#xff0c;设置相应的强度和颜色&#xff1b;PostProcessVolume设置曝光&#xff0c;设置Min/Max Brightness为1&#xff1b; 与关闭Game Sett…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023