ROS海龟跟随（坐标变换）

基于坐标变换的ros海龟跟随学习记录

本文用于记录ros海龟跟随的实验，不使用tf广播变换跟随事例，旨在理解坐标变换在实际项目中的应用。

---

接下来就是代码环节

1、调用相关库

程序需要用到生成乌龟的服务、发布Twist消息来控制小海龟、获取海龟位置信息
于是我们调用Spawn、Twist以及Pose

#!/usr/bin/env python3
#程序功能：第二个乌龟跟随第一个乌龟运动
import  sys
import rospy
import math
from turtlesim.srv import Spawn
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose

2、初始化ros节点

把这个节点命名为turtle_following，这里命名随意，无伤大雅

rospy.init_node("turtle_following")

3、生成第二只小海龟，并把这个小海龟命名为ykp（默认我们在终端已经开启了turtlesim_node，也就是已经有了第一个叫做turtle1的海龟）

#生成海龟
rospy.wait_for_service('/spawn')
client = rospy.ServiceProxy('/spawn',Spawn)
response = client(1.0,1.0,0,"ykp")  #返回name
rospy.loginfo("turtle_name = %s",response)

4、声明一个发布者，准备向ykp这只小海龟发布话题

pub = rospy.Publisher('/ykp/cmd_vel',Twist,queue_size=10)

5、接下来就是订阅海龟的位姿并触发中断回调函数

rospy.Subscriber('/turtle1/pose',Pose,turtlesimInfoCallback1)
rospy.Subscriber('/ykp/pose',Pose,turtlesimInfoCallback2)

可以看到我们将turtle1，也就是第一只小海龟的订阅回调函数取名为了turtlesimInfoCallback1；将第二只小海龟的订阅回调函数取名为了turtlesimInfoCallback2。

6、顺理成章，我们需要写两个回调函数的内容

先来看第一个海龟turtle1，这只海龟会作为我们用键盘来控制“跑”的海龟，所以只需要能够实时获取这只海龟的位姿就足够了

哦对，忘记说了 补充一下我们首先在最开始声明三个全局变量，因为我们需要在回调函数中也用到这些变量

global pose1 #第一个海龟的位姿
global pose2 #第二个海龟的位姿
global pub #话题发布

回到正题，开始写第一个海龟的回调函数，获取位姿

def turtlesimInfoCallback1(msg):
    global pose1
    pose1.x = msg.x
    pose1.y = msg.y
    pose1.theta = msg.theta
    rospy.loginfo("turtle1:x=%f,y=%f,z=%f",pose1.x,pose1.y,pose1.theta)   #订阅第一个乌龟的姿态

获取到第一只海龟位姿后，那么turtle1的任务现在就完成了 接下来是全文的重点部分，也就是第二只海龟的回调函数。
任务：获取自身的坐标/位姿，并通过turtle1的位姿来向ykp发布话题，使得ykp能够成功跟上turtle1
我们将问题慢慢拆分开：
1、获取ykp的位姿

#订阅第二个乌龟的姿态  并向其发送运动指令
def turtlesimInfoCallback2(msg):
    global pose2
    pose2.x = msg.x
    pose2.y = msg.y
    pose2.theta = msg.theta
    rospy.loginfo("turtle2:x=%f,y=%f,z=%f",pose2.x,pose2.y,pose2.theta)  #第二个乌龟的姿态

接下来进入重点部分
2、根据turtle1来调整ykp的位置
首先定义三个变量来存储 旋转分量、偏移分量以及最终位置分量

    rotated = Pose()   #旋转
    deviation =Pose()   #偏移
    end_pose =Pose()  #最终

为了后面代码的简洁，定义s和c两个变量，分别表示sin和cos

    c = math.cos(pose2.theta)   #cos
    s = math.sin(pose2.theta)    #sin

用图示来直观感受一下旋转矩阵和偏移矩阵

xr、yr的旋转矩阵代表的就是海龟1在海龟2坐标系下的坐标，而下面的偏移矩阵则是坐标原点的偏移。
所以将旋转矩阵和偏移矩阵用代码表示。

	rotated.x = c*pose1.x+s*pose1.y
    rotated.y = -1*s*pose1.x+c*pose1.y
    rospy.loginfo("rotated:x=%f,y=%f",rotated.x,rotated.y)   #海龟1 在海龟2坐标系下的 坐标
    
    deviation.x = -1*(c*pose2.x+s*pose2.y)
    deviation.y = -1*((-1)*s*pose2.x+c*pose2.y)
    rospy.loginfo("deviation:x=%f,y=%f",deviation.x,deviation.y)  #原点到海龟2的距离

