简介
程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行
实现原理
乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。
启动乌龟显示节点
在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
编写两只乌龟发布坐标信息的节点
编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息
实现流程:C++ 与 Python 实现流程一致
实现流程
新建功能包，添加依赖
编写服务客户端，用于生成一只新的乌龟
编写发布方，发布两只乌龟的坐标信息
编写订阅方，订阅两只乌龟信息，生成速度信息并发布
运行
具体操作步骤
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
服务客户端实现（小乌龟生成代码实现）
/*
创建第二只小乌龟
*/
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
//执行初始化
ros::init(argc,argv,"create_turtle");
//创建节点
ros::NodeHandle nh;
//创建服务客户端
ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
ros::service::waitForService("/spawn");
turtlesim::Spawn spawn;
spawn.request.name = "turtle2";
spawn.request.x = 1.0;
spawn.request.y = 2.0;
spawn.request.theta = 3.12415926;
bool flag = client.call(spawn);
if (flag)
{
ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
}
else
{
ROS_INFO("乌龟2创建失败!");
}
ros::spin();
return 0;
}
发布方实现（发布两只小乌龟位置）
/*
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
控制乌龟运动，将两个坐标系的相对位置动态发布
实现分析:
1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3.将 pose 信息转换成 坐标系相对信息并发布
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 ROS 句柄
4.创建订阅对象
5.回调函数处理订阅到的数据(实现TF广播)
5-1.创建 TF 广播器
5-2.创建 广播的数据(通过 pose 设置)
5-3.广播器发布数据
6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
//声明变量
std::string turtle_name;
void doPose(const turtlesim::Pose::ConstPtr& pose){
//获取位姿信息，转换成坐标系相对关系（最核心），并且发布
// 5-1.创建 TF 广播器
static tf2_ros::TransformBroadcaster broadcaster;
// 5-2.创建 广播的数据(通过 pose 设置)
geometry_msgs::TransformStamped tfs;
// |----头设置
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
// |----坐标系 ID
//tfs.child_frame_id = "turtle1";
tfs.child_frame_id = turtle_name;
// |----坐标系相对信息设置
tfs.transform.translation.x = pose->x;
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
//位姿信息中没有四元数，但是有一个偏航角，又已知乌龟是2D，既没有翻滚与俯仰角
//所以可以得出乌龟的欧拉角： 0 0 theat
// |--------- 四元数设置
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 5-3.广播器发布数据
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"dynamic_tf_pub");
// 3.创建 ROS 句柄
ros::NodeHandle nh;
/*
解析launch文件通过args传入的参数
*/
if (argc !=2)
{
ROS_ERROR("请传入一个参数");
return 1;
}else
{
turtle_name=argv[1];
}
// 4.创建订阅对象
/*
关键点：
1.订阅的话题名称，turtle1或者turtle2动态传入
2.子坐标系动态传入
*/
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name+"/pose",1000,doPose);
// 5.回调函数处理订阅到的数据(实现TF广播)
//
// 6.spin
ros::spin();
return 0;
}
订阅方实现（解析坐标信息并且生成速度信息）
/*
需求:
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:
1.包含头文件
2.初始化 ros 节点
3.创建 ros 句柄
4.创建 TF 订阅对象
5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
解析 son1 中的点相对于 son2 的坐标
6.spin
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/Twist.h"
/*
乌龟速度计算过程：
A. 创建发布对象，用来对于速度进行发布
B. 计算相对距离与相对角度并组织速度消息
组织速度，只需要设置线速度X和角速度Z
X=系数*开方（X^2+Y^2）
Z=系数*反正切（对边，临边）
C. 发布信息
*/
int main(int argc, char *argv[])
{ setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"sub_frames");
// 3.创建 ros 句柄
ros::NodeHandle nh;
// 4.创建 TF 订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
//A. 创建乌龟速度发布节点
ros::Publisher pub=nh.advertise<geometry_msgs::Twist>("/turtle/cmd_vel",100);
ros::Rate r(1);
while (ros::ok())
{
try
{
/*
计算son1与son2的相对关系
A相对于B的坐标关系
参数1：目标坐标系 B
参数2：源坐标系 A
参数3：ros::Time(0) 取间隔最短的两个坐标关系帧计算相对关系
返回值: geometry_msgs : : TransformStamped 源相对于目标坐标系的相对关系
*/
// 解析 son1 中的点相对于 son2 的坐标
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
// ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
// ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
// ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
// tfs.transform.translation.x,
// tfs.transform.translation.y,
// tfs.transform.translation.z
// );
//B. 计算相对速度信息
geometry_msgs::Twist twist;
//长度
twist.linear.x=0.5*sqrt(pow(tfs.transform.translation.x,2)+pow(tfs.transform.translation.y,2));
//角度
twist.angular.z=4*atan2(tfs.transform.translation.x,tfs.transform.translation.y);
//C. 发布
pub.publish(twist);
// // 坐标点解析
// geometry_msgs::PointStamped ps;
// ps.header.frame_id = "son1";
// ps.header.stamp = ros::Time::now();
// ps.point.x = 1.0;
// ps.point.y = 2.0;
// ps.point.z = 3.0;
// geometry_msgs::PointStamped psAtSon2;
// psAtSon2 = buffer.transform(ps,"son2");
// ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
// psAtSon2.point.x,
// psAtSon2.point.y,
// psAtSon2.point.z
// );
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("异常信息:%s",e.what());
}
r.sleep();
// 6.spin
ros::spinOnce();
}
return 0;
}
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="tf_test" type="test01_new_turtle" name="turtle2" output="screen" />
<!-- 3.需要启动两个乌龟相对于世界的坐标关系的发布-->
<!--
基本实现思路：
1.节点只编写一次
2.这个节点需要启动两次
3.节点启动后动态传参：第一次启动传递turtl1,第二次启动传递turtle2
-->
<node pkg="tf_test" type="test01_pub_turtle" name="pub1" args=" turtle1" output="screen" />
<node pkg="tf_test" type="test01_pub_turtle" name="pub2" args=" turtle2" output="screen" />
<node pkg="tf_test" type="test01_control_turtle" name="control" output="screen" />
</launch>
最终实现现象
生成两只小乌龟，并且通过键盘控制小乌龟1运动，小乌龟2会跟随小乌龟1运动，最终两只乌龟坐标重合
