ros2笔记- 5.2 python 中手眼坐标转换

server/2025/1/12 5:47:43/

本节继续跟小鱼老师学习5.2.先看下需求

相机固定在右上方的camera_link处，机械臂的底座固定在base_link处从base_link到camera_link的位置是固定不变的，瓶子可能是变的。求base_link到bottle_link的坐标关系，方便控制机械臂抓取瓶子。

5.2.1 从机械臂底座到相机|静态TF发布

TF发布时，旋转使用四元组，需求是欧拉角，涉及欧拉角到四元组转换。先安装依赖

bohu@bohu-TM1701:~$ sudo apt install ros-$ROS_DISTRO-tf-transformations

在chat5/chap5_ws/src新建功能包demo_python_tf

ros2 pkg create demo_python_tf --build-type ament_python --dependencies  rclpy geometry_msgs tf_ros tf_transformations --license Apache-2.0

功能包下src/demo_python_tf/demo_python_tf新建文件：static_tf_boadcaster.py

python">import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster #静态坐标发布器
from geometry_msgs.msg import TransformStamped #消息接口
from tf_transformations import quaternion_from_euler #欧拉角转四元函数
import math  #角度转弧度函数class StaticTFBroadcaster(Node):def __init__(self):super().__init__('static_tf2_broadcaster')self.static_broadcaster_ = StaticTransformBroadcaster(self)self.publish_static_tf()def publish_static_tf(self):#发布静态TF 从base_link 到camera_link 之间的坐标关系transform = TransformStamped()transform.header.frame_id = 'base_link'transform.header.stamp = self.get_clock().now().to_msg()transform.child_frame_id = 'camera_link'transform.transform.translation.x =0.5transform.transform.translation.y =0.3transform.transform.translation.z =0.6#欧拉角转换四元数，math.radians 把角度值转为弧度值q = quaternion_from_euler(math.radians(180),0,0)transform.transform.rotation.x = q[0]transform.transform.rotation.y = q[1]transform.transform.rotation.z = q[2]transform.transform.rotation.w = q[3]#发布静态坐标转换self.static_broadcaster_.sendTransform(transform)self.get_logger().info(f'发布TF:{transform}')def main():rclpy.init()node = StaticTFBroadcaster()rclpy.spin(node)rclpy.shutdown()

在setup.py注册节点，接着构建并运行节点

bohu@bohu-TM1701:~/chapt5/chapt5_ws$ source install/setup.bash 
bohu@bohu-TM1701:~/chapt5/chapt5_ws$ ros2 run demo_python_tf static_tf_broadcaster
[INFO] [1736503566.890079131] [static_tf2_broadcaster]: 发布TF:geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1736503566, nanosec=863857715), frame_id='base_link'), child_frame_id='camera_link', transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.5, y=0.3, z=0.6), rotation=geometry_msgs.msg.Quaternion(x=1.0, y=0.0, z=0.0, w=6.123233995736766e-17)))

新开终端，使用tf2_echo查看base_link和camera_link之间的关系。

bohu@bohu-TM1701:~$ ros2 topic echo /tf_static 
transforms:
- header:stamp:sec: 1736494013nanosec: 735445405frame_id: base_laserchild_frame_id: wall_pointtransform:translation:x: 0.3y: 0.0z: 0.0rotation:x: 0.0y: 0.0z: 0.0w: 1.0
---
transforms:
- header:stamp:sec: 1736503566nanosec: 863857715frame_id: base_linkchild_frame_id: camera_linktransform:translation:x: 0.5y: 0.3z: 0.6rotation:x: 1.0y: 0.0z: 0.0w: 6.123233995736766e-17
---
transforms:
- header:stamp:sec: 1736493993nanosec: 89597686frame_id: base_linkchild_frame_id: base_lasertransform:translation:x: 0.1y: 0.0z: 0.2rotation:x: 0.0y: 0.0z: 0.0w: 1.0
---

bohu@bohu-TM1701:~$ ros2 run tf2_ros tf2_echo base_link camera_link  
[INFO] [1736505274.209950428] [tf2_echo]: Waiting for transform base_link ->  camera_link: Invalid frame ID "base_link" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [0.500, 0.300, 0.600]
- Rotation: in Quaternion [1.000, 0.000, 0.000, 0.000]
- Rotation: in RPY (radian) [3.142, -0.000, 0.000]
- Rotation: in RPY (degree) [180.000, -0.000, 0.000]
- Matrix:1.000  0.000  0.000  0.5000.000 -1.000 -0.000  0.3000.000  0.000 -1.000  0.6000.000  0.000  0.000  1.000

5.2.2 python 动态TF发布

功能包下src/demo_python_tf/demo_python_tf新建文件dynamic_tf_boradcaster.py

