ROS TF坐标变化，小乌龟跟随案例

ROS TF的基础，四元数、欧拉角理论见该篇文章，讲解非常详细

https://blog.csdn.net/zhanghm1995/article/details/84644984

1.程序说明

本实例创建两个乌龟，乌龟1乌龟2，控制乌龟1随意移动，保证乌龟2实时跟随乌龟1。想要达到这个效果，程序设计思路为：

1）我们需要获取乌龟1的位姿信息，本程序只考虑x,y，以及欧拉角的偏航角（Yaw），将乌龟1的位姿变换到世界坐标系（tf变换），并发布乌龟1的位姿信息

2） 有了乌龟1的位姿信息，我们还需要创建乌龟2用于跟随乌龟1，为了让乌龟2跟随乌龟1，我们需要为乌龟2赋予线速度和角度，两者都可以通过简单的计算公式算出。

 

2.5 补充

创建工作空间，功能包，两个C++文件

$ source /opt/ros/kinetic/setup.zsh
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace
$ cd ~/catkin_ws
$ catkin_make 
$ souce ~/catkiin_ws/devel/setup.zsh
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf roscpp geometry_msgs tf turtlesim
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.zsh

在CmakeList.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})

2.广播程序

创建程序turtle_tf_broadcaster.cpp，用于广播乌龟1的位姿

#include 
#include 
#include 

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // tf广播器
    static tf::TransformBroadcaster br;

    // 根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);

    // 发布坐标变换
    br.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];

    // 订阅乌龟的pose信息
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    ros::spin();

    return 0;
};

程序分为两个函数，主函数main,与回调函数poseCallback

main:

判断传入参数是否有两个，由于要做跟随必须有两个刚体，参数不为2报错退出

 if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    };

拿到第一个海龟的变量名（std::string turtle_name定义全局变量），也就是被跟随的一方，订阅海龟的pose信息，调用回调函数，频率为10hz。ros:spin()使程序不中止，持续订阅

 turtle_name = argv[1];

    // 订阅乌龟的pose信息
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    ros::spin();

poseCallback(const turtlesim::PoseConstPtr& msg)

tf::TransformBroadcaster创建tf广播对象，用于发布海龟的位姿

transform.setOrigin设置被跟随海龟的初始位姿，传入参数tf::Vector3，即海龟在当前空间坐标系的坐标信息，由于这里只考虑2维坐标，所以z赋0。

tf::Quaternion设置海龟的欧拉角RPY，因为只考虑xy，所以只需要设置欧拉角的Y轴(Yaw)，你站在一个平面原地转一圈，就是绕Y轴旋转一周，比较好理解，我们通过实参msg来获取theta向量角

tf::StampedTransform传四个参数,transform位姿，现在时间，父坐标系，子坐标系

    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

补充：

欧拉角参考图

 

tf::Transform官方文档：http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transform.html

 tf::Quaternion官方文档：http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Quaternion.html

tf::StampedTransform：http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1StampedTransform.html

 

3.监听程序

创建程序turtle_tf_listener.cpp用于监听乌龟1的位姿，并发布线速度与角度

#include 
#include 
#include 
#include 

int main(int argc, char** argv)
{
    // 初始化节点
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle node;

    // 通过服务调用，产生第二只乌龟turtle2
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
    node.serviceClient("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    // 定义turtle2的速度控制发布器
    ros::Publisher turtle_vel =
    node.advertise("turtle2/cmd_vel", 10);

    // tf监听器
    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            // 查找turtle2与turtle1的坐标变换
            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需要运动的线速度和角速度
        // 并发布速度控制指令，使turtle2向turtle1移动
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }
    return 0;
};

代码解读

ros::service命名空间同样提供一些便利的函数如：exists() 和waitForService()，ros::service::waitForService("spawn")等待名为spawn的server端创建

serviceClient("spawn")创建client端对象，订阅server名为spawn的Service

turtlesim::Spawn srv;add_turtle.call(srv);上两句仅创建Service，并没有实际的Spawn对象，此处创建，通过call声明Spawn

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle = node.serviceClient("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

  node.advertise("turtle2/cmd_vel", 10);定义话题名为turtle2的速度控制发布器，频率10hz，tf::TransformListener listener;定义tf监听器，监听第一只乌龟，也就是被跟随乌龟的位姿信息, ros::Rate rate(10.0);定义话题发布频率10hz

// 定义turtle2的速度控制发布器
    ros::Publisher turtle_vel =
    node.advertise("turtle2/cmd_vel", 10);

    // tf监听器
    tf::TransformListener listener;

    ros::Rate rate(10.0);

node非异常中断则持续监听

 while (node.ok())
    {
       ...
    }

通过监听器监听被跟随海龟的坐标变化，listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));参数依次为海龟2（跟随者），海龟1（被跟随者），时间间隔0（变化后马上响应），持续监听时间3s。

得到乌龟2到乌龟1的坐标变换，listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);

tf::StampedTransform transform;
        try
        {
            // 查找turtle2与turtle1的坐标变换
            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;
        }

 t2到t1的线速度公式为:

t2.linear.x=速度系数*sqrt(t1.x²+t2.y²)，线速度等于原点指向(x,y)的模，0.5为速度系数，为1则等于被跟随乌龟的速度，这里设为0.5，使其只有被跟随乌龟一半的速度，当然设置多少取决于你。

角速度计算公式：

t2.angular.z=角度系数*atan2(y/x)，atan2用于计算方位角，值域是-π到π

// 根据turtle1和turtle2之间的坐标变换，计算turtle2需要运动的线速度和角速度
        // 并发布速度控制指令，使turtle2向turtle1移动
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);

        rate.sleep();

 

4.roslauch

创建lauch目录，创建start_demo_with_listener.launch文件

运行roslauch文件

roslaunch learning_tf start_demo_with_listener.launch

角度系数分析

关于角度系数为何取4，可以观察一下取不同值的效果，从视觉上可以看出，角度系数越小，乌龟2到乌龟1的曲线就越弯曲，这也很好理解，我们开车的时候遇到急的弯就需要打多方向。

角度系数为4

角度系数为8