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

news2025/1/20 15:45:18

上个教程将静态坐标系广播到 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 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()

代码解释：

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

定义启动节点对象

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.coloradmin.cn/o/1667576.html

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

相关文章

显卡、显卡驱动、CUDA、cuDNN、CUDA Toolkit、NVCC、nvidia-smi等概念的区别与联系

显卡、显卡驱动、CUDA、cuDNN、CUDA Toolkit、NVCC、nvidia-smi等概念的区别与联系

在科技日新月异的今天&#xff0c;显卡、显卡驱动、CUDA、cuDNN、CUDA Toolkit、NVCC、nvidia-smi等术语已经成为了科技领域的重要组成部分。本文旨在阐述这些术语之间的区别与联系&#xff0c;帮助您更好地理解它们在技术生态系统中的作用。 一、显卡 显卡&#xff0c;也称为…
阅读更多...
利用PS在不伤背景的前提下根据颜色去除图像上不想要的内容

利用PS在不伤背景的前提下根据颜色去除图像上不想要的内容

下面为一个例子&#xff0c;去除图像上红色的虚线 Step1.用套索工具框选带有颜色的部分 Step2.切换到魔术棒工具&#xff0c;上端选项中&#xff0c;点击与选区交叉&#xff0c;连续这一项不要勾选 Step3.在需要去除的部分点击一下即可在框选范围内选中所有同颜色的区域&#x…
阅读更多...
“二代”接班进行时：达利食品许阳阳揭秘“零食大王”成长密钥

“二代”接班进行时：达利食品许阳阳揭秘“零食大王”成长密钥

“二代接班”早已不是一个新鲜话题。近年来&#xff0c;随着时间的推移&#xff0c;那些伴随改革开放和中国制造崛起的民营企业&#xff0c;更多的正在经历或已完成“二代接班”。 “毛巾王子”家的洁丽雅&#xff0c;最近因大手笔签约多位代言人而引起讨论的九牧王&#xff0…
阅读更多...
用python写算法——队列笔记

用python写算法——队列笔记

1.队列定义 队列是一种特殊的线性表&#xff0c;它只允许在表的前端进行删除操作&#xff0c;在表的后端进行插入操作&#xff0c;和栈一样&#xff0c;队列是一种操作受限制的线性表。进行插入操作的端称为队尾&#xff0c;进行删除操作的端称为队头。队列中没有元素时&#…
阅读更多...
MATLAB的Bar3函数调节渐变色(内附渐变色库.mat及.m文件免费下载链接)

MATLAB的Bar3函数调节渐变色(内附渐变色库.mat及.m文件免费下载链接)

一. colormap函数 可以使用colormap函数&#xff1a; t1[281.1,584.6, 884.3,1182.9,1485.2; 291.6,592.6,896,1197.75,1497.33; 293.8,596.4,898.6,1204.4,1506.4; 295.8,598,904.4,1209.0,1514.6];bar3(t1,1) set(gca,XTickLabel,{300,600,900,1200,1500},FontSize,10) set…
阅读更多...
机器学习周记（第三十八周：语义分割）2024.5.6~2024.5.12

机器学习周记（第三十八周：语义分割）2024.5.6~2024.5.12

目录 摘要 ABSTRACT 1 DeeplabV3实现思路 预测部分 ①主干网络介绍​编辑 ② 加强特征提取结构 ③ 利用特征获得预测结果 摘要 本周继续了语义分割的学习&#xff0c;主要学习了DeepLabV3的部分实现思路&#xff0c;即DeepLabV3的整个模型的预测过程&#xff0c;并通过代…
阅读更多...
HCIP（BGP综合实验）--8

HCIP（BGP综合实验）--8

一&#xff1a;实验要求 二&#xff1a;实现过程 &#xff08;一&#xff09;配置IP地址&#xff1a; AR1: [AR1]int g0/0/0 [AR1-GigabitEthernet0/0/0]ip add 12.1.1.1 24 [AR1-GigabitEthernet0/0/0]int l0 [AR1-LoopBack0]ip add 172.16.0.1 32 [AR1-LoopBack0]int l1 […
阅读更多...
vscode怎么设置背景图片?

vscode怎么设置背景图片?

vscode背景图片是可以自己设置的&#xff0c;软件安装后默认背景的颜色是黑色的&#xff0c;这是默认的设计&#xff0c;如果要修改背景为指定的图片&#xff0c;那么我们需要安装插件&#xff0c;然后再通过代码来设置背景图片的样式&#xff0c;下面我们就来看看详细的教程。…
阅读更多...
【一步一步了解Java系列】：了解Java与C语言的运算符的“大同小异”

【一步一步了解Java系列】：了解Java与C语言的运算符的“大同小异”

看到这句话的时候证明&#xff1a;此刻你我都在努力~ 加油陌生人~ 个人主页&#xff1a; Gu Gu Study ​​ 专栏&#xff1a;一步一步了解Java 喜欢的一句话&#xff1a; 常常会回顾努力的自己&#xff0c;所以要为自己的努…
阅读更多...
VScode 修改 Markdown Preview Enhanced 主题与字体

