不使用ros-TF功能包实现小海龟跟随，并记录跟随海龟运动路程

本文主要是对

ROS海龟跟随（坐标变换）

（https://blog.csdn.net/weixin_45842867/article/details/125434346）

文章中的内容进行了简单删改，并针对新手添加了基础的说明

1.各文件详细代码

功能包创建完成后，首先在包内新建一个scripts文件夹，放置python文件

1. new_turtle.py

#! /usr/bin/env python
#coding:utf-8

#1.导包
import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse

if __name__ == "__main__":
    # 2.初始化 ros 结点
    rospy.init_node("service_call_p")
    # 3.创建服务客户端
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.等待服务启动
    req = SpawnRequest()
    # 5.创建请求数据
    # 在（1，1）位置生成小海龟2
    req.x = 1.0
    req.y = 1.0
    req.theta = 0
    req.name = "turtle2"
    client.wait_for_service()
    # 6.发送请求并处理响应
    try:
        response = client.call(req)
        rospy.loginfo("乌龟创建成功，名字是:%s",response.name)
    except Exception as e:
        rospy.logerr("服务调用失败....")

主要功能为：在指定位置生成小海龟

2. control.py

#!/usr/bin/env python
#coding:utf-8

#注意：前两行注释可以识别
#程序功能：第二个乌龟跟随第一个乌龟运动
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
global last_pose

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 dis_sum 
    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)

    distance = math.sqrt((last_pose.x-pose2.x)*(last_pose.x-pose2.x)+(last_pose.y-pose2.y)*(last_pose.y-pose2.y))
    dis_sum = distance+dis_sum
    last_pose.x = pose2.x
    last_pose.y = pose2.y


    rospy.loginfo("距离和: %f ",dis_sum)  #距离和

    pub.publish(vel_msg)   #发布速度信息 使第二个乌龟运动


if __name__ == "__main__":
    pose1 = Pose()   #给全局变量赋初值
    pose2 = Pose()
    #为计算运行总里程而添加
    last_pose = Pose()
    last_pose.x = 1
    last_pose.y = 1
    dis_sum = 0


    rospy.init_node("control")

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

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

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

说明见：https://blog.csdn.net/weixin_45842867/article/details/125434346

乌龟运动路程：上一时刻乌龟位置与这一时刻乌龟位置的累加和

3. turtle.launch

<launch>
    <!-- 1、准备工作：启动乌龟的 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="test_pkg" type="new_turtle.py" name="turtle2" output="screen"/>
    <!-- 3、订阅两只海龟的位置信息，并发送速度指令;  -->
    <node pkg="test_pkg" type="control.py" name="control" output="screen"/>
</launch>

4. CMakeLists.txt

## Mark executable scripts (Python etc.) for installation
## in contrast to getsetup.py, you can choose the destination

将含有该注释的代码段修改为

## Mark executable scripts (Python etc.) for installation
## in contrast to getsetup.py, you can choose the destination
catkin_install_python(PROGRAMS
  scripts/control.py
  scripts/new_turtle.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

5. package.xml

该文件未作修改

---

2.调试过程中遇到的问题

在运行launch文件后，发生报错

ModuleNotFoundError: No module named 'rospkg'

（小乌龟并不会进行跟随）

查阅资料可知，原因为：

base 环境下缺少rospkg包,所以需要安装rospkg包

但针对该问题，因

第一行的路径是指定运行脚本的解释器的位置

第二行指定Encoding

原代码为：

#!/usr/bin/env python3
#coding:utf-8

修改为：

#!/usr/bin/env python
#coding:utf-8

对第一行路径进行了修改，重新启动launch文件，报错消失，运行结果如图