ROS TF 变换实现小乌龟跟随

实现分析：

核心是生成两只乌龟（采用服务的形式在窗体中生成一只新的乌龟），乌龟A和乌龟B都要发布相对于世界坐标系的坐标信息，然后订阅到该消息转换获取A相对于B坐标系的信息，最后生成速度信息，发布并控制B运动。其中乌龟A受键盘控制节点控制，乌龟B不受键盘控制节点控制，此处应该注意键盘控制节点发布的话题名。

创建功能包

相关依赖：tf2、tf2_ros、tf2_geometry_msgs、roscpp、rospy、std_msgs、geometry_msgs、turtlesim。（tf2、tf2_ros、tf2_geometry_msgs是与tf2坐标变换有关的依赖）

mkdir -p turtle_ws/src
cd turtle_ws/src
catkin_create_pkg turtle_follow tf2 tf2_ros roscpp rospy std_msgs geometry_msgs turtlesim    // 创建乌龟跟随功能包

生成两只乌龟：

生成第二只乌龟的方法可以通过编码方式或者命令行调用的方式生成。

命令行的方式：

编码的方式：

上面已经创建生成第二只乌龟的功能包，下面直接附上代码。

#include "ros/ros.h"
#include "turtlesim/Spawn.h"   

int main(int argc, char *argv[])
{
    /*
        1、初始化ros节点
        2、创建ros句柄
        3、创建service客户端
        4、等待服务启动
        5、发送请求
        6、处理响应结果（返回值是bool类型）
    */
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"turtle_born");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 等待服务启动，两种方式均可。
    //client.waitForExistence(); 
    ros::service::waitForService("/spawn");
    // 创建服务对象，并组织发布信息
    turtlesim::Spawn spawn;
    spawn.request.name  = "turtle2";
    spawn.request.x     = 2.0;
    spawn.request.y     = 1.0;
    spawn.request.theta = 0.0;
    // 处理响应结果，bool类型
    bool flag = client.call(spawn);
    if (flag)
    {
        ROS_INFO("新的乌龟已经生成，名字是：%s", spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("新乌龟生成失败！");
    }

    return 0;
}

发布方（发布两只乌龟的坐标信息）

实现逻辑，订阅两只乌龟的位姿信息，然后再转换成坐标信息，两只乌龟实现逻辑相同，可以使用launch文件传入参数的方式实现两只小乌龟。

代码如下：

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"

std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr& pose)  // 注意回调函数形参编写方式
{
    // 创建tf广播器，注意是static。目的是防止每次调用回调函数的时候都创建一个发布对象。
    static tf2_ros::TransformBroadcaster broadcaster;
    // 将pose信息转换成TransformStamped
    geometry_msgs::TransformStamped tfs;

    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = turtle_name;
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    
    // 利用欧拉角转四元数
    tf2::Quaternion quat2eular;
    quat2eular.setRPY(0,0,pose->theta);
    tfs.transform.rotation.w = quat2eular.getW();
    tfs.transform.rotation.x = quat2eular.getX();
    tfs.transform.rotation.y = quat2eular.getY();
    tfs.transform.rotation.z = quat2eular.getZ();
    
    // 数据组织完毕以后，发送数据
    broadcaster.sendTransform(tfs); // 数据已经封装在tfs这个对象中了。
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");   // 设置编码，防止ROS_INFO打印乱码。
    ros::init(argc, argv, "pose_sub");
    ros::NodeHandle nh;
    // 解析传入的命名空间，launch文件的参数设置
    if(argc != 2)           // 判断参数个数是否正确
    {
        ROS_ERROR("请传入正确的参数");
    }
    else
    {
        turtle_name = argv[1];
        ROS_INFO("乌龟%s坐标发送启动", turtle_name.c_str());
    }
    
    // 创建订阅乌龟位姿的对象
    ros::Subscriber poseSub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose", 1000, doPose);
    //下面编写回调函数处理订阅到的位姿信息，病转换为坐标发布出去。

    ros::spin();  // 有回调函数必有spin()

    return 0;

}

订阅方（解析坐标信息，并生成速度信息）

实现逻辑：订阅乌龟A和乌龟B的TF广播信息，查找并转换时间最近的TF信息。

将乌龟A转换成相对乌龟B的坐标，在计算线速度和角速度并发布。线速度和角速度的计算方法如下：

/*
     这个可以将两个坐标点之间的距离当做斜边，做直角三角形
     x = 系数 * 开方(x^2 + y^2)
     z = 系数 * 反正切(对边，邻边)        系数是经验值。
*/
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.y, tfs.transform.translation.x);

整体代码如下：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "control_pub");
    ros::NodeHandle nh;
    
    // 创建tf订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    // 创建速度发布的对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);

    ros::Rate rate(10); // 10Hz速度发布
    // 处理订阅到的TF信息，这里采用try语句
    while(ros::ok())
    {
        try
        {
            /* 
                先获取turtle1相对于turtle2的位置信息
                lookupTransform()函数
                参数1： 目标坐标系
                参数2： 源坐标系
                返回值：geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系
                时间为ros::Time(0)，即寻求最近时刻的tf转换信息
            */
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

            // 根据坐标信息生成速度信息
            /*
                这个可以将两个坐标点之间的距离当做斜边，做直角三角形
                x = 系数 * 开方(x^2 + y^2)
                z = 系数 * 反正切(对边，邻边)        系数是经验值。
            */
            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.y, tfs.transform.translation.x);
            
            // 发布速度信息
            pub.publish(twist); 
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示：%s", e.what());
        }
        
        rate.sleep();      // 休眠
        ros::spinOnce();   // 注意此处是ros::spinOnce()与while配合使用
    }

    return 0;
}

创建的launch文件如下:

<?xml version="1.0"?>
<launch>

    <!-- 启动乌龟节点与键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    <!-- 启动创建第二只乌龟的节点 -->
    <node pkg="turtle_follow" type="turtle_born" name="turtle2" output="screen" />
    <!-- 启动两个坐标发布节点 -->
    <node pkg="turtle_follow" type="pose_sub" name="tf_pub1" output="screen" args="turtle1" />
    <node pkg="turtle_follow" type="pose_sub" name="tf_pub2" output="screen" args="turtle2" />
    <!-- 启动坐标转换节点 -->
    <node pkg="turtle_follow" type="control_pub" name="listener" output="screen" />

</launch>

实验结果：

 设想：

跟随的速度算法是否可以优化？