机器人坐标系Launch文件解析

Launch文件解析

先来看下刚才运行的launch文件，里边启动了四个节点，分别是：

• 小海龟仿真器
• 海龟1的坐标系广播
• 海龟2的坐标系广播
• 海龟跟随控制

其中，两个坐标系的广播复用了turtle_tf_broadcaster节点，通过传入的参数名修改维护的坐标系名称。

learning_tf/launch/turtle_following_demo.launch.py

from launch import LaunchDescriptionfrom launch.actions import DeclareLaunchArgumentfrom launch.substitutions import LaunchConfigurationfrom launch_ros.actions import Nodedef generate_launch_description():    return LaunchDescription([        Node(            package='turtlesim',            executable='turtlesim_node',            name='sim'        ),        Node(            package='learning_tf',            executable='turtle_tf_broadcaster',            name='broadcaster1',            parameters=[                {'turtlename': 'turtle1'}            ]        ),        DeclareLaunchArgument(            'target_frame', default_value='turtle1',            description='Target frame name.'        ),        Node(            package='learning_tf',            executable='turtle_tf_broadcaster',            name='broadcaster2',            parameters=[                {'turtlename': 'turtle2'}            ]        ),        Node(            package='learning_tf',            executable='turtle_following',            name='listener',            parameters=[                {'target_frame': LaunchConfiguration('target_frame')}            ]        ),     ])

坐标系动态广播

海龟1和海龟2在world坐标系下的坐标变换，在turtle_tf_broadcaster节点中实现，除了海龟坐标系的名字不同之外，针对两个海龟的功能是一样的。

learning_tf/turtle_tf_broadcaster.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@说明: ROS2 TF示例-广播动态的坐标变换"""import rclpy                                       # ROS2 Python接口库from rclpy.node import Node                        # ROS2 节点类from geometry_msgs.msg import TransformStamped     # 坐标变换消息import tf_transformations                          # TF坐标变换库from tf2_ros import TransformBroadcaster           # TF坐标变换广播器from turtlesim.msg import Pose                     # turtlesim小海龟位置消息class TurtleTFBroadcaster(Node):    def __init__(self, name):        super().__init__(name)                                # ROS2节点父类初始化        self.declare_parameter('turtlename', 'turtle')        # 创建一个海龟名称的参数        self.turtlename = self.get_parameter(                 # 优先使用外部设置的参数值，否则用默认值            'turtlename').get_parameter_value().string_value        self.tf_broadcaster = TransformBroadcaster(self)      # 创建一个TF坐标变换的广播对象并初始化        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                    # 设置一个坐标变换的目标坐标系        transform.transform.translation.x = msg.x                     # 设置坐标变换中的X、Y、Z向的平移        transform.transform.translation.y = msg.y        transform.transform.translation.z = 0.0        q = tf_transformations.quaternion_from_euler(0, 0, msg.theta) # 将欧拉角转换为四元数（roll, pitch, yaw）        transform.transform.rotation.x = q[0]                         # 设置坐标变换中的X、Y、Z向的旋转（四元数）        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):    rclpy.init(args=args)                                # ROS2 Python接口初始化    node = TurtleTFBroadcaster("turtle_tf_broadcaster")  # 创建ROS2节点对象并进行初始化    rclpy.spin(node)                                     # 循环等待ROS2退出    node.destroy_node()                                  # 销毁节点对象    rclpy.shutdown()                                     # 关闭ROS2 Python接口

完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：

entry_points={        'console_scripts': [            'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main',            'turtle_tf_broadcaster = learning_tf.turtle_tf_broadcaster:main',            'tf_listener = learning_tf.tf_listener:main',        ],    },

海龟跟随

坐标系都正常广播了，接下来我们就可以订阅两只海龟的位置关系，并且变换成速度指令进行控制啦。

learning_tf/turtle_following.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@说明: ROS2 TF示例-通过坐标变化实现海龟跟随功能"""import mathimport rclpy                                              # ROS2 Python接口库from rclpy.node import Node                               # ROS2 节点类import tf_transformations                                 # TF坐标变换库from tf2_ros import TransformException                    # TF左边变换的异常类from tf2_ros.buffer import Buffer                         # 存储坐标变换信息的缓冲类from tf2_ros.transform_listener import TransformListener  # 监听坐标变换的监听器类from geometry_msgs.msg import Twist                       # ROS2 速度控制消息from turtlesim.srv import Spawn                           # 海龟生成的服务接口class TurtleFollowing(Node):    def __init__(self, name):        super().__init__(name)                                      # ROS2节点父类初始化        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:                    now = rclpy.time.Time()                        # 获取ROS系统的当前时间                    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()                          # 创建一个请求的数据                request.name = 'turtle2'                           # 设置请求数据的内容，包括海龟名、xy位置、姿态                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):    rclpy.init(args=args)                       # ROS2 Python接口初始化    node = TurtleFollowing("turtle_following")  # 创建ROS2节点对象并进行初始化    rclpy.spin(node)                            # 循环等待ROS2退出    node.destroy_node()                         # 销毁节点对象    rclpy.shutdown()                            # 关闭ROS2 Python接口

完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：

entry_points={        'console_scripts': [            'static_tf_broadcast