【ROS2】四、使用tf坐标变换实现小海龟跟随

1、创建功能包

创建工作空间并在工作空间下创建功能包：

mkdir -p turtle_tf_ws/src
cd turtle_ws/src
ros2 pkg create --build-type ament_cmake turtle_tf --dependencies rclcpp geometry_msgs tf2 tf2_ros turtlesim

完成该实验所需要的依赖：rclcpp geometry_msgs tf2 tf2_ros turtlesim
功能包创建完成后，就可以在Vscode上编写代码了。

2、编写tf广播器

2.1、生成第一只小海龟

首先打开一个终端，在终端输入代码：

ros2 run turtlesim turtlesim_node

我们要将这第一只小海龟的坐标信息发布出去，首先我们要获取小海龟的位姿，可以通过ros2 topic list查看当前存在的话题。在下图中看出小海龟是通过"/turtle1/pose"这个话题将自己的位姿信息发布出来的。

知道小海龟发布的话题后，我们就可以开始编写代码了。

2.2、代码实现

#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/LinearMath/Quaternion.h"
rclcpp::Node::SharedPtr node=nullptr;
std::string name;
//订阅回调函数
void turtle_callback(const turtlesim::msg::Pose::SharedPtr pose)
{
    //创建广播器对象，没有默认构造，需要将节点对象作为参数传入
    static tf2_ros::TransformBroadcaster broad(node);
    //创建坐标信息对象
    geometry_msgs::msg::TransformStamped tf_pose;
    tf_pose.header.stamp = node->now();
    //主坐标系
    tf_pose.header.frame_id = "world";
    //子坐标系
    tf_pose.child_frame_id = name;
    tf_pose.transform.translation.x = pose->x;
    tf_pose.transform.translation.y = pose->y;
    tf_pose.transform.translation.z = 0.0;
    RCLCPP_INFO(rclcpp::get_logger("pose"),"turtle1 pose : x: %.2f , y: %.2f yaw: %.2f",pose->x,pose->y,pose->theta);
    //四元数对象，用于将欧拉角转换为四元数
    tf2::Quaternion qua;
    //设置欧拉角
    qua.setRPY(0,0,pose->theta);
    //欧拉角转换为四元数
    tf_pose.transform.rotation = tf2::toMsg(qua);
    //广播位置坐标
    broad.sendTransform(tf_pose);
}

