机器人系统ros2-开发实践07-将机器人的状态广播到 tf2(Python)

embedded/2024/10/18 22:30:46/

上个教程将静态坐标系广播到 tf2，基于这个基础原理这个教程将演示机器人的点位状态发布到tf2

1. 写入广播节点

我们首先创建源文件。转到learning_tf2_py我们在上一教程中创建的包。在src/learning_tf2_py/learning_tf2_py目录中输入以下命令来下载示例广播示例代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

用vc code 打开源码文件

# Copyright 2021 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.import mathfrom geometry_msgs.msg import TransformStampedimport numpy as npimport rclpy
from rclpy.node import Nodefrom tf2_ros import TransformBroadcasterfrom turtlesim.msg import Pose# This function is a stripped down version of the code in
# https://github.com/matthew-brett/transforms3d/blob/f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/euler.py
# Besides simplifying it, this version also inverts the order to return x,y,z,w, which is
# the way that ROS prefers it.
def quaternion_from_euler(ai, aj, ak):ai /= 2.0aj /= 2.0ak /= 2.0ci = math.cos(ai)si = math.sin(ai)cj = math.cos(aj)sj = math.sin(aj)ck = math.cos(ak)sk = math.sin(ak)cc = ci*ckcs = ci*sksc = si*ckss = si*skq = np.empty((4, ))q[0] = cj*sc - sj*csq[1] = cj*ss + sj*ccq[2] = cj*cs - sj*scq[3] = cj*cc + sj*ssreturn qclass FramePublisher(Node):def __init__(self):super().__init__('turtle_tf2_frame_publisher')# Declare and acquire `turtlename` parameterself.turtlename = self.declare_parameter('turtlename', 'turtle').get_parameter_value().string_value# Initialize the transform broadcasterself.tf_broadcaster = TransformBroadcaster(self)# Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose# callback function on each messageself.subscription = self.create_subscription(Pose,f'/{self.turtlename}/pose',self.handle_turtle_pose,1)self.subscription  # prevent unused variable warningdef handle_turtle_pose(self, msg):t = TransformStamped()# Read message content and assign it to# corresponding tf variablest.header.stamp = self.get_clock().now().to_msg()t.header.frame_id = 'world't.child_frame_id = self.turtlename# Turtle only exists in 2D, thus we get x and y translation# coordinates from the message and set the z coordinate to 0t.transform.translation.x = msg.xt.transform.translation.y = msg.yt.transform.translation.z = 0.0# For the same reason, turtle can only rotate around one axis# and this why we set rotation in x and y to 0 and obtain# rotation in z axis from the messageq = quaternion_from_euler(0, 0, msg.theta)t.transform.rotation.x = q[0]t.transform.rotation.y = q[1]t.transform.rotation.z = q[2]t.transform.rotation.w = q[3]# Send the transformationself.tf_broadcaster.sendTransform(t)def main():rclpy.init()node = FramePublisher()try:rclpy.spin(node)except KeyboardInterrupt:passrclpy.shutdown()

代码解释：

def quaternion_from_euler(ai, aj, ak):ai /= 2.0aj /= 2.0ak /= 2.0ci = math.cos(ai)si = math.sin(ai)cj = math.cos(aj)sj = math.sin(aj)ck = math.cos(ak)sk = math.sin(ak)cc = ci*ckcs = ci*sksc = si*ckss = si*skq = np.empty((4, ))q[0] = cj*sc - sj*csq[1] = cj*ss + sj*ccq[2] = cj*cs - sj*scq[3] = cj*cc + sj*ssreturn q

这段 Python 代码的目的是将欧拉角转换为四元数表示。
欧拉角通常以滚转（roll）、俯仰（pitch）、偏航（yaw）的形式给出，也就是代码中的 (ai, aj, ak)。这个函数首先将这些角度除以2（假设输入的角度原本是以弧度为单位），然后使用三角函数计算四元数的组成部分。

这里逐步解释代码的每个部分：

  • 角度转换为弧度：
    假设输入的角度是以弧度为单位，首先将这些角度除以2。这一步是必要的，因为四元数的计算公式需要使用半角。
  • 计算半角的三角函数值：
    ci, si, cj, sj, ck, sk 分别是半角 ai, aj, ak 的余弦和正弦值。
  • 结合三角函数结果计算四元数分量：
    根据将欧拉角转换为四元数的特定公式进行组合，该公式考虑了旋转轴的顺序。结果是一个四元数 [q0, q1, q2, q3]，其中 q0 是标量部分，[q1, q2, q3] 是向量部分。
  • 返回四元数：
    返回的四元数以 numpy 数组的形式，适用于 Python 中的数值计算。
