ROS python实现乌龟跟随

产生两只乌龟，中间的乌龟(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/Spawn1、导包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.5request.y=2.0request.theta=-3request.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.xts.transform.translation.y=pose.yts.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]#修改1sub=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中加相应的配置