int main(int argc, char *argv[])
{
    //初始化
    rclcpp::init(argc,argv);
    //判断输入参数是否合法
    if(argc!=2)
    {
        printf("请输入小海龟名字");
        return -1;
    }
    //给小海龟名字赋值
    name=argv[1];
    //创建一个节点指针
    node = std::make_shared<rclcpp::Node>("turtle_pub_Node");
    //创建订阅者对象，前面必须有对象接收，否则无法订阅到消息
    auto sub = node->create_subscription<turtlesim::msg::Pose>(name+"/pose",100,turtle_callback);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

对CMakeList.txt文件进行修改：

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
ament_target_dependencies(turtle_tf_broadcaster rclcpp std_msgs tf2_ros tf2 geometry_msgs turtlesim)

target_link_libraries(
  turtle_tf_broadcaster
  ${catkin_LIBRARIES})

install(TARGETS
  turtle_tf_broadcaster
  DESTINATION lib/${PROJECT_NAME})

此时切换到终端中，返回工作空间目录下，对功能包进行编译：

colcon build --packages-select turtle_tf

2.3、测试

编译完成后我们就可以测试广播器了。
首先先打开小乌龟节点，创建一直小乌龟：

ros2 run turtlesim turtlesim_node

运行广播器节点：

ros2 run turtle_tf turtle_tf_broadcaster turtle1

效果：

可以看到小乌龟的位姿信息已经被打印出来，也可以再打开一个键盘控制节点，控制小乌龟运动，观察打印的值。

3、创建第二只海龟并实现跟随

3.1、查看控制乌龟运动的话题名称

刚刚我们已经看了乌龟位姿信息是通过"/turtle1/pose"这个话题发布出来的。同理我们通过ros2 topic list命令可以看到还有一个话题跟命令有关的，就是"/turtle1/cmd_vel"，后面我们将通过发布者发布"/turtle1/cmd_vel"话题控制第二只乌龟运动。

3.2、创建乌龟跟随节点

3.3、代码实现

#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
#include "turtlesim/srv/spawn.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include <chrono>
using namespace std::chrono_literals;

int main(int argc, char *argv[])
{
    //创建一个时钟对象，buffer构造时需要作为参数
    rclcpp::Clock::SharedPtr clock;
    //初始化
    rclcpp::init(argc,argv);
    //创建一个节点指针
    auto node = std::make_shared<rclcpp::Node>("listener_node");
    //创建客户端对象，用于生成另一只乌龟
    auto client = node->create_client<turtlesim::srv::Spawn>("spawn");
    //获取当前时钟
    clock = node->get_clock();
    //创建一个buffer对象，用于接受坐标转换后的数据
    tf2_ros::Buffer buffer(clock);
    //创建一个监听器
    auto listener = std::make_shared<tf2_ros::TransformListener>(buffer);
    //等待连接服务器
    while (!client->wait_for_service(1s))
    {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting");
            return 0;
        }
            RCLCPP_INFO(node->get_logger(), "service not available, waiting again...");     
    }
    //创建服务请求对象
    auto req = std::make_shared<turtlesim::srv::Spawn::Request>();
    //生成小海龟
    auto result = client->async_send_request(req);
    //创建一个发布者对象，用于发布第二只小海龟的运动命令信息
    auto pub = node->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel");
    //判断是否能实现坐标变换
    buffer.canTransform("turtle2","turtle1",tf2::TimePoint(),tf2::durationFromSec(1.0));
    //频率10Hz
    rclcpp::WallRate r(10);
    //循环监听
    while (rclcpp::ok())
    {
        //创建坐标消息对象，存放转换后的坐标信息
        static geometry_msgs::msg::TransformStamped tur1_;
        //监听，返回turtlr2相对turtle1的坐标
        tur1_ = buffer.lookupTransform("turtle2","turtle1",tf2::TimePoint());
        //创建小海龟控制消息对象，用于控制第二只小海龟运动
        geometry_msgs::msg::Twist vel;
        //计算角度和线速度
        vel.angular.z = atan2(tur1_.transform.translation.y,tur1_.transform.translation.x);
        vel.linear.x = 0.5*sqrt(pow(tur1_.transform.translation.x,2)+pow(tur1_.transform.translation.y,2));
        //发布消息
        pub->publish(vel);
        r.sleep();
    }
    rclcpp::shutdown();
    return 0;
}

我们通过服务器来创建第二只乌龟，然后创建一个发布者对象，通过坐标变换监控器监控第一只乌龟与第二只乌龟的坐标变化，将获取到的第一只乌龟与第二只乌龟的相对坐标计算出第二只乌龟的航向角和线速度，从而控制第二只乌龟跟随第一只乌龟。
修改CMakeList.txt文件：

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
ament_target_dependencies(turtle_tf_broadcaster rclcpp std_msgs tf2_ros tf2 geometry_msgs turtlesim)
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
ament_target_dependencies(turtle_tf_listener rclcpp std_msgs tf2_ros tf2 geometry_msgs turtlesim)

target_link_libraries(
  turtle_tf_broadcaster
  ${catkin_LIBRARIES})

install(TARGETS
  turtle_tf_broadcaster
  turtle_tf_listener
  DESTINATION lib/${PROJECT_NAME})

此时切换到终端中，返回工作空间目录下，对功能包进行编译：

colcon build --packages-select turtle_tf

3.4、测试

这时候整个工程已经完成了，接下来就是测试功能了。

首先创建第一只乌龟，打开第一只乌龟和第二只乌龟的监控器：

ros2 run turtle_tf turtle_tf_broadcaster turtle1

在另一个终端中：

ros2 run turtle_tf turtle_tf_broadcaster turtle2

在打开一个终端，运行乌龟跟随节点：

ros2 run turtle_tf turtle_tf_listener

运行乌龟键盘控制节点：

ros2 run turtlesim turtle_teleop_key

在第三步的时候就能看到界面上出现了两只乌龟，并且第二只乌龟向第一只乌龟运动，最终到达第一只乌龟的位置。

通过键盘控制乌龟运动，可以看到第二只乌龟一直跟着第一只乌龟跑。

也可以打开rviz2查看两个坐标之间的关系：