5.6 坐标变换实操
5.6 坐标变换实操
本节主要介绍如何实现”乌龟跟随“以及“乌龟护航”案例。
5.6.1 乌龟跟随案例以及分析
1.案例需求
需求：编写程序实现，程序运行后会启动 turtlesim_node 节点，该节点会生成一个窗口，窗口中有一只原生乌龟（turtle1），紧接着再生成一只新的乌龟（turtle2），无论是 turtle1 静止或是被键盘控制运动时，turtle2 都会以 turtle1 为目标并向其运动。
2.案例分析
“乌龟跟随”案例的核心是如何确定 turtle1 相对 turtle2 的位置，只要该位置确定就可以把其作为目标点并控制 turtle2 向其运动。相对位置的确定可以通过坐标变换实现，大致思路是先分别广播 turtle1 相对于 world 和 turtle2 相对于 world 的坐标系关系，然后再通过监听坐标系关系进一步获取 turtle1 相对于 turtle2 的坐标系关系。
3.流程简介
案例实现主要步骤如下：
编写程序调用 /spawn 服务生成一只新乌龟；
编写坐标变换广播实现，通过该实现可以广播 turtle1 相对于 world 和 turtle2 相对于 world 的坐标系关系；
编写坐标变换监听实现，获取 turtle1 相对于 turtle2 的坐标系关系并生成控制 turtle2 运动的速度指令；
编写 launch 文件集成相关节点；
编辑配置文件；
编译；
执行。
案例我们会采用 C++ 和 Python 分别实现，二者都遵循上述实现流程。
4.准备工作
1.新建功能包
终端下进入工作空间的src目录，调用如下两条命令分别创建C++功能包和Python功能包。
ros2 pkg create cpp05_exercise --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim
ros2 pkg create py05_exercise --build-type ament_python --dependencies rclpy tf_transformations tf2_ros geometry_msgs turtlesim
2.创建launch目录
功能包cpp05_exercise和py05_exercise下分别新建launch文件，并编辑配置文件。
功能包cpp05_exercise的 CMakeLists.txt 文件添加如下内容：
功能包py05_exercise的 setup.py 文件中需要修改 data_files 属性，修改后的内容如下：
5.6.2 乌龟跟随实现（C++）
1.编写生成新乌龟实现
功能包 cpp05_exercise 的 src 目录下，新建 C++ 文件 exer01_tf_spawn.cpp，并编辑文件，输入如下内容：
2.编写坐标变换广播实现
功能包 cpp05_exercise 的 src 目录下，新建 C++ 文件 exer02_tf_broadcaster.cpp，并编辑文件，输入如下内容：
3.编写坐标变换监听实现
功能包 cpp05_exercise 的 src 目录下，新建 C++ 文件 exer03_tf_listener.cpp，并编辑文件，输入如下内容：
4.编写 launch 文件
launch 目录下新建文件exer01_turtle_follow.launch.py，并编辑文件，输入如下内容：
7.执行
当前工作空间下启动终端，输入如下命令运行launch文件：
. install/setup.bash
ros2 launch cpp05_exercise exer01_turtle_follow.launch.py
再新建终端，启动 turtlesim 键盘控制节点：
ros2 run turtlesim turtle_teleop_key
该终端下可以通过键盘控制 turtle1 运动，并且 turtle2 会跟随 turtle1 运动。最终的运行结果与演示案例类似。
5.6.3 乌龟跟随实现（Python）
1.编写生成新乌龟实现
功能包 py05_exercise 的 py05_exercise 目录下，新建 Python 文件 exer01_tf_spawn_py.py，并编辑文件，输入如下内容：
2.编写坐标变换广播实现
功能包 py05_exercise 的 py05_exercise 目录下，新建 Python 文件 exer02_tf_broadcaster_py.py，并编辑文件，输入如下内容：
功能包 py05_exercise 的 py05_exercise 目录下，新建 Python 文件 exer03_tf_listener_py.py，并编辑文件，输入如下内容：
4.编写 launch 文件
launch 目录下新建文件 exer01_turtle_follow.launch.xml，并编辑文件，输入如下内容：
5.编辑配置文件
1.package.xml
在创建功能包时，所依赖的功能包已经自动配置了，配置内容如下：
5.6.4 乌龟护航案例以及分析
1.案例需求
需求：生成多只乌龟，以 turtlesim_node 的原生乌龟（turtle1）为中心，无论 turtle1 是静止或运动时，都以某种队形为 turtle1 护航。
2.案例分析
“乌龟护航”案例的核心本质还是坐标变换，如果要围绕 turtle1 组成某个固定队形，那么可以把队形中每个点位看作一个坐标系，这些坐标系相对于 turtle1 坐标系的关系是静态的，可以通过静态广播器发布这些点与 turtle1 的坐标系关系，发布后在整个坐标系关系树中，点坐标系到 turtle1坐标系、turtle1坐标系到 world 坐标系、护航的乌龟坐标系到 world 坐标系的关系都是已知的，那么就可以换算出护航的乌龟坐标系与其对应的点坐标系的相对关系，进而就可以计算并发布控制护航乌龟运动的速度指令了。
3.流程简介
本案例中生成新的乌龟、坐标变换广播与监听都已实现，直接编写 launch 文件组织节点即可，主要步骤如下：
编写 launch 文件集成相关节点；
编译；
执行。
案例我们会采用 C++ 和 Python 分别实现，二者都遵循上述实现流程。
5.6.5 乌龟护航实现（C++）
1.编写 launch 文件
功能包 cpp05_exercise 的 launch 目录下新建文件exer02_turtle_escort.launch.py，并编辑文件，输入如下内容：
5.6.6 乌龟护航实现（Python）
1.编写 launch 文件
功能包 py05_exercise 的 launch 目录下新建文件exer02_turte_escort.launch.xml，并编辑文件，输入如下内容：
5.7 本章小结
本章主要介绍了ROS2工具中的一个重要模块：坐标变换。核心内容如下：
坐标变换相关消息：
geometry_msgs/msg/TransformStamped和geometry_msgs/msg/PointStamped；坐标变换广播：静态广播、动态广播和坐标点发布；
坐标变换监听：坐标系变换、坐标点变换。
坐标变换本质还是消息的发布与订阅，所以其框架与话题通信类似，也是由接口消息、发布方（广播）、订阅方（监听）三个要素组成的。与一般的话题通信不同的是，一般话题通信的消息发布方之间是孤立的、没有内在联系，而坐标变换的多个广播对象广播的消息需要被保存进“缓存”，这些消息可以通过坐标系id和时间戳关联到一起，然后监听对象则可以通过这些内在关联处理缓存中的数据。除此之外，本章还介绍了坐标变换中一些比较实用的工具，并且还通过“乌龟跟随”与“乌龟护航”案例强化了坐标变换相关API的使用。
B站有完整的ros系列教程视频，可以观看完整内容ros课程ROS2理论与实践
更多内容将在猛狮知识星球社区更新最新课程，后续将推出更多优质内容——详情可关注猛狮集训营公众号和猛狮集训营官方网站。
