ROS学习笔记12——tf坐标变换

以ros自带的小乌龟作为案例的载体，完成通信方式、坐标变换等的练习，实现程序启动之初 产生两只乌龟，中间的乌龟(turtle1) 和 左下乌龟(turtle2), turtle2 会自动运行至turtle1的位置，并且键盘控制时，只是控制 turtle1 的运动，但是 turtle2 可以跟随 turtle1 运行。

1、创建功能包

创建项目功能包依赖于roscpp、rospy、std_msgs、 tf2、tf2_ros、tf2_geometry_msgs、geometry_msgs、turtlesim

2、服务客户端（生成新的小乌龟turtle2）

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*
    需求：向服务端发送请求，生成一只新乌龟
    话题：/spawn -> 通过rosservice list看到
    消息类型：turtlesim/Spawn -> 通过rosservice info /spawn看到
    消息的具体内容：。。。 -> 通过rossrv info turtlesim/Spawn看到
*/

int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL, "");
    ros::init(argc, argv,"create_new_turtle");
    ros::NodeHandle nh;
    
    //创建服务对象
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    //组织数据
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 1.57;
    client.waitForExistence();//判断服务器状态

    //发送请求
    bool flag = client.call(spawn);//flag用来接收响应状态的，响应结果也会被设置进spawn对象
    if(flag)
    {
        ROS_INFO("新乌龟 %s 成功生成", spawn.response.name.c_str());
    }
    else{
        ROS_INFO("新乌龟生成失败");
    }
    ros::spin();
    return 0;
}

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

可以订阅乌龟的位姿信息，然后转换成坐标系关系，两只乌龟的实现逻辑是相同的，修改订阅话题和消息类型之后，新建一个cpp重复实现一遍逻辑当然是可以的，但是加大了代码量，并且降低了代码的复用率。这些细微的差异可以通过args设置参数来传入：

• 该节点需要启动两次
• 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
发布方实现
    需求：订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
    准备：
        话题：/turtle1/pose、/turtle2/pose
        消息：turtlesim/Pose
    
    流程：
        1、包含头文件
        2、设置编码、初始化、NodeHandle等
        3、创建订阅对象、订阅话题：/turtle1/pose、/turtle2/pose
        4、**回调函数，处理订阅的消息：把订阅的位姿信息转换成坐标系相对关系，并发布
        5、spin()
*/

//声明接收传递参数的变量
std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr &pose)
{
    //获取位姿信息，转换成坐标系相对关系，并发布
    //5.1 创建发布对象
    static tf2_ros::TransformBroadcaster pub;   //设为static静态变量，每次回调使用同一个pub对象
    //5.2 组织被发布的数据  
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    //**子级坐标系名称动态传入
    ts.child_frame_id = turtle_name;
    //坐标系偏移量
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;

    tf2::Quaternion qnt;
    qnt.setRPY(0,0,pose->theta);
    ts.transform.rotation.w = qnt.getW();
    ts.transform.rotation.x = qnt.getX();
    ts.transform.rotation.y = qnt.getY();
    ts.transform.rotation.z = qnt.getZ();
    
    //5.3 发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    /* code */
    /*
        解析 launch 文件通过 args 传入的参数
    */
    if(argc != 4)
    {
        ROS_INFO("请传入一个参数");
        return 1;
    }else
    {
        turtle_name = argv[1];
    }

    setlocale(LC_ALL,"");
    // 2、设置编码、初始化、NodeHandle等
    ros::init(argc,argv,"dynamic_pub_turtle");
    ros::NodeHandle nh;
    // **3、创建订阅对象、订阅话题：/turtle1 或者 turtle2 是动态传入的
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",100,doPose);
    // 4、**回调函数，处理订阅的消息：把订阅的位姿信息转换成坐标相对关系，并发布
    // 5、spin()    
    ros::spin();
    return 0;
}

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

#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"
/*
    需求1：换算出 turtle1 相对于 turtle2 的关系
    需求2：计算角速度和线速度并发布
*/
int main(int argc, char *argv[])
{
    /* code */
    setlocale(LC_ALL, "");
    // 2、编码、初始化、NodeHandle
    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::Duration(2).sleep();
    ros::Rate rate(10);
    while(ros::ok())
    {

        try
        {
        // １、计算 turtle1 和 turtle2 之间的相对关系
        /* lookupTransform(参数1：B，参数2：A，参数3)
            ## A 相对于 B 的坐标系关系
            参数1："目标坐标系 B"
            参数2："源坐标系 A"
            参数3：ros::Time(0) 取间隔最短的两个坐标系帧计算相对关系
            返回值：geometry_msgs::TransformStamped
        */
        geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
        ROS_INFO("turtle1 相对于 turtle2 的信息：父级名称:%s,子级名称:%s,偏移量:(%.2f,%.2f,%.2f)",
                    tfs.header.frame_id.c_str(), //turtle2
                    tfs.child_frame_id.c_str(),   //turtle1
                    tfs.transform.translation.x,
                    tfs.transform.translation.y,
                    tfs.transform.translation.z
                    ); 
        //B、根据相对位置计算并组织速度消息
        geometry_msgs::Twist twist;
        /*
           linear.x = 系数 * 开放(y^2 + x^2)
           angular.z = 反正切(对边，临边)
        */
        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);

        //C、发布
        pub.publish(twist);

        }
        catch(const std::exception& e)
        {
            //std::cerr << e.what() << '\n';
            ROS_INFO("异常消息：%s", e.what());
        }
        
        rate.sleep();
        // 5、spinOnce()
        ros::spinOnce();
    }
    return 0;
}

5、运行

使用 launch 文件组织需要运行的节点

<launch>
    <!-- 启动小乌龟GUI -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <!-- 键盘控制节点 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
    <!-- 创建新的小乌龟 -->
    <node pkg="tfs_test" type="create_new_turtle" name="turtle2" output="screen" />

    <!-- 需要启动两个乌龟相对于世界坐标系的坐标关系的发布节点 -->
    <!-- 实现思路：
            1、只编写一个节点
            2、这个节点需要启动两次
            3、节点启动时，动态传参：
                        第一次：turtle1
                        第二次：turtle2
     -->
    <node pkg="tfs_test" type="pub_turtle" name="pub_turtle1" args="turtle1" output="screen" />
    <node pkg="tfs_test" type="pub_turtle" name="pub_turtle2" args="turtle2" output="screen" />
    
    <!-- 订阅turtle1与 turtle2 相对于世界坐标系的坐标关系
         并生成 turtle1 相对于 turtle2 的坐标关系
         再生成速度消息 -->
    <node pkg="tfs_test" type="control_turtle2" name="control" output="screen" />
</launch>

6、记得配置CMakeLists