机器人系统ros2-开发实践06-将静态坐标系广播到 tf2(Python)-定义机器人底座与其传感器或非移动部件之间的关系

发布静态变换对于定义机器人底座与其传感器或非移动部件之间的关系非常有用。例如，最容易推断激光扫描仪中心框架中的激光扫描测量结果。

1. 创建包

首先，我们将创建一个用于本教程和后续教程的包。调用的包learning_tf2_py将依赖于geometry_msgs、python3-numpy、rclpy、tf2_ros_py和turtlesim

cd ros2_study/src

ros2 pkg create --build-type ament_python --license Apache-2.0 -- learning_tf2_py

learning_tf2_py您的终端将返回一条消息，验证您的包及其所有必需文件和文件夹的创建。

2 编写静态广播节点

我们首先创建源文件。在src/learning_tf2_py/learning_tf2_py目录中输入以下命令来下载示例静态广播器代码：

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

这个有点慢，可能要等一会才下好

下载后可查看：

static_turtle_tf2_broadcaster.py 代码内容如下：

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster


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 StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame