ROS python实现乌龟跟随

news2024/10/7 10:20:51

产生两只乌龟，中间的乌龟(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中加相应的配置