TF综合项目——小乌龟的跟随项目

需求

程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

示例

实现思路

由于算法不了解导致的思路错误：

• 调用之前模块，产生小乌龟
• 用键盘控制第一个小乌龟运动，向第二个小乌龟话题发送运动消息，使其跟随运动
• 即，首先要订阅turtle1的运动消息，即发布turtle1在world坐标系下的位置，turtle2接收到消息，规划目标点，以及相应的速度规划。

根据小乌龟只能有线速度和角速度的算法，不能单纯将两只小乌龟放在世界坐标系下去，实现跟随（当有合适的算法也可以，会更加简单且不涉及到TF），因此正确思路如下：

• 产生两只小乌龟
• 控制第一只小乌龟
• 发布两只小乌龟相对于坐标系的，坐标信息关系
• 接受坐标位置关系，然后转化为小乌龟1在小乌龟2坐标系下的位置，有了相对位置，再采取相应的算法计算角速度和线速度，给小乌龟2的话题发布即可。

代码实现

顶层文件：

<launch>
<!-- ===============================小乌龟出生地====================================================== -->
<!-- 小乌龟启动节点 -->
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"/>
<!-- 键盘控制节点 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>
<!-- 添加第二个小乌龟 -->
<node pkg="comunication_learn" type="turtlesim_create.py" name="turtle2" args= "turtle2 2 3 4 1.57"      output="screen"/>
<!-- ===============================消息发生地====================================================== -->
<!-- turtle1发布自己的位置信息,相对于world坐标系 -->
<node pkg='tf_learn' type='pub_tf_dynamic_m.py' name='tf_turtle1'  args='/turtle1/pose world turtle1'   output='screen'/>
<node pkg='tf_learn' type='pub_tf_dynamic_m.py' name='tf_turtle2'  args='/turtle2/pose world turtle2'   output='screen'/>

<!-- turtle2接收到广播消息,并进行数据处理 -->
<node pkg='tf_learn' type='sub_turtle2.py' name='sub_tf_turtle2'  args='turtle2 turtle1'   output='screen'/>


</launch>

坐标处理文件：

#! usr/bin/env python

import rospy
import tf2_ros
import sys
from tf2_geometry_msgs import PointStamped
from geometry_msgs.msg import Twist
import math
'''
需求：通过缓存获取坐标系之间的相对关系,然后进行数据处理
步骤：
    1.导包
    2.初始化ros节点
    3.创建缓存
    4.创建监听对象
    5.获取坐标系之间的相对关系  

'''
parent_link = sys.argv[1]
child_link = sys.argv[2]

if __name__ == "__main__":

    rospy.init_node("dynamic_tf_listener")
    rospy.loginfo("开始获取坐标系之间的相对关系")

    #创建缓存
    buffer = tf2_ros.Buffer()
    #等待缓存来临
    rospy.sleep(1)
    #创造监听对象
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)

    while not rospy.is_shutdown():
        try:
            #获取坐标系之间的相对关系
            trans = buffer.lookup_transform(parent_link, child_link, rospy.Time())
            rospy.loginfo("获取成功")
            rospy.loginfo("位置：%s"%trans.transform.translation)
            rospy.loginfo("四元数：%s"%trans.transform.rotation)

            pub = rospy.Publisher('/turtle2/cmd_vel',Twist,queue_size=10)
            rate = rospy.Rate(10)

            msg = Twist()
            msg.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
            msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)


            
            pub.publish(msg)
            rate.sleep()





        except Exception as e:
            rospy.logerr("错误提示:%s",e)

        

    

总结

• 认清楚如何将坐标关系转化为线速度角速度的关系