VScode 修改 Markdown Preview Enhanced 主题与字体

VScode 修改 Markdown Preview Enhanced 主题与字体 1. 修改前后效果对比2. 修改主题2.1 更改默认主题2.2 修改背景色 3. 修改字体 VS Code基础入门使用可查看&#xff1a; VS Code 基础入门使用&#xff08;配置&#xff09;教程 其他Vs Code 配置可关注查看&#xff1a; Vs C…
阅读更多...
使用train.py----yolov7

使用train.py----yolov7

准备工作 在训练之前&#xff0c;数据集的工作和配置环境的工作要做好 数据集&#xff1a;看这里划分数据集&#xff0c;训练自己的数据集。_划分数据集后如何训练-CSDN博客 划分数据集2&#xff0c;详细说明-CSDN博客 配置环境看这里 从0开始配置环境-yolov7_gpu0是inter g…
阅读更多...
使用python撰写计算书

使用python撰写计算书

使用python撰写电路计算书 1、效果预览 下图是效果预览&#xff0c;可以写公式&#xff0c;画图&#xff0c;带单位计算 我们通常写计算书&#xff0c;使用mathcad或者maple等商业软件&#xff0c;但是个人使用可能还行&#xff0c;在很多公司是不允许使用破解版的。这时…
阅读更多...
Spring Boot 自动装配

Spring Boot 自动装配

本篇主要介绍Spring Boot 自动装配的相关内容。 目录 一、什么是自动装配 二、Bean的扫描方式 ComponentScan Import ImportSelector接口 三、Spring Boot自动装配原理 一、什么是自动装配 在我们在创建Spring Boot项目时往往会根据项目需求&#xff0c;引入很多第三方…
阅读更多...
分享一个处理大文件效率拉满的神器

分享一个处理大文件效率拉满的神器

&#x1f3c3;‍♂️ 微信公众号: 朕在debugger© 版权: 本文由【朕在debugger】原创、需要转载请联系博主&#x1f4d5; 如果文章对您有所帮助&#xff0c;欢迎关注、点赞、转发和订阅专栏&#xff01; 前言 系统当天有些表的数据需要恢复成前一天的样子&#xff0c;幸好有…
阅读更多...
Redis 的主从复制

Redis 的主从复制

Redis 的主从复制 1、主从复制的实现2、主从复制的同步功能(PSYNC)2.1、部分重同步 本文讲解的Redis 主从复制机制&#xff0c;是基于 2.8及以后的版本而言&#xff0c;2.8以前的版本主从复制机制与此有所不同&#xff0c;请知悉。 Redis的复制功能分为 同步 (psync) 和 命令传…
阅读更多...
HCIP 6（BGP综合实验）

HCIP 6（BGP综合实验）

一、实验拓扑 二、实验要求 1.AS1中存在两个环回&#xff0c;一个地址为192.168.1.0/24&#xff0c;该地址不能在任何协议中宣告&#xff1b;AS3中存在两个环回&#xff0c;一个地址为192.168.2.0/24&#xff0c;该地址不能在任何协议中宣告&#xff0c;最终要求这两个环回可以…
阅读更多...
bash tab 补全报错 bash: syntax error near unexpected token `(‘

bash tab 补全报错 bash: syntax error near unexpected token `(‘

使用 vim 编辑文件时&#xff0c;敲下 vim xxx 后&#xff0c;再键入 tab 键报进行补全报错 bash: syntax error near unexpected token (. 打开 bash 的命令执行详情 set -v 定位到具体的代码&#xff1a; 显然&#xff0c;代码位于 bash 补全的逻辑当中。 定位代码具体的…
阅读更多...
SpringCloud 2023.0.1

SpringCloud 2023.0.1

本文介绍如何使用 springboot3及cloud2023 进行微服务模块化开发 采用父-module 模块开发 父工程 demo-java pom.xml <!--配置 springboot的依赖的版本号, 方便 module 进行继承--><dependencyManagement><dependencies><!--增加 springboot的依赖--&g…
阅读更多...
亲测-wordpress文章实时同步发布修改删除多个站点的WP2WP插件

亲测-wordpress文章实时同步发布修改删除多个站点的WP2WP插件

一款将wordpress文章同步到其他WordPress网站的插件&#xff0c;通过这款插件&#xff0c;可以保持不同博客之间文章发布、修改、删除的同步。 安装步骤&#xff1a; 主站和分站都要上传这个插件 1.把插件上传到wp-content\plugins解压出来wp2wp文件夹&#xff0c;然后启用插…
阅读更多...
【强训笔记】day20

【强训笔记】day20

NO.1 思路&#xff1a;先判断能对砍几个回合&#xff0c;取最小值&#xff0c;因为回合数是整数&#xff0c;所以可能存在都大于0的情况&#xff0c;再判断一下如果都存活就再对砍一次&#xff0c;直到一家存活或者都死亡。 代码实现&#xff1a; #include<iostream>u…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023