产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行
乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。
Python实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
2.服务客户端(生成乌龟)
#! /usr/bin/env python
import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse
"""
需求：向服务器发送请求生成一只乌龟
话题：/spawn
消息：turtlesim/Spawn
1、导包
2、初始化ROS节点
3、创建服务的客户端对象
4、组织数据并发送请求
5、处理响应结果
"""
if __name__=="__main__":
#2、初始化ROS节点
rospy.init_node("service_call_p")
#3、创建服务的客户端对象
client=rospy.ServiceProxy("/spawn",Spawn)
#4、组织数据并发送请求
request = SpawnRequest()
request.x=4.5
request.y=2.0
request.theta=-3
request.name="turtle2"
#4-2 判断服务器状态并发送
client.wait_for_service()#客户端等待服务，若服务端没有启动则挂起
try:
response=client.call(request)
#5、处理响应结果
rospy.loginfo("生成乌龟的名字叫：%s",response.name)
except Exception as e:
rospy.logerr("请求处理异常")3.发布方(发布两只乌龟的坐标信息)
#! /usr/bin/env python
import rospy
import tf.transformations
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
import sys
"""
发布方：订阅乌龟的位姿信息，转换处呢个坐标系的相对关系，再发布
准备：
话题：/turtle1/pose
类型：/turtlesim/Pose
流程：
1、导包
2、初始化ROS节点
3、创建订阅对象
4、回调函数处理订阅到的消息（核心）
5、spin()
"""
#接受乌龟名称的变量
turtle_name = ""
def doPose(pose): #参数为订阅到的消息 pose
#创建发布坐标系相对关系的对象
pub=tf2_ros.TransformBroadcaster()
#将pose转换成坐标系相对关系消息
ts=TransformStamped()
ts.header.frame_id="world" #被参考的坐标系
ts.header.stamp=rospy.Time.now()
#修改2--------------------------------------------------------------
ts.child_frame_id=turtle_name
#子级坐标系相对于父级坐标系的偏移量
ts.transform.translation.x=pose.x
ts.transform.translation.y=pose.y
ts.transform.translation.z=0
#四元数
#从欧拉角转换四元数
"""
乌龟是2D的，不存在X上的翻滚Y上偏航，只有Z上的偏航
0 0 pose.thera
"""
qtn=tf.transformations.quaternion_from_euler(0,0,pose.theta)
ts.transform.rotation.x=qtn[0]
ts.transform.rotation.y=qtn[1]
ts.transform.rotation.z=qtn[2]
ts.transform.rotation.w=qtn[3]
#发布
pub.sendTransform(ts)
if __name__=="__main__":
# 2、初始化ROS节点
rospy.init_node("dynamic_pub_p")
# 3、创建订阅对象
#解析传入的参数（现在传入几个参数？文件全路径+传入的参数+自己定义的节点名称+日志文件路径）
if len(sys.argv)!=4:
rospy.loginfo("参数个数不对")
sys.exit(1)
else:
turtle_name=sys.argv[1]
#修改1
sub=rospy.Subscriber(turtle_name+"/pose",Pose,doPose,queue_size=100)
# 4、回调函数处理订阅到的消息（核心）
# 5、spin()
rospy.spin()4.订阅方(解析坐标信息并生成速度信息)
#! /usr/bin/env python
import rospy
import tf2_ros
import tf2_geometry_msgs
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import TransformStamped,Twist
import math
if __name__=="__main__":
# 2、初始化
rospy.init_node("static_sub_p")
# 3、创建订阅对象
#3-1 创建缓存对象
buffer=tf2_ros.Buffer()
#3-2 创建订阅对象（将缓存传入）
sub=tf2_ros.TransformListener(buffer)
# 创建速度消息发布对象
pub=rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=100)
# 5、转换逻辑实现，调用tf封装的算法
rate=rospy.Rate(10)
while not rospy.is_shutdown():
try:
#--------------------计算相son1相对于son2的坐标关系
"""
参数1：目标坐标系
参数2：源坐标系
参数3：rospy.Time(0)----------取时间间隔最近的两个坐标系帧（son1相对world与son2相对world)来计算结果
返回值：son1与son2的坐标关系
"""
ts=buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
rospy.loginfo("父级坐标系：%s,子级坐标系：%s,偏移量（%.2f,%.2f,%.2f）",ts.header.frame_id,ts.child_frame_id,
ts.transform.translation.x,ts.transform.translation.y,ts.transform.translation.z)
#组织Twist消息
twist=Twist()
#线速度=系数*坐标系原点的间距=系数*（x^2 +y^2 )再开方
#角速度=系数*夹角 =系数*atan2(y,x)
twist.linear.x=0.5*math.sqrt(math.pow(ts.transform.translation.x,2)
+ math.pow(ts.transform.translation.y,2))
twist.angular.z=4*math.atan2(ts.transform.translation.y,ts.transform.translation.x)
#发布消息
pub.publish(twist)
except Exception as e:
rospy.logwarn("错误提示：%s",e)
# 7、spain() |spinOnce()
rate.sleep()使用 launch 文件组织需要运行的节点
<launch>
<!--
流程详解：
1.准备工作：启动乌龟的GUI节点和键盘控制节点
2、需要调用服务器生成一只新的乌龟
3、发布两只乌龟的坐标信息
4、订阅坐标信息，并转换成乌龟A相对于乌龟B 的坐标信息，最后再生成控制乌龟的速度信息
-->
<!--1.准备工作：启动乌龟的GUI节点和键盘控制节点-->
<!--乌龟GUI-->
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
<!--键盘控制-->
<node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
<!--2、需要调用服务器生成一只新的乌龟-->
<node pkg="tf04_test" type="test01_new_turtle_p.py" name="turtle2" output="screen" />
<!--3、发布两只乌龟的坐标信息
A、复用之前的乌龟坐标发布功能
B、调用节点时，以参数的方式传递乌龟名称，解析参数置换：订阅的话题消息和子级坐标系的名称
-->
<node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub1" args="turtle1" output="screen" />
<node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub2" args="turtle2" output="screen" />
<!--4、订阅坐标信息，并转换成乌龟A相对于乌龟B 的坐标信息，最后再生成控制乌龟的速度信息-->
<node pkg="tf04_test" type="test03_control_turtle2_p.py" name="control" output="screen" />
</launch>先修改py文件的权限并在CMakeList中加相应的配置
运行launch文件
参考链接：
[1]207坐标变换实操Python01_生成乌龟-ROS常用组件_哔哩哔哩_bilibili