self.turtlename = self.declare_parameter('turtlename', 'turtle').get_parameter_value().string_value

定义并获取一个参数turtlename，它指定一个海龟名称，例如turtle1或turtle2。就是定义一个机器人对象

之后，节点订阅主题并对每条传入消息turtleX/pose运行函数。handle_turtle_pose

self .subscription = self.create_subscription(Pose,f'/{self.turtlename}/pose',self.handle_turtle_pose,1)

现在，我们创建一个TransformStamped对象并为其提供适当的元数据。

  1. 我们需要为正在发布的转换提供一个时间戳，并且我们只需通过调用 来用当前时间来标记它self.get_clock().now()。这将返回 所使用的当前时间Node。

  2. 然后我们需要设置我们正在创建的链接的父框架的名称，在本例中为world。

  3. 最后，我们需要设置我们正在创建的链接的子节点的名称，在本例中这是海龟本身的名称。

海龟姿势消息的处理程序函数广播该海龟的平移和旋转，并将其作为帧world到帧的变换发布turtleX。

t = TransformStamped()# Read message content and assign it to
# corresponding tf variables
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = self.turtlename

这段代码是在处理与图形界面中的“乌龟”，它使用了欧拉角到四元数的转换来处理乌龟的旋转，同时也设置了乌龟在2D空间中的位置。以下是详细解释：

设置位置坐标：
t.transform.translation.x = msg.x 和 t.transform.translation.y = msg.y：这两行代码从消息中获取乌龟在x和y轴上的位置坐标，并将其赋给变换的位置属性。
t.transform.translation.z = 0.0：由于乌龟仅存在于2D平面中，z轴的坐标设置为0。
设置旋转：
因为乌龟只能在2D空间中绕z轴旋转（即仅围绕一个轴旋转），因此旋转在x轴和y轴的分量需要设置为0。
q = quaternion_from_euler(0, 0, msg.theta)：这行代码调用之前提到的函数，将欧拉角转换为四元数。由于乌龟只在一个平面上旋转，所以x和y的旋转角度为0，而z轴的旋转角度从消息中获取。
t.transform.rotation.x = q[0]、t.transform.rotation.y = q[1]、t.transform.rotation.z = q[2] 和 t.transform.rotation.w = q[3]：这些行将计算出的四元数分量赋值给旋转的相应属性。

# Turtle only exists in 2D, thus we get x and y translation
# coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg.x
t.transform.translation.y = msg.y
t.transform.translation.z = 0.0# For the same reason, turtle can only rotate around one axis
# and this why we set rotation in x and y to 0 and obtain
# rotation in z axis from the message
q = quaternion_from_euler(0, 0, msg.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]

最后，我们将构建的转换传递给负责广播sendTransform

# Send the transformation
self.tf_broadcaster.sendTransform(t)

1.2 添加入口点

要允许命令运行您的节点，您必须将入口点添加到（位于目录中）。
更改
/home/yhg/ros2_study/src/learning_tf2_py/setup.py

在括号之间添加以下行’console_scripts’:：

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

在这里插入图片描述

2. 编写启动文件

现在为此演示创建一个启动文件。在 learning_tf2_py下新增launch 文件夹，在launch 文件夹创建 turtle_tf2_demo.launch.py 文件

代码如下：

from launch import LaunchDescription
from launch_ros.actions import Nodedef generate_launch_description():return LaunchDescription([Node(package='turtlesim',executable='turtlesim_node',name='sim'),Node(package='learning_tf2_py',executable='turtle_tf2_broadcaster',name='broadcaster1',parameters=[{'turtlename': 'turtle1'}]),])

首先我们从和包中导入所需的模块launch_ros。应该注意的是，这launch是一个通用的启动框架（不是 ROS 2 特定的），并且launch_ros具有 ROS 2 特定的东西，比如我们在这里导入的节点。

现在我们运行节点来启动turtlesim 模拟并使用我们的节点将turtle1状态广播到tf2 。turtle_tf2_broadcaster

定义启动节点对象

Node(package='turtlesim',executable='turtlesim_node',name='sim'
),
Node(package='learning_tf2_py',executable='turtle_tf2_broadcaster',name='broadcaster1',parameters=[{'turtlename': 'turtle1'}]
),

2.2 添加依赖

导航回到learning_tf2_py目录 ，新增

package.xml使用文本编辑器打开。添加与启动文件的导入语句相对应的以下依赖项

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

在这里插入图片描述

这在执行其代码时声明了额外的必需launch和launch_ros依赖项

2.3 更新setup.py

  1. 在文件顶部添加依赖
import os
from glob import glob
  1. 在data_files 中加入
 (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),

在这里插入图片描述

3.构建

在工作区的根目录中运行rosdep以检查是否缺少依赖项。

