Ros入门学习003——小乌龟跟随

小乌龟跟随实验

思路：
1、在gui中生成两只小乌龟，并利用tf发布两只乌龟相对base_link的坐标
2、将乌龟1的全局坐标转换到乌龟2的局部坐标下，通过/turtle2/cmd_vel话题发布移动指令，试turtle2朝着turtle1的方向跟随

<turtle_spawn.py>
生成第二只乌龟

#!/usr/bin/env python
# -*- coding:utf-8 -*-
# @Time:2021/9/29 下午3:08
# @Author:ljt
import rospy
from turtlesim.srv import Spawn,SpawnRequest
def turtle_spawn(x,y,theta,name):
    Client=rospy.ServiceProxy('/spawn',Spawn)
    Client.wait_for_service()
    req=SpawnRequest()
    req.x=x
    req.y=y
    req.theta=theta
    req.name=name
    try:
        response=Client.call(req)
        rospy.loginfo("New turtle's name:%s",response.name)
    except Exception as e:
        rospy.logerr('Spawn Error:%s',e)
if __name__ == '__main__':
    rospy.init_node('turtle_spawn')
    turtle_spawn(1.0,1.0,0,'turtle2')

<tf_turtle_publisher.py>
通过话题/turtle/pose获取乌龟在全局的坐标，并用tf发布

#!/usr/bin/env python
# -*- coding:utf-8 -*-
# @Time:2021/9/29 下午3:24
# @Author:ljt
import rospy
import sys
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
def CallbackFunction(pose,turtle_name):
    broadcaster = tf2_ros.StaticTransformBroadcaster()
    tfs = TransformStamped()
    tfs.header.frame_id = 'base_link'
    tfs.header.seq = rospy.Time.now()
    tfs.header.seq = 102
    tfs.child_frame_id = turtle_name
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0
    qtn = tf.transformations.quaternion_from_euler(0, 0, pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]
    broadcaster.sendTransform(tfs)
if __name__ == '__main__':
    rospy.init_node('tf_turtle_pubilsher')
    '''由launch文件传入参数，当传入一个参数时，sys.argv有四位参数，其中第二位为
    传入参数，其他为系统信息'''
    turtle_name=sys.argv[1]
    rospy.logwarn('%s',turtle_name)
    sub=rospy.Subscriber('/'+turtle_name+'/pose',Pose,CallbackFunction,(turtle_name),queue_size=100)
    rospy.spin()

<tf_turtle_following.py>
将turtle1的全局坐标转换到turtle2的局部坐标，在控制turtle2调整角度和速度进行跟随

#!/usr/bin/env python
# -*- coding:utf-8 -*-
# @Time:2021/9/29 下午6:13
# @Author:ljt
import rospy
import tf2_ros
from geometry_msgs.msg import Twist
import numpy as np
def turtle_following():
    rospy.init_node('turtle_following')
    pub=rospy.Publisher('/turtle2/cmd_vel',Twist,queue_size=100)
    buffer=tf2_ros.Buffer()
    sub=tf2_ros.TransformListener(buffer)
    rate=rospy.Rate(10)
    follow_msg=Twist()
    while(not rospy.is_shutdown()):
        rate.sleep()
        try:
            trans_pose=buffer.lookup_transform('turtle2','turtle1',rospy.Time(0))
            trans_x=trans_pose.transform.translation.x
            trans_y=trans_pose.transform.translation.y
            trans_z=trans_pose.transform.translation.z
            follow_msg.linear.x=0.5*np.sqrt(np.power(trans_x,2)+np.power(trans_y,2))
            if (trans_x==0):
                trans_x=0.001
            if (trans_x<0 and trans_y<0):
                follow_msg.angular.z = -4 * np.arctan(trans_y / trans_x)
            else:
                follow_msg.angular.z=4*np.arctan(trans_y/trans_x)
            pub.publish(follow_msg)
        except Exception as e:
            rospy.logerr('Following Error:%s',e)
if __name__ == '__main__':
    turtle_following()

<turtle_following.launch>
launch文件启动各节点

<launch>
	<node pkg="turtlesim" type="turtlesim_node" name="turtle_gui" output="screen"/>
	<node pkg="turtlesim" type="turtle_teleop_key" name="turtle_keyboard" output="screen"/>
	<node pkg="turtle_follow" type="turtle_spawn.py" name="turtle_spawn" output="screen"/>
	<!--分别获取并tf发布两只乌龟的全局坐标-->
	<node pkg="turtle_follow" type="tf_turtle_publisher.py" name="tf_turtle1_publisher" output="screen" args="turtle1"/>
	<node pkg="turtle_follow" type="tf_turtle_publisher.py" name="tf_turtle2_publisher" output="screen" args="turtle2"/>
	<node pkg="turtle_follow" type="tf_turtle_following.py" name="tf_turtle_following" output="screen"/>
</launch>