python">import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster #静态坐标发布器
from geometry_msgs.msg import TransformStamped #消息接口
from tf_transformations import quaternion_from_euler #欧拉角转四元函数
import math  #角度转弧度函数class DynamicTFBroadcaster(Node):def __init__(self):super().__init__('tf2_broadcaster')self.broadcaster_ = TransformBroadcaster(self)self.timer_ = self.create_timer(0.01,self.publish_tf)def publish_tf(self):#发布TF 从 camera_link 到 bottle_link 之间的坐标关系transform = TransformStamped()transform.header.frame_id = 'camera_link'transform.header.stamp = self.get_clock().now().to_msg()transform.child_frame_id = 'bottle_link'transform.transform.translation.x =0.2transform.transform.translation.y =0.3transform.transform.translation.z =0.5#欧拉角转换四元数，math.radians 把角度值转为弧度值q = quaternion_from_euler(0,0,0)transform.transform.rotation.x = q[0]transform.transform.rotation.y = q[1]transform.transform.rotation.z = q[2]transform.transform.rotation.w = q[3]#发布静态坐标转换self.broadcaster_.sendTransform(transform)self.get_logger().info(f'发布TF:{transform}')def main():rclpy.init()node = DynamicTFBroadcaster()rclpy.spin(node)rclpy.shutdown()

代码逻辑跟静态类似，换了用定时器去定期调用发布。

再setup.py注册节点dynamic_tf_boradcaster

构建并运行ros2 run demo_python_tf dynamic_tf_boradcaster

[INFO] [1736507040.648139069] [tf2_broadcaster]: 发布TF:geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1736507040, nanosec=646839153), frame_id='camera_link'), child_frame_id='bottle_link', transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.2, y=0.3, z=0.5), rotation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)))
[INFO] [1736507040.657711192] [tf2_broadcaster]: 发布TF:geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1736507040, nanosec=656823767), frame_id='camera_link'), child_frame_id='bottle_link', transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.2, y=0.3, z=0.5), rotation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)))

打开新的终端，使用tf2_echo查看camera_link和bottle_link之间的关系。

bohu@bohu-TM1701:~/chapt5/chapt5_ws$ ros2 run tf2_ros tf2_echo camera_link bottle_link
[INFO] [1736506996.731929593] [tf2_echo]: Waiting for transform camera_link ->  bottle_link: Invalid frame ID "camera_link" passed to canTransform argument target_frame - frame does not exist
At time 1736506997.686809033
- Translation: [0.200, 0.300, 0.500]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
  1.000  0.000  0.000  0.200
  0.000  1.000  0.000  0.300
  0.000  0.000  1.000  0.500
  0.000  0.000  0.000  1.000
At time 1736506998.686777901
- Translation: [0.200, 0.300, 0.500]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
  1.000  0.000  0.000  0.200
  0.000  1.000  0.000  0.300
  0.000  0.000  1.000  0.500
  0.000  0.000  0.000  1.000
。。。

对比下静态发布，动态发布的At time 是有数值的，静态的是0.0

5.2.3 通过python 查询TF关系

至此，base_link和camera_link之间静态发布、camera_link和bottle_link之间动态发布已完成，接下来需要查询base_link到bottle_link的关系。

功能包下src/demo_python_tf/demo_python_tf新建文件tf_listener.py

python">import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener, Buffer
from tf_transformations import euler_from_quaternionclass TFListener(Node):def __init__(self):super().__init__("tf2_listener")self.buffer_ = Buffer()self.listner_ = TransformListener(self.buffer_,self)self.timer_ = self.create_timer(1,self.get_transfrom)def get_transfrom(self):try:#查询缓冲区最近的数据中，base_link 到bottle_link的TF关系result = self.buffer_.lookup_transform('base_link','bottle_link',rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))transform = result.transformself.get_logger().info(f'平移:{transform.translation}')self.get_logger().info(f'旋转:{transform.rotation}')ratation_euler = euler_from_quaternion([transform.rotation.x,transform.rotation.y,transform.rotation.z,transform.rotation.w])self.get_logger().info(f'平移rpy:{ratation_euler}')except Exception as e:self.get_logger().warn(f'获取坐标转换异常{str(e)}')def main():rclpy.init()node = TFListener()rclpy.spin(node)rclpy.shutdown()

TFListener 类中初始化方法中，创建了TransformListener类型监听器，监听器会订阅/tf 和/tf_static 话题，然后将收到数据放入到buffer_.最后创建定时器，每秒调用get_transfrom查询TF转换。

setup.py添加节点tf_listener。构建，先后启动

ros2 run demo_python_tf dynamic_tf_boradcaster

ros2 run demo_python_tf static_tf_broadcaster