rosdep install -i --from-path src --rosdistro humble -y

运行结果如下：在这里插入图片描述

仍然在工作区的根目录中构建您的包：

colcon build --packages-select learning_tf2_py

打开一个新终端，导航到工作区的根目录，然后获取安装文件：

. install/setup.bash

4 运行

现在运行启动文件，将启动turtlesim模拟节点和turtle_tf2_broadcaster节点：

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

在第二个终端窗口中键入以下命令：

ros2 run turtlesim turtle_teleop_key

在这里插入图片描述

现在，使用该tf2_echo工具检查海龟姿势是否确实广播到 tf2：

在这里插入图片描述
控制小乌龟移动可观察到上面数字的变动


http://www.ppmy.cn/embedded/39749.html

相关文章

Android ViewFlipper

Android ViewFlipper

Android ViewFlipper 在很多APP都有如下的头条/热榜效果&#xff08;上下自动翻滚&#xff09; 这种效果可以使用很多方式实现&#xff0c;有一个简便的方式可以使用ViewFlipper控件实现&#xff0c;ViewFlipper控件继承结果如下&#xff1a; 可以看出ViewFlipper 继承自ViewA…
阅读更多...
TEINet: Towards an Efficient Architecture for Video Recognition 论文阅读

TEINet: Towards an Efficient Architecture for Video Recognition 论文阅读

TEINet: Towards an Efficient Architecture for Video Recognition 论文阅读 Abstract1 Introduction2 Related Work3 Method3.1 Motion Enhanced Module3.2 Temporal Interaction Module3.3 TEINet 4 Experiments5 Conclusion阅读总结 文章信息; 原文链接&#xff1a;https:…
阅读更多...
linux安装配置Docker保姆级教程

linux安装配置Docker保姆级教程

Docker到底是什么? Docker 是一个开源的应用容器引擎&#xff0c;让开发者可以打包他们的应用以及依赖包到一个可移植的镜像中&#xff0c;然后发布到任何流行的 Linux或Windows操作系统的机器上&#xff0c;也可以实现虚拟化。 容器是完全使用沙箱机制&#xff0c;相互之间…
阅读更多...
Linux\_c输出

Linux\_c输出

第一条Linux_c输出 初界面 : ls # 显示目录下的文件cd # 进入到某个目录 # 比如 我进入了Codels # 发现没有显示, 说明为文件下为空vim cpucdoe.c # 创建一个 .c的源码文件进入到了vim的编辑界面: i # 按i 就可以进行编辑 , 下面显示插入标识在编辑模式下, 可以通…
阅读更多...
掌握SQL的时间序列分析利器：LEAD与LAG函数详解

掌握SQL的时间序列分析利器：LEAD与LAG函数详解

在SQL中&#xff0c;处理时间序列数据时&#xff0c;经常需要查看当前行与相邻行之间的关系。这时&#xff0c;LEAD和LAG两个窗口函数就显得尤为重要。它们允许我们访问结果集中的前一行或后一行的数据&#xff0c;为数据分析和处理提供了极大的便利。本文将详细介绍LEAD和LAG函…
阅读更多...
微搭低代码入门04数据模型

微搭低代码入门04数据模型

目录 1 创建数据模型2 一对多3 通用选项集4 API总结 上一篇我们介绍了页面管理&#xff0c;页面是盛放组件的容器&#xff0c;组件在配置属性的时候需要进行数据绑定。数据是通过创建数据模型来进行存储&#xff0c;本篇我们介绍一下数据模型的相关操作。 1 创建数据模型 微搭…
阅读更多...
行业新应用：电机驱动将成为机器人的动力核心

行业新应用：电机驱动将成为机器人的动力核心

电机已经遍布当今社会人们生活的方方面面&#xff0c;不仅应用范围越来越广&#xff0c;更新换代的速度也日益加快。按照工作电源分类&#xff0c;可以将它划分为直流电机和交流电机两大类型。直流电机中&#xff0c;按照线圈类型分类&#xff0c;又可以分为有铁芯的电机、空心…
阅读更多...
【xxl-job | 第三篇】SpringBoot整合xxl-job

【xxl-job | 第三篇】SpringBoot整合xxl-job

文章目录 3.SpringBoot整合xxl-job3.1定时任务服务配置3.1.1导入maven依赖3.1.2yml配置3.1.3XxlJobConfig配置类3.1.4定时任务类 3.2xxl-job配置3.2.1新增执行器3.2.2新增任务3.2.3执行任务3.2.4查看日志3.2.5查看任务后台日志 3.3小结 3.SpringBoot整合xxl-job 3.1定时任务服…
阅读更多...
最新文章

Copyright @ 2022~2023