所用的学习链接：

【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程P204-

【以上视频笔记见http://www.autolabor.com.cn/book/ROSTutorials/】

一、目标

产生两只乌龟 A 和 B ，B 会自动运行至A的位置。

键盘只控制 A 的运动，而 B 跟随 A 运动。

乌龟跟随实现的核心，是乌龟 A 和 B 都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

1. 启动乌龟显示节点
2. 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
3. 编写两只乌龟发布坐标信息的节点
4. 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

二、准备工作

1.创建环境

① 创建工作空间

# mkdir -p 空间名称/src   # 必须得有 src
mkdir -p 7.24_turtle/src  
cd 7.24_turtle
catkin_make# 启动
code .

② 创建功能包 tf1_turtle

右键src ---> create catkin package→自定义package name（例 tf1_turtle）→dependencies：roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim

roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim

如果有包缺失的话可以调用以下代码加载： 

# sudo apt-get install ros-<ros版本>-包名
# 包名中的 下划线（_）要换成杠（-）
# sudo apt-get install ros-noetic-包名

ctrl+shift+b 编译运行，选择 catkin_make：build，看是否报错，没报错则依赖包没有出现问题。

③ 创建文件夹

在 tf1_turtle 目录下新建文件夹 launch(存放launch文件)、scripts(存放python文件)

④ 创建 t1_turtle2.cpp 文件

在 src 文件夹下新建文件 t1_turtle2.cpp，用于生成一只新的小乌龟

// 1.头文件
#include "ros/ros.h"
#include "turtlesim/Spawn.h"/* 
目标：向服务器发送请求，生成一只新的乌龟话题：/spawn消息：turtlesim/Spawn
1.头文件
2.初始化 ROS 节点
3.创建节点句柄
4.创建客户端对象
5.组织数据并发送
6.处理响应 
*/int main(int argc, char *argv[])
{setlocale(LC_ALL,"");// 2.初始化 ROS 节点ros::init(argc,argv,"service_call");// 3.创建节点句柄ros::NodeHandle nh;// 4.创建客户端对象ros::ServiceClient client= nh.serviceClient<turtlesim::Spawn>("/spawn");// 5.组织数据并发送//   5-1.组织请求数据//      创建乌龟turtlesim::Spawn spawn;//      乌龟坐标spawn.request.x=1.0;spawn.request.y=4.0;//      转向 90 度spawn.request.theta=1.57;//      乌龟名字spawn.request.name="turtle2";//   5-2.发送请求//      判断服务器状态//      ros::service::waitForService("/spawn");client.waitForExistence();//      flag 接收响应状态，响应结果也会被设置进 spawn 对象bool flag=client.call(spawn);  // 6.处理响应if(flag){ROS_INFO("乌龟生成成功，新乌龟较：%s",spawn.response.name.c_str());}else{ROS_INFO("请求失败！");}return 0;
}

⑤ 修改 CMakeLists.txt 文件

修改 tf1_turtle 目录下的 CMakeLists.txt 文件

