ROS2——基础6（TF2机器人坐标系管理器、Gazebo）

目录

[1 TF2简介](#1 TF2简介)

[2 TF命令行测试](#2 TF命令行测试)

[3 静态TF广播](#3 静态TF广播)

[4 TF监听](#4 TF监听)

---

1 TF2机器人坐标系管理器

1.1 TF2简介

机器人系统中有很多坐标系，机器人中心点设为基坐标系base link、雷达所在位置为雷达坐标系laser link、机器人移动里程计累积位置的参考系称为里程计坐标系odom link、里程计有累积误差和飘逸又有绝对位置参考系/地图坐标系map link。TF2在ROS中用于管理机器人的坐标系。

1.2 TF命令行测试

sudp apt install ros-humble-turtle-tf2-py
ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

另启终端

ros2 run turtlesim turtle_teleop_key

另启终端，查看turtle1和2两个坐标系之间的具体关系

ros2 run tf2_ros tf2_echo turtle2 turtle1

那么这个工程包是怎么构建的呢，详情如下。

1.3 静态TF广播

（1）创建static_tf_broadcaster.py

安装tf工具-创建tf_demo功能包并进入-编写程序

sudo apt install ros-humble-tf2 \
                 ros-humble-tf2-tools \
                 ros-humble-tf2-ros \
                 ros-humble-tf-transformations \
                 ros-humble-tf2-msgs \
                 ros-humble-tf2-geometry-msgs \
                 ros-humble-tf2-kdl \
                 ros-humble-tf2-eigen \
                 ros-humble-tf2-sensor-msgs
cd ros2_ws/src
ros2 pkg create tf_demo --build-type ament_python --dependencies rclpy
cd tf_demo/tf_demo/
gedit static_tf_broadcaster.py

#!/usr/bin/env python3 
# -*- coding: utf-8 -*- 

# ROS2 Python 接口库 
import rclpy 
# ROS2 节点类
from rclpy.node import Node 
# 坐标变换消息 
from geometry_msgs.msg import TransformStamped 
# TF 坐标变换库
import tf_transformations  
# TF 静态坐标系广播器类 
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster 

class StaticTFBroadcaster(Node): 
  def __init__(self, name): 
    # ROS2 节点父类初始化
    super().__init__(name)  
    # 创建一个 TF 广播器对象   
    self.tf_broadcaster = StaticTransformBroadcaster(self) 
    
    # 创建一个坐标变换的消息对象 
    static_transformStamped = TransformStamped() 
    # 设置坐标变换消息的时间戳 
    static_transformStamped.header.stamp = self.get_clock().now().to_msg() 
    # 设置一个坐标变换的源坐标系 
    static_transformStamped.header.frame_id = 'world' 
    # 设置一个坐标变换的目标坐标系 
    static_transformStamped.child_frame_id = 'house' 
    # 设置坐标变换中的 X、Y、Z 向的平移
    static_transformStamped.transform.translation.x = 10.0 
    static_transformStamped.transform.translation.y = 5.0 
    static_transformStamped.transform.translation.z = 0.0 
    # 将欧拉角转换为四元数（roll, pitch, yaw） 
    quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0) 
    # 设置坐标变换中的 X、Y、Z 向的旋转（四元数） 
    static_transformStamped.transform.rotation.x = quat[0] 
    static_transformStamped.transform.rotation.y = quat[1] 
    static_transformStamped.transform.rotation.z = quat[2] 
    static_transformStamped.transform.rotation.w = quat[3] 
    
    # 广播静态坐标变换，广播后两个坐标系的位置关系保持不变 
    self.tf_broadcaster.sendTransform(static_transformStamped) 
    
def main(args=None): 
  # ROS2 Python 接口 初始化
  rclpy.init(args=args) 
  # 创建 ROS2 节点对 象并进行初始化
  node = StaticTFBroadcaster("static_tf_broadcaster")  
  # 循环等待 ROS2 退出 
  rclpy.spin(node) 
  # 销毁节点对象
  node.destroy_node()  
  rclpy.shutdown()

chmod +x static_tf_broadcaster.py

（2）创建tf_listener.py

gedit tf_listener.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node

from tf2_ros import TransformListener, Buffer

class TFListener(Node):
    def __init__(self):
        super().__init__('tf_listener')

        # 创建 TF buffer 和 listener
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)

        # 每 0.5s 查询一次 TF
        self.timer = self.create_timer(0.5, self.on_timer)

    def on_timer(self):
        try:
            # 查询 world -> home 的 TF
            trans = self.buffer.lookup_transform(
                'world',   # parent
                'house',    # child
                rclpy.time.Time()
            )

            # 打印 TF 坐标
            t = trans.transform.translation
            self.get_logger().info(
                f"home 相对 world 的位置 = ({t.x:.2f}, {t.y:.2f}, {t.z:.2f})"
            )

        except Exception as e:
            self.get_logger().warn(f"TF 尚未准备好：{e}")

def main(args=None):
    rclpy.init(args=args)
    node = TFListener()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

chmod +x tf_listener.py

（3）setup.py设置

（4）编译运行

cd ~/ros2_ws
colcon build
source ./install/setup.bash
ros2 run tf_demo static_tf_broadcaster

另启终端

cd ros2_ws
source ./install/setup.bash
ros2 run tf_demo tf_listener

1.4 TF监听

（1）创建turtle_tf_broadcaster.py

cd ros2_ws/src/tf_demo/tf_demo/
gedit turtle_tf_broadcaster.py

#!/usr/bin/env python3 
# -*- coding: utf-8 -*- 

# ROS2 Python 接口库
import rclpy  
# ROS2 节点类
from rclpy.node import Node  
# 坐标变换消息 
from geometry_msgs.msg import TransformStamped 
# TF 坐标变换库
import tf_transformations  
# TF 坐标变换广播器 
from tf2_ros import TransformBroadcaster 
# turtlesim 小海龟位置消息
from turtlesim.msg import Pose 

class TurtleTFBroadcaster(Node): 
  def __init__(self, name): 
    # ROS2 节点父类初始化 
    super().__init__(name) 
    # 创建一个海龟名称的参数
    self.declare_parameter('turtlename', 'turtle') 
    # 优先使用外 部设置的参数值，否则用默认值 
    self.turtlename = self.get_parameter( 
      'turtlename').get_parameter_value().string_value 
    # 创建一个 T F 坐标变换的广播对象并初始化   
    self.tf_broadcaster = TransformBroadcaster(self) 
    # 创建一个订 阅者，订阅海龟的位置消息 
    self.subscription = self.create_subscription( 
      Pose, 
      # 使用参数中获 取到的海龟名称
      f'/{self.turtlename}/pose', 
      self.turtle_pose_callback, 1) 
      
  # 创建一个处理海龟位置消息的回调函数，将位置消息转变成坐标变换    
  def turtle_pose_callback(self, msg): 
    # 创建 一个坐标变换的消息对象
    transform = TransformStamped()  
    # 设置坐标变换消息的时间戳 
    transform.header.stamp = self.get_clock().now().to_msg() 
    # 设 置一个坐标变换的源坐标系 
    transform.header.frame_id = 'world' 
    # 设 置一个坐标变换的目标坐标系 
    transform.child_frame_id = self.turtlename 
    # 设 置坐标变换中的 X、Y、Z 向的平移 
    transform.transform.translation.x = msg.x 
    transform.transform.translation.y = msg.y 
    transform.transform.translation.z = 0.0 
    # 将欧拉角转换为四元数（roll, pitch, yaw） 
    q = tf_transformations.quaternion_from_euler(0, 0, msg.theta) 
    # 设 置坐标变换中的 X、Y、Z 向的旋转（四元数） 
    transform.transform.rotation.x = q[0] 
    transform.transform.rotation.y = q[1] 
    transform.transform.rotation.z = q[2] 
    transform.transform.rotation.w = q[3] 
    # Send the transformation 
    # 广播坐标变换，海 龟位置变化后，将及时更新坐标变换信息
    self.tf_broadcaster.sendTransform(transform) 
    
def main(args=None): 
  # ROS2 Python 接口 初始化
  rclpy.init(args=args) 
  # 创建 ROS2 节点对象并进行初始化 
  node = TurtleTFBroadcaster("turtle_tf_broadcaster") 
  # 循环等待 ROS2 退出
  rclpy.spin(node)  
  # 销毁节点对象 
  node.destroy_node() 
  # 关闭 ROS2 Python 接口
  rclpy.shutdown() 

chmod +x turtle_tf_broadcaster.py

（2）创建turtle_following.py

gedit turtle_following.py

#!/usr/bin/env python3 
# -*- coding: utf-8 -*- 

import math
# ROS2 Python 接口库
import rclpy 
# ROS2 节点类 
from rclpy.node import Node 
# TF 坐标变换库
import tf_transformations  
# TF 左边变换的异常类
from tf2_ros import TransformException 
# 存储坐标变换信息的缓冲类 
from tf2_ros.buffer import Buffer 
# 监听坐标变换 的监听器类 
from tf2_ros.transform_listener import TransformListener 
# ROS2 速度控制消息
from geometry_msgs.msg import Twist 
# 海龟生成的服务接口
from turtlesim.srv import Spawn 

class TurtleFollowing(Node): 
  def __init__(self, name): 
    # ROS2 节点父类初始化
    super().__init__(name) 
    # 创建 一个源坐标系名的参数 
    self.declare_parameter('source_frame', 'turtle1') 
    # 优先 使用外部设置的参数值，否则用默认值
    self.source_frame = self.get_parameter( 
      # 创建保 存坐标变换信息的缓冲区
      'source_frame').get_parameter_value().string_value 
    self.tf_buffer = Buffer() 
    # 创建坐标变换的监听器   
    self.tf_listener = TransformListener(self.tf_buffer, self) 
    # 创建 一个请求产生海龟的客户端
    self.spawner = self.create_client(Spawn, 'spawn')  
    # 是否已经请求海龟生成服务的标志位 
    self.turtle_spawning_service_ready = False 
    # 海龟是否产生成功的标志位 
    self.turtle_spawned = False 
    # 创建跟随运动海龟的速度话题 
    self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1) 
    # 创建 一个固定周期的定时器，控制跟随海龟的运动 
    self.timer = self.create_timer(1.0, self.on_timer) 
    
  def on_timer(self): 
    # 源坐标系
    from_frame_rel = self.source_frame  
    # 目标坐标系
    to_frame_rel = 'turtle2' 
    # 如果已经请求海龟生成服务 
    if self.turtle_spawning_service_ready: 
      # 如果跟随海龟已经生成 
      if self.turtle_spawned: 
        try:
          # 获取 ROS 系统的当前时间 
          now = rclpy.time.Time() 
          # 监听当前时刻源坐标系到目标坐标系的坐标变换 
          trans = self.tf_buffer.lookup_transform( 
            to_frame_rel, 
            from_frame_rel, 
            now) 
        # 如果坐标变换获取失败，进入异常报告     
        except TransformException as ex: 
          self.get_logger().info( 
            f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}') 
          return 
        # 创建速度控制消息   
        msg = Twist() 
        # 根据海龟角度，计算角速度
        scale_rotation_rate = 1.0 
        msg.angular.z = scale_rotation_rate * math.atan2( 
          trans.transform.translation.y, 
          trans.transform.translation.x) 
          
        # 根据海龟距离，计算线速度  
        scale_forward_speed = 0.5 
        msg.linear.x = scale_forward_speed * math.sqrt( 
          trans.transform.translation.x ** 2 + 
          trans.transform.translation.y ** 2) 
        
        # 发布速度指令，海龟跟随运动 
        self.publisher.publish(msg) 
      # 如果跟随海龟没有生成   
      else: 
        # 查看海龟是否生成 
        if self.result.done(): 
          self.get_logger().info( 
            f'Successfully spawned {self.result.result().name} ') 
          self.turtle_spawned = True 
        # 依然没有生成跟随海龟  
        else: 
          self.get_logger().info('Spawn is not finished') 
    # 如果没有请求海龟生成服务      
    else: 
      # 如果海龟生成服务器已经准备就绪 
      if self.spawner.service_is_ready(): 
        # 创建一 个请求的数据 
        request = Spawn.Request() 
        # 设置请求数据的内容，包括海龟名、xy 位置、姿态 
        request.name = 'turtle2' 
        request.x = float(4) 
        request.y = float(2) 
        request.theta = float(0) 
        # 发送服务请求 
        self.result = self.spawner.call_async(request) 
        # 设置标志位，表示已经发送请求 
        self.turtle_spawning_service_ready = True 
      else:
        # 海龟生成服务器还没准备就绪的提示 
        self.get_logger().info('Service is not ready') 
      
def main(args=None): 
  # ROS2 Python 接口初始化
  rclpy.init(args=args) 
  # 创建 ROS2节点对象并进行初始化
  node = TurtleFollowing("turtle_following") 
  # 循环等待 ROS2 退出 
  rclpy.spin(node) 
  # 销毁节点对象 
  node.destroy_node() 
  # 关闭 ROS2 Python 接口
  rclpy.shutdown() 

chmod +x turtle_following.py

（3）创建turtle_following.launch.py

cd ..
mkdir launch
cd launch/
gedit turtle_following.launch.py

from launch import LaunchDescription 
from launch.actions import DeclareLaunchArgument 
from launch.substitutions import LaunchConfiguration 
from launch_ros.actions import Node 

def generate_launch_description(): 
  return LaunchDescription([ 
    Node(
      package='turtlesim', 
      executable='turtlesim_node', 
      name='sim' 
    ),
    
    Node(
      package='tf_demo', 
      executable='turtle_tf_broadcaster', 
      name='broadcaster1', 
      parameters=[ {'turtlename': 'turtle1'} ] 
    ),
    
    DeclareLaunchArgument(
      'target_frame', default_value='turtle1', 
      description='Target frame name.' 
    ),
    Node(
      package='tf_demo', 
      executable='turtle_tf_broadcaster', 
      name='broadcaster2', 
      parameters=[ {'turtlename': 'turtle2'} ] 
    ),
    
    Node(
      package='tf_demo', 
      executable='turtle_following', 
      name='listener', 
      parameters=[ {'target_frame': LaunchConfiguration('target_frame')} ] 
    ), 
  ])

chmod +x turtle_following.launch.py

（4）setup.py设置

（5）编译运行

cd ~/ros2_ws
colcon build
source ./install/setup.bash
ros2 launch tf_demo turtle_following.launch.py

另启终端

ros2 run turtlesim turtle_teleop_key

可以通过上下左右箭进行控制，另一只小乌龟跟随。

2 Gazebo三维物理仿真平台

2.1 Gazebo简介

ROS中常用的三维物理仿真平台，支持动力学引擎、可实现高质量图形渲染、模拟机器人及其周边环境、加入摩擦力、弹性系数等物理属性。

2.2 安装及运行

Ubuntu22.04推荐装Gazebo Harmonic、如使用ROS2 Humble推荐装Gazebo Fortress，Ubuntu24.04推荐装Gazebo Jetty、如使用ROS2 Jazzy推荐装Gazebo Harmonic

官方安装教程见：https://gazebosim.org/docs/harmonic/getstarted/

也可使用如下指令，将humble替换成自己的Ubuntu版本即可，系统会自动选择适配版本安装。

sudo apt install ros-humble-gazebo-*

启动，终端输入：

gazebo

2.3 使用教程

官网：https://gazebosim.org/docs/harmonic/getstarted/

3 Rviz工具