ROS实验三（坐标变换）

------------👆👆👆其他章节点开专栏查看👆👆👆------------

---

写在前面

专栏中的ROS实验一和ROS实验二详细介绍了如何安装虚拟机，如何安装ROS，如何安装Ubuntu，以及Ubuntu中的一些基础指令。不熟悉的朋友可以点开专栏查看之前的章节。

本章带领大家理解和实践ROS中的坐标变换（TF）系统，完成以下任务：

1.使用 Python 编写发布方与订阅方，实现动态 TF 坐标变换。

2.使用 Python 编写程序实现海龟跟随的案例，要求产生两只海龟，海龟 B 会 自动运行至海龟 A 的位置，并且键盘控制时，只是控制海龟 A 的运动，但海龟 B 可以跟随海龟 A 运动。⭐

---

详细步骤

1.使用 Python 编写发布方与订阅方，实现动态 TF 坐标变换

以小海龟程序为例，小海龟程序中有两个坐标系，分别是以小海龟为中心的海龟坐标系和以整个地图顶点为中心的世界坐标系。同一个点在不同坐标系中有不同的表示方式。

我们的任务是使用键盘控制海龟移动，并将海龟坐标系上一点(1.3, 3.3, 0.0)在世界坐标系上的位置动态发布。首先创建功能包，把它命名为task3吧。

cd catkin_ws/src
catkin_create_pkg task4 tf rospy geometry_msgs turtlesim

用vscode打开工作空间：

code .

在task3功能包下添加launch文件夹和scripts文件夹，并添加文件。如下图所示：

其中，transform_point_publisher.py用于将海龟坐标系下的点变换到world坐标系，并发布坐标信息，代码如下：

#!/usr/bin/env python3
import rospy
import tf
import geometry_msgs.msg

def transform_point():
    rospy.init_node('transform_point_publisher')

    listener = tf.TransformListener()

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            # 等待从 turtle1 到 world 的 tf 可用
            listener.waitForTransform('/world', '/turtle1', rospy.Time(0), rospy.Duration(4.0))

            # 构造海龟坐标系下的点
            point_in_turtle = geometry_msgs.msg.PointStamped()
            point_in_turtle.header.frame_id = "turtle1"
            point_in_turtle.header.stamp = rospy.Time(0)
            point_in_turtle.point.x = 1.3
            point_in_turtle.point.y = 3.3
            point_in_turtle.point.z = 0.0

            # 将点从 turtle1 坐标系变换到 world 坐标系
            point_in_world = listener.transformPoint("world", point_in_turtle)

            rospy.loginfo("Point in world: (%.2f, %.2f, %.2f)", 
                          point_in_world.point.x,
                          point_in_world.point.y,
                          point_in_world.point.z)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        rate.sleep()

if __name__ == '__main__':
    transform_point()

 turtle_tf_broadcster.py用于订阅发布的坐标。代码如下：

#!/usr/bin/env python3
import rospy
import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

turtle_tf_demo.launch可以理解为一键启动文件，代码如下：

<launch>
  <!-- 启动 turtlesim -->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

  <!-- 启动键盘控制 -->
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

  <!-- 启动 TF 广播器 -->
  <node pkg="task3" type="turtle_tf_broadcaster.py" name="tf_broadcaster" output="screen">
    <param name="turtle" value="turtle1"/>
  </node>

  <!-- 启动点转换器 -->
  <node pkg="task3" type="transform_point_publisher.py" name="point_transformer" output="screen"/>
</launch>

完成后进入终端，输入以下指令：

cd ~/catkin_ws#进入工作空间

#添加可执行权限
chmod +x src/task3/scripts/turtle_tf_broadcaster.py
chmod +x src/task3/scripts/transform_point_publisher.py

catkin_make
source devel/setup.bash
roslaunch task3 turtle_tf_demo.launch

结果如下图所示：

2.实现小海龟跟随

具体实现过程是：通过订阅 turtle1 和 turtle2 的位姿，实时计算它们之间的距离和角度差，然后发布线速度和角速度控制 turtle2 运动，从而实现海龟 B 自动跟随海龟 A 的效果。

本任务需要用到的文件有follow.py，spawn_turtle.py和follow.launch：

follow.py文件用于实现追踪功能：

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
import math

class TurtleFollower:
    def __init__(self):
        rospy.init_node('turtle_follower', anonymous=True)

        # 跟随者控制发布器
        self.cmd_pub = rospy.Publisher('/turtle2/cmd_vel', Twist, queue_size=10)

        # 订阅目标者的位置
        rospy.Subscriber('/turtle1/pose', Pose, self.target_pose_callback)
        rospy.Subscriber('/turtle2/pose', Pose, self.follower_pose_callback)

        self.target_pose = Pose()
        self.follower_pose = Pose()

        self.rate = rospy.Rate(10)

    def target_pose_callback(self, msg):
        self.target_pose = msg

    def follower_pose_callback(self, msg):
        self.follower_pose = msg

    def run(self):
        while not rospy.is_shutdown():
            dx = self.target_pose.x - self.follower_pose.x
            dy = self.target_pose.y - self.follower_pose.y
            distance = math.sqrt(dx**2 + dy**2)
            angle_to_target = math.atan2(dy, dx)
            angle_diff = angle_to_target - self.follower_pose.theta

            # 归一化角度差
            angle_diff = math.atan2(math.sin(angle_diff), math.cos(angle_diff))

            vel = Twist()
            vel.linear.x = 2.0 * distance
            vel.angular.z = 6.0 * angle_diff
            self.cmd_pub.publish(vel)

            self.rate.sleep()

if __name__ == '__main__':
    follower = TurtleFollower()
    follower.run()

spawn_turtle.py用于生成第二只小海龟：

#!/usr/bin/env python3
import rospy
from turtlesim.srv import Spawn

if __name__ == '__main__':
    rospy.init_node('spawn_turtle2')

    rospy.wait_for_service('/spawn')
    try:
        spawner = rospy.ServiceProxy('/spawn', Spawn)
        spawner(4.0, 2.0, 0.0, 'turtle2')
        rospy.loginfo("Spawned turtle2 successfully.")
    except rospy.ServiceException as e:
        rospy.logerr("Service call failed: %s", e)

follow.launch文件用于一键启动hhhh

<launch>
  <!-- 启动 turtlesim -->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

  <!-- 键盘控制 turtle1 -->
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

  <!-- 启动 turtle2 -->
  <node pkg="task3" type="spawn_turtle.py" name="spawn_turtle2" output="screen"/>

  <!-- 跟随控制 -->
  <node pkg="task3" type="follow.py" name="turtle_follower" output="screen"/>
</launch>

 打开终端，输入下面的指令：

cd ~/catkin_ws
chmod +x src/task3/scripts/follow.py
chmod +x src/task3/scripts/spawn_turtle.py
catkin_make
source devel/setup.bash
roslaunch task3 follow.launch

任务完成！😊😊