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

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

1. 写入广播节点

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

cpp 复制代码
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

用vc code 打开源码文件

cpp 复制代码
# 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 math

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

from 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.0
    aj /= 2.0
    ak /= 2.0
    ci = 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*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


class FramePublisher(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')

        # Declare and acquire `turtlename` parameter
        self.turtlename = self.declare_parameter(
            'turtlename', 'turtle').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.tf_broadcaster = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)
        self.subscription  # prevent unused variable warning

    def handle_turtle_pose(self, msg):
        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

        # 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]

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


def main():
    rclpy.init()
    node = FramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

代码解释:

cpp 复制代码
def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = 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*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return 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 中的数值计算。

cpp 复制代码
self.turtlename = self.declare_parameter(
  'turtlename', 'turtle').get_parameter_value().string_value

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


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

cpp 复制代码
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。

cpp 复制代码
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]:这些行将计算出的四元数分量赋值给旋转的相应属性。

cpp 复制代码
# 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

cpp 复制代码
# Send the transformation
self.tf_broadcaster.sendTransform(t)

1.2 添加入口点

要允许命令运行您的节点,您必须将入口点添加到(位于目录中)。

更改

/home/yhg/ros2_study/src/learning_tf2_py/setup.py

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

cpp 复制代码
'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

2. 编写启动文件

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

代码如下:

cpp 复制代码
from launch import LaunchDescription
from launch_ros.actions import Node


def 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

定义启动节点对象

cpp 复制代码
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使用文本编辑器打开。添加与启动文件的导入语句相对应的以下依赖项

cpp 复制代码
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

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

2.3 更新setup.py

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

3.构建

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

cpp 复制代码
rosdep install -i --from-path src --rosdistro humble -y

运行结果如下:

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

cpp 复制代码
colcon build --packages-select learning_tf2_py

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

cpp 复制代码
. install/setup.bash

4 运行

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

cpp 复制代码
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

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

cpp 复制代码
ros2 run turtlesim turtle_teleop_key

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

控制小乌龟移动可观察到上面数字的变动

相关推荐
清安无别事4 分钟前
闲聊?泳池清洁机器人?
机器人
zhd15306915625ff5 分钟前
库卡机器人维护需要注意哪些事项
安全·机器人·自动化
baiduopenmap6 分钟前
百度世界2024精选公开课:基于地图智能体的导航出行AI应用创新实践
前端·人工智能·百度地图
小任同学Alex9 分钟前
浦语提示词工程实践(LangGPT版,服务器上部署internlm2-chat-1_8b,踩坑很多才完成的详细教程,)
人工智能·自然语言处理·大模型
新加坡内哥谈技术15 分钟前
微软 Ignite 2024 大会
人工智能
nuclear201128 分钟前
使用Python 在Excel中创建和取消数据分组 - 详解
python·excel数据分组·创建excel分组·excel分类汇总·excel嵌套分组·excel大纲级别·取消excel分组
江瀚视野42 分钟前
Q3净利增长超预期,文心大模型调用量大增,百度未来如何分析?
人工智能
Lucky小小吴43 分钟前
有关django、python版本、sqlite3版本冲突问题
python·django·sqlite
陪学1 小时前
百度遭初创企业指控抄袭,维权还是碰瓷?
人工智能·百度·面试·职场和发展·产品运营
QCN_1 小时前
湘潭大学人工智能考试复习1(软件工程)
人工智能