# 原句子
# 1.add_executable(${PROJECT_NAME}_node src/tf1_turtle_node.cpp)
# 2.add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# 3. target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )# 修改为
# 1.
add_executable(t1_turtle2 src/t1_turtle2.cpp)
# 2.
add_dependencies(t1_turtle2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# 3.
target_link_libraries(t1_turtle2${catkin_LIBRARIES}
)

⑥ 创建 t1_turtle1.launch 文件

在 launch 文件夹下新建文件 t1_turtle1.launch

<launch><!-- 启动乌龟GUI节点 --><node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"/><!-- 键盘控制节点 --><!-- 也可以不加这一行，启动后新建终端，运行rosrun turtlesim turtle_teleop_key来启动键盘--><!-- 发布的是关于第一只乌龟的运动消息，因此只能控制第一只 --><node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen"/><!-- 生成新乌龟的节点 --><node pkg="tf1_turtle" type="t1_turtle2" name="turtle2" output="screen"/>
</launch>

这里我因为 type 写错，出现过 RLException: Unable to launch 的错误，注意检查

 ⑦ 给予文件执行权限

在 7.24_turtle / src / tf1_turtle / src 目录下打开终端，运行

# 使文件成为非root用户也可执行的文件
chmod +x *.cpp

ctrl+shift+b 编译运行，选择 catkin_make：build，看是否报错，没报错则依赖包没有出现问题。

2. 测试运行

roslaunch tf1_turtle t1_turtle1.launch

弹出一个窗口，上面有两只小乌龟。

选中运行 launch 文件的终端窗口为当前窗口，上下左右可以控制小乌龟运动。

三、 消息发布和订阅

（1）新建 t2_pub_turtle.cpp（发布）

#include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/* 
发布方：订阅乌龟的位姿信息，转换成相对于窗体的坐标系并发布话题：/turtle/pose消息：/turtlesim/Pose1.头文件
2.设置编码、初始化 ros 节点
3.创建订阅对象，订阅 /turtlex/pose
4.回调函数处理订阅的 pose 信息将 pose 信息转换成 TransFormStamped发布
5.spin*/// 声明变量接收传递的参数
std::string turtle_name;void doPose(const turtlesim::Pose::ConstPtr &pose)
{// 获取位姿信息，转换成坐标系相对关系（核心），并发布// a.创建TF发布对象static tf2_ros::TransformBroadcaster pub;// b.组织被发布的数据geometry_msgs::TransformStamped ts;ts.header.frame_id = "world";ts.header.stamp = ros::Time::now();// 关键动态参数// ts.child_frame_id="turtle1";ts.child_frame_id = turtle_name;// 坐标系偏移量设置ts.transform.translation.x = pose->x;ts.transform.translation.y = pose->y;ts.transform.translation.z = 0;// 坐标系四元数/* 位姿信息中没有四元数，但有偏航角度。乌龟为 2D,无翻滚和俯仰角度所以乌龟欧拉角：0 0 theta*/tf2::Quaternion qtn;qtn.setRPY(0, 0, pose->theta);ts.transform.rotation.x = qtn.getX();ts.transform.rotation.y = qtn.getY();ts.transform.rotation.z = qtn.getZ();ts.transform.rotation.w = qtn.getW();// c.发布pub.sendTransform(ts);
}int main(int argc, char *argv[])
{// 2.设置编码、初始化 ROS节点setlocale(LC_ALL, "");ros::init(argc, argv, "dynamic_pub");ros::NodeHandle nh;// 解析通过launch传入的arg参数if (argc != 2){ROS_ERROR("请传入一个参数");return 1;}else{turtle_name = argv[1];}// 3.创建订阅对象，订阅/turtlex/pose// 关键动态参数// ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);ros::Subscriber sub = nh.subscribe(turtle_name + "/pose", 100, doPose);// 4.回调函数处理订阅的 pose 信息ros::spin();return 0;
}

（2）新建 t3_control_turtle2.cpp（订阅）

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"/* 
订阅方：计算 turtle1 和 turtle2 的 相对关系计算 角速度和线速度并发布1.头文件
2.编码、初始化、NodeHandle
3.创建订阅对象
4.编写解析逻辑
5.spinOnce()*/int main(int argc, char *argv[])
{// 2.编码、初始化、NodeHandlesetlocale(LC_ALL, "");ros::init(argc, argv, "tfs_sub");ros::NodeHandle nh;// 3.创建订阅对象tf2_ros::Buffer buffer;tf2_ros::TransformListener sub(buffer);// A.创建发布对象ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 100);// 4.编写解析逻辑ros::Rate rate(10);while (ros::ok()){// 核心try{// 1.计算 turtle1 和 turtle2 的 相对关系/* 1 相对于 2 的坐标系关系参数1：目标坐标系 B参数2：源坐标系   A参数：ros::Time(0) 取间隔最短的两个坐标系关系帧计算相对关系返回值：geometry_msgs::TransformStamped 源相对于坐标系的相对关系*/geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2", "turtle1", ros::Time(0));// ROS_INFO("son1 相对于 son2 的信息：父级%s,子级：%s 偏移量(%.2f,%.2f,%.2f)",//         son1ToSon2.header.frame_id.c_str(),  //turtle2//         son1ToSon2.child_frame_id.c_str(),   //turtle1//         son1ToSon2.transform.translation.x,//         son1ToSon2.transform.translation.y,//         son1ToSon2.transform.translation.z//         );// B.根据相对计算并组织速度消息geometry_msgs::Twist twist;// 线速度，不会横着y，不会飞z，只有x前后// x = 系数 * 开方( y ^ 2 + x ^ 2 )twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x, 2) + pow(son1ToSon2.transform.translation.y, 2));// 角速度，没有打滚x,没有抬头低头y,只有左右转z// z = 系数 * 反正切( 对边 ， 临边 )twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y, son1ToSon2.transform.translation.x);// C.发布spub.publish(twist);}catch (const std::exception &e){ROS_INFO("错误提示:%s", e.what());}rate.sleep();ros::spinOnce();}// 5.spinOnce()return 0;
}

（3）修改 CMakeLists.txt

修改 tf1_turtle 目录下的 CMakeLists.txt 文件

（4）修改 t1_turtle1.launch 文件

<launch><!-- 1.启动乌龟GUI节点 --><node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" /><!-- 键盘控制节点 --><!-- 也可以不加这一行，启动后新建终端，运行rosrun turtlesim turtle_teleop_key来启动键盘--><!-- 发布的是关于第一只乌龟的运动消息，因此只能控制第一只 --><node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" /><!-- 2.生成新乌龟的节点 --><node pkg="tf1_turtle" type="t1_turtle2" name="turtle2" output="screen" /><!-- 3.启动2个乌龟相对于世界的坐标关系的发布因为代码内容相似，因此用一个节点。对该节点启动2次，分别传参 turtle1 和 turtle2 --><node pkg="tf1_turtle" type="t2_pub_turtle" name="pub1" args="turtle1" output="screen" /><node pkg="tf1_turtle" type="t2_pub_turtle" name="pub2" args="turtle2" output="screen" /><!-- 4.订阅坐标信息，转换成 乌龟A 相对于 乌龟B 的坐标信息，再生成控制 乌龟B 的速度信息 --><node pkg="tf1_turtle" type="t3_control_turtle2" name="control" output="screen" /></launch>

四、运行乌龟跟随

roslaunch tf1_turtle t1_turtle1.launch

操控键盘，第二只小乌龟将一直跟随第一只移动。距离越远，第二只小乌龟的加速越大