最终的位置=旋转分量+偏移分量

	end_pose.x=rotated.x+deviation.x            #旋转+偏移
    end_pose.y=rotated.y+deviation.y
    rospy.loginfo("end_pose:x=%f,y=%f",end_pose.x,end_pose.y)

速度和距离成线性关系，所以线速度=k*距离
角度则可以用x，y的正切表示 前面也可以用k倍来调整旋转的速度，k值越大，旋转的越快。
最后再将ykp海龟的速度信息发布出去就大功告成了~

	vel_msg = Twist()
    vel_msg.linear.x = 0.5*math.sqrt(end_pose.x * end_pose.x + end_pose.y * end_pose.y)
    vel_msg.angular.z = 4*math.atan2(end_pose.y,end_pose.x)
    pub.publish(vel_msg)   #发布速度信息 使第二个乌龟运动

乌龟跟随效果展示

现将完整代码贴图如下

#!/usr/bin/env python3
#程序功能：第二个乌龟跟随第一个乌龟运动
import  sys
import rospy
import math
from turtlesim.srv import Spawn
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from turtlesim.srv import Kill

global pose1
global pose2
global pub

def turtlesimInfoCallback1(msg):
    global pose1
    pose1.x = msg.x
    pose1.y = msg.y
    pose1.theta = msg.theta
    rospy.loginfo("turtle1:x=%f,y=%f,z=%f",pose1.x,pose1.y,pose1.theta)   #订阅第一个乌龟的姿态

#订阅第二个乌龟的姿态  并向其发送运动指令
def turtlesimInfoCallback2(msg):
    global pose2
    pose2.x = msg.x
    pose2.y = msg.y
    pose2.theta = msg.theta
    rospy.loginfo("turtle2:x=%f,y=%f,z=%f",pose2.x,pose2.y,pose2.theta)  #第二个乌龟的姿态
    rotated = Pose()   #旋转
    deviation =Pose()   #偏移
    end_pose =Pose()  #最终
    c = math.cos(pose2.theta)   #cos
    s = math.sin(pose2.theta)    #sin
    
    rotated.x = c*pose1.x+s*pose1.y
    rotated.y = -1*s*pose1.x+c*pose1.y
    rospy.loginfo("rotated:x=%f,y=%f",rotated.x,rotated.y)   #海龟1 在海龟2坐标系下的 坐标
    
    deviation.x = -1*(c*pose2.x+s*pose2.y)
    deviation.y = -1*((-1)*s*pose2.x+c*pose2.y)
    rospy.loginfo("deviation:x=%f,y=%f",deviation.x,deviation.y)  #原点到海龟2的距离
    
    end_pose.x=rotated.x+deviation.x            #旋转+偏移
    end_pose.y=rotated.y+deviation.y
    rospy.loginfo("end_pose:x=%f,y=%f",end_pose.x,end_pose.y)

    vel_msg = Twist()
    vel_msg.linear.x = 0.5*math.sqrt(end_pose.x * end_pose.x + end_pose.y * end_pose.y)
    vel_msg.angular.z = 4*math.atan2(end_pose.y,end_pose.x)
    pub.publish(vel_msg)   #发布速度信息 使第二个乌龟运动

# def turtlesim_kill():
#     rospy.wait_for_service('/kill')
#     kill_turtle = rospy.ServiceProxy('/kill',Kill)
#     kill_name=kill_turtle("ykp")
#     rospy.loginfo("kill_name = %s",kill_name)

# def turtlesim_clear():
#     rospy.wait_for_service('/clear')
#     clear_turtle = rospy.ServiceProxy('/clear',Empty)
#     rospy.loginfo("clear_name = %s",clear_turtle)
    

if __name__ == "__main__":
    pose1 = Pose()   #给全局变量赋初值
    pose2 = Pose()

    rospy.init_node("turtle_following")

    #turtlesim_kill()   #方便调试  用于杀死上一个乌龟
    #turtlesim_clear()

    #生成海龟
    rospy.wait_for_service('/spawn')
    client = rospy.ServiceProxy('/spawn',Spawn)
    response = client(1.0,1.0,0,"ykp")  #返回name
    rospy.loginfo("turtle_name = %s",response)

    #声明一个发布者   准备向‘ykp’发送消息
    pub = rospy.Publisher('/ykp/cmd_vel',Twist,queue_size=10)

    #订阅乌龟位置和角度 
    rospy.Subscriber('/turtle1/pose',Pose,turtlesimInfoCallback1)
    rospy.Subscriber('/ykp/pose',Pose,turtlesimInfoCallback2)

    rospy.spin()  #类似于while(1)