古月居ROS入门21讲——18.tf坐标系广播与监听的编程实现

1.如何实现一个tf广播器（broadcaster）

1. 定义TF广播器（TransformBroadcaster）
2. 创建坐标变换值
3. 发布坐标变换（sendTransform）

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    //创建tf的广播器
    static tf::TransformBroadcaster broadcaster;

    //初始化tf数据
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));  //Vector三维向量表示平移
    tf::Quaternion q;  //四元数表示旋转
    q.setRPY(0, 0, msg->theta);  //平面只有绕z轴的旋转
    transform.setRotation(q);

    //广播world与海龟坐标系之间的tf数据
    //tf::StampedTransform只能用在C++里，只是C++的一个类，一种数据格式，并不是一个消息。
    //将world和turtle_name的坐标转换关系发送出去
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf_broadcaster");

    //输入参数作为海龟的名字
    if(argc != 2)
    {
        ROS_ERROR("Need turtle name as argument");
        return -1;
    }

    turtle_name = argv[1];

    //订阅海龟的位姿话题
    ros::NodeHandle n;
    ros::Subscriber sub= n.subscribe(turtle_name+"/pose", 10, &poseCallback);

    ros::spin();

    return 0;
}

2.如何实现一个tf监听器（listener）

1. 定义TF监听器（TransformListener）
2. 查找坐标变换（waitForTransform、lookupTransform）

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle n;

    //请求产生turtle2
    ros::service::waitForService("/spawn");  //等待发现/spawn服务
    ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");  //创建一个服务客户端
    //初始化服务请求数据
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    //创建发布turtle2速度控制指令的发布者，发布名为/turtle2/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

    //创建tf的监听器
    tf::TransformListener listener;

    ros::Rate loop_rate(10.0);

    while(n.ok())
    {
        //获取turtle1和turtle2坐标系之间的数据
        tf::StampedTransform transform;
        try
        {
            //等待系统中是否存在/turtle1、/turtle2这两个坐标系，存在的话才会跳到下一句，等待3秒之后就会提示超时，引发错误
            listener.waitForTransform("turtle2", "turtle1", ros::Time(0), ros::Duration(3.0));
            //查询当前时刻的坐标转换关系
            listener.lookupTransform("turtle2", "turtle1", ros::Time(0), transform);
        }
        catch(tf::TransformException &ex)
        {
            ROS_ERROR("%S", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        //根据turtle1与turtle2坐标系之间的位置关系，计算turtle2的移动速度
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + 
                                      pow(transform.getOrigin().y(), 2));
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        //发布turtle2的速度控制指令
        turtle_vel_pub.publish(vel_msg);

        loop_rate.sleep();
    }
    return 0;
}

3.配置CMakeLists.txt中的编译规则

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

4.编译与运行

cd ~/my_ws
catkin_make
source ~/my_ws/devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key

ROS重映射机制，第一次运行的tf广播__name:=turtle1_tf_broadcaster，程序中my_tf_broadcaster将被替换为turtle1_tf_broadcaster；第二次运行的tf广播__name:=turtle2_tf_broadcaster，程序中my_tf_broadcaster将被替换为turtle2_tf_broadcaster，同一个程序运行两次，避免命名产生冲突。

为什么要运行两次呢？这是因为每次广播的是小海龟和world坐标系之间的转换关系，然后在监听程序中获取查询turtle1和turtle2坐标系之间的转换关系，完成跟随。

参考：

1. 古月居ROS入门21讲
2. 中国大学MOOC—《机器人操作系统入门》课程讲义