ROS -- 坐标变换实操--两只乌龟坐标变换

C++ ：

注：代码更多是前面的例子做简单修改

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 = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen" />

    <!-- 3. 需要启动2个乌龟相对于世界的坐标关系发布-->
    <!--
        基本实现思路：
            1. 节点只编写一个
            2. 这个节点需要启动2次
            3. 节点启动时动态传参：第一次启动传递 turtle1 第二次启动传递 turtle2
    -->
    <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub1" args = "turtle1" output = "screen" />
    <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub2" args = "turtle2" output = "screen" />

    <!--需要订阅 turtle1 与 turtle2 的相对于世界坐标的坐标消息，并转换成 turtle1 相当于 turtle2 的坐标关系，再生成速度消息-->
    <node pkg = "tf04_test" type = "test03_control_turtle2" name = "control" output = "screen" />

</launch>

生成2只乌龟：

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

/*
    需求：是向服务端发送请求,生成一只新乌龟
        话题：/spawn
        消息：turtlesim/Spawn
    1.包含头文件
    2.初始化ROS节点
    3.创建节点句柄
    4.创建客户端对象
    5.组织数据并发布
    6.处理响应
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化ROS节点
    ros::init(argc,argv,"service_call");
    // 3.创建节点句柄
    ros::NodeHandle nh;
    // 4.创建客户端对象
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 5.组织数据并发布
    //5-1 组织请求数据
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 4.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "turtle2";
    //5-2 发送请求
    //判断服务器状态(二选一)
    // ros::service::waitForService("/spawn");
    client.waitForExistence();
    bool flag = client.call(spawn);
    // 6.处理响应
    if (flag)
    {
        ROS_INFO("乌龟生成成功,新乌龟叫:%s",spawn.response.name.c_str());
    }else{
        ROS_INFO("请求失败!!!");
    }
    return 0;
}

发布：

#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
        消息: /turtle1/Pose

    流程：
    1.包含头文件
    2.设置编码、初始化、NodeHandle
    3.创建订阅对象，订阅/turtle1/pose
    4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系，并发布（关注）
    5.spin()
*/

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

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //获取位姿信息，转换成坐标系相对关系（核心），并发布
    //a.创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    //b.组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    //关键点2：动态传人
    ts.child_frame_id = turtle_name;
    //坐标偏移量设置
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数
    /*
        位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是 2D ，没有翻滚与俯仰角度，所以可以得出乌龟的欧拉角：0 0 theta
    */
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);

    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    //c.发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    // 2.设置编码、初始化、NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    /*
        解析 launch 文件通过 args 传入的参数
    */
   if (argc != 2)
   {
       ROS_ERROR("请传入一个参数");
       return 1;
   } else{
       turtle_name = argv[1];
   }
   

    // 3.创建订阅对象，订阅/turtle1/pose
    //关键点1：订阅的话题名称，turret1 或turtle2 动态传人
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,doPose);
    // 4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系，并发布（关注）
     

    // 5.spin()
    ros::spin();
    return 0;
}

控制跟随：

#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[])
{
    // 2.编码、初始化、NodeHandle 创建
    setlocale(LC_ALL,"");
    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::Rate rate(10);
    while (ros::ok())
    {
        //核心
        try{
            //1.计算 son1 与 son2 的相对关系
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            // ROS_INFO("son1 相对于 son2 的信息：父级:%s, 子级:%s, 偏移量(%.2f,%.2f,%.2f)" ,
            //         son1ToSon2.header.frame_id.c_str(),  //turtle2
            //         son1ToSon2.child_frame_id.c_str() ,  //turtle1
            //         son1ToSon2.transform.translation.x,
            //         son1ToSon2.transform.translation.y,
            //         son1ToSon2.transform.translation.z);

            //B. 根据相对关系计算并组织速度消息
            geometry_msgs::Twist twist;
            /*
                组织速度，只需要设置线速度的 x 与 角速度的 z
                x = 系数*开方(y^2 + x^2)
                z = 系数*反正切(对边，邻边)
            */
            twist.linear.x = 0.5*sqrt(pow(son1ToSon2.transform.translation.x,2) + pow(son1ToSon2.transform.translation.y,2));
            twist.angular.z = 4*atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x);

            //C. 发布
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
    // 5.spinOnce()
    ros::spinOnce();
    }
    return 0;
}

输出：

直接运行 launch 文件就好