启动查询节点

bohu@bohu-TM1701:~/chapt5/chapt5_ws$ ros2 run demo_python_tf tf_listener
[INFO] [1736510287.179842369] [tf2_listener]: 平移:geometry_msgs.msg.Vector3(x=0.7, y=-1.1102230246251565e-16, z=0.09999999999999998)
[INFO] [1736510287.180704164] [tf2_listener]: 旋转:geometry_msgs.msg.Quaternion(x=1.0, y=0.0, z=0.0, w=6.123233995736766e-17)
[INFO] [1736510287.181605758] [tf2_listener]: 平移rpy:(3.141592653589793, -0.0, 0.0)
[INFO] [1736510288.121728923] [tf2_listener]: 平移:geometry_msgs.msg.Vector3(x=0.7, y=-1.1102230246251565e-16, z=0.09999999999999998)


http://www.ppmy.cn/server/157677.html

相关文章

【Web安全】SQL 注入攻击技巧详解：UNION 注入（UNION SQL Injection）

【Web安全】SQL 注入攻击技巧详解：UNION 注入（UNION SQL Injection）

【Web安全】SQL 注入攻击技巧详解：UNION 注入（UNION SQL Injection） 引言 UNION注入是一种利用SQL的UNION操作符进行注入攻击的技术。攻击者通过合并两个或多个SELECT语句的结果集，可以获取数据库中未授权的数据。这种注入技术要…
阅读更多...
高斯函数Gaussian绘制matlab

高斯函数Gaussian绘制matlab

高斯 约翰卡尔弗里德里希高斯，（德语：Johann Carl Friedrich Gau，英语：Gauss，拉丁语：Carolus Fridericus Gauss）1777年4月30日–1855年2月23日，德国著名数学家、物理学家…
阅读更多...
简单的spring boot tomcat版本升级

简单的spring boot tomcat版本升级

简单的spring boot tomcat版本升级 1. 需求 我们使用的springboot版本为2.3.8.RELEASE，对应的tomcat版本为9.0.41，公司tomcat对应版本发现攻击者可发送不完整的POST请求触发错误响应，从而可能导致获取其他用户先前请求的数据，造…
阅读更多...
原生JavaScript实现文本内容的文字数量变化，适配容器宽度和高度，文本内容文字字体的字号大小自动缩小，保障文字全部都在容器内显示完

原生JavaScript实现文本内容的文字数量变化，适配容器宽度和高度，文本内容文字字体的字号大小自动缩小，保障文字全部都在容器内显示完

<!DOCTYPE html> <html lang"en"><head><meta charset"UTF-8"><meta name"viewport" content"widthdevice-width, initial-scale1.0"><title>文字自动填充容器</title><style>body {…
阅读更多...
Python对象的序列化和反序列化工具：Joblib与Pickle

Python对象的序列化和反序列化工具：Joblib与Pickle

在Python中&#xff0c;序列化是将内存中的对象转换为可存储或传输的格式的过程。常见的序列化格式有JSON、YAML、Pickle和Joblib等。其中&#xff0c;Pickle和Joblib是最常用的用于序列化和反序列化Python对象的工具。虽然这两者有很多相似之处&#xff0c;但它们在某些方面有…
阅读更多...
【react进阶】create-react-app高阶配置

【react进阶】create-react-app高阶配置

create-react-app新建项目还是官网推荐的主流方法&#xff0c;当然vite已经使用越来越广泛了&#xff0c;构建速度也是比cra快几倍。记录下怎么用cra来搭建一个react项目。 在index.html文件中使用变量 1.在script代码中的使用方式 <script>const $ window.$ "…
阅读更多...
面试：类模版中函数声明在.h，定义在.cpp中，其他cpp引用引入这个头文件，会有什么错误?

面试：类模版中函数声明在.h，定义在.cpp中，其他cpp引用引入这个头文件，会有什么错误?

1、概述 类模版中函数声明在.h&#xff0c;定义在.cpp中&#xff0c;其他cpp引用引入这个头文件&#xff0c;会有什么错误?报编译错误&#xff1a;error C2512: Demo<int>: no appropriate default constructor available 举例如下代码&#xff1a;demo.h 声明模版类 …
阅读更多...
K-means算法在无监督学习中的应用

K-means算法在无监督学习中的应用

K-means算法在无监督学习中的应用 K-means算法是一种典型的无监督学习算法&#xff0c;广泛用于聚类分析。在无监督学习中&#xff0c;模型并不依赖于标签数据&#xff0c;而是根据输入数据的特征进行分组。K-means的目标是将数据集分成K个簇&#xff0c;使得同一簇内的数据点…
阅读更多...
最新文章

Copyright @ 2022~2023