ROS 机器人技术 - 广播与接收 TF 变换

#include <ros/ros.h>

// 存储要发布的坐标变换
#include <geometry_msgs/TransformStamped.h>

// 四元数
#include <tf2/LinearMath/Quaternion.h>

// 变换广播者
#include <tf2_ros/transform_broadcaster.h>

// 乌龟的位姿定义
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg) 
{
    // 创建 tf 广播对象
    static tf2_ros::TransformBroadcaster br;

    // 存储要发布的坐标变换消息
    geometry_msgs::TransformStamped transformStamped;

    // 变换的时间戳
    transformStamped.header.stamp = ros::Time::now();
    
    // 父坐标系名称
    transformStamped.header.frame_id = "world";
    
    // 当前要发布的坐标系名称 - 乌龟的名字
    transformStamped.child_frame_id = turtle_name;

    // 乌龟在二维平面运动，所以 z 坐标高度为 0
    transformStamped.transform.translation.x = msg->x;
    transformStamped.transform.translation.y = msg->y;
    transformStamped.transform.translation.z = 0.0;

    // 用四元数存储乌龟的旋转角
    tf2::Quaternion q;

    // 因为乌龟在二维平面运动，只能绕 z 轴旋转，所以 x，y 轴的旋转量为 0
    q.setRPY(0, 0, msg->theta);

    // 把四元数拷贝到要发布的坐标变换中
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();
    
    // 用 tf 广播者把订阅的乌龟位姿发布到 tf 中
    br.sendTransform(transformStamped);
}

int main(int argc, char** argv)
{
    // 当前节点的名称
    ros::init(argc, argv, "my_tf2_broadcaster");
    ros::NodeHandle private_node("~");

    // 判断当前要广播的乌龟节点名字
    if (!private_node.hasParam("turtle")) {
        // launch 文件和命令行都没有传递乌龟名称，就直接退出
        if (argc != 2) {
            ROS_ERROR("need turtle name as argument");
            return -1;
        };

        // launch 文件中如果没有定义乌龟名称，就在命令行中加上
        turtle_name = argv[1];
    } else {
        // 从 launch 文件获取乌龟名称参数
        private_node.getParam("turtle", turtle_name);
    }

    ros::NodeHandle node;

    // 订阅一个节点的 pose msg，在回调函数中广播订阅的位姿消息到 tf2 坐标系统中
    // turtle_name 为 turtle1 时广播 turtle1 的位姿到 tf 中
    // turtle_name 为 turtle2 时广播 turtle2 的位姿到 tf 中
    ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback);

    ros::spin();
    return 0;
};

#include <ros/ros.h>

// 接受 tf 变换
#include <tf2_ros/transform_listener.h>

// 转换消息 
#include <geometry_msgs/TransformStamped.h>

// 发布到乌龟 2 的运动消息：角速度和线速度
#include <geometry_msgs/Twist.h>

// 再生服务
#include <turtlesim/Spawn.h>

// 实现乌龟 2 跟随乌龟 1 运动
int main(int argc, char** argv)
{
    // 当前节点的名字
    ros::init(argc, argv, "my_tf2_listener");

    ros::NodeHandle node;
    ros::service::waitForService("spawn");
    ros::ServiceClient spawner = node.serviceClient<turtlesim::Spawn>("spawn");
    
    turtlesim::Spawn turtle;
    
    turtle.request.x = 4;
    turtle.request.y = 2;
    turtle.request.theta = 0;
    turtle.request.name = "turtle2";
    spawner.call(turtle);

    // 角速度和线速度消息发布者，用来发布计算后的新的速度消息
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    // tf 变换缓存区，最多缓存 10 秒
    tf2_ros::Buffer tfBuffer;

    // 创建监听 tf 变换对象，创建完毕即开始监听，通常定义为成员变量
    tf2_ros::TransformListener tfListener(tfBuffer);
    
    ros::Rate rate(10.0);
    while (node.ok()) {
        // 用来保存寻找的坐标变换
        geometry_msgs::TransformStamped transformStamped;
        try{
          	// 寻找坐标变换
      		  transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
        }
        catch (tf2::TransformException &ex) {
            ROS_WARN("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // 用来保存角速度和线速度
        geometry_msgs::Twist vel_msg;

        // 新的角速度为寻找到的变换角速度的 4 倍 - 使得第二个乌龟的运动轨迹转弯更快，且轨迹是弧线
        vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);
        
        // 新的线速度是寻找到的变换线速度的 0.5 倍 - 使得第二个乌龟的运动速度为第一个乌龟的一半
        vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));
        
        // 发布新的速度消息，乌龟 2 节点的内部订阅了这个消息，所以乌龟 2 会收到新的角速度和线速度，以此产生跟随运动
        turtle_vel.publish(vel_msg);
      
        rate.sleep();
    }

    return 0;
};

// 保存寻找的变换
geometry_msgs::TransformStamped transformStamped;

// 寻找 turtle1 到 turtle2 的坐标变换
// target_frame: turtle2 
// source_frame: turtle1
// ros::Time(0): 获取变换的时间，
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

// 第四个参数是阻塞等待的超时时间
listener.waitForTransform("/turtle2", "/turtle1", ros::Time::now(), ros::Duration(3.0));

transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));