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

墨蓝 2023-02-14 02:28 10阅读 0赞

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

[https://blog.csdn.net/zhanghm1995/article/details/84644984][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赋予线速度和角度，两者都可以通过简单的计算公式算出。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70][]

### 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 <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 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));  
> \}

**补充：**

欧拉角参考图

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 1][]

tf::Transform官方文档：[http://docs.ros.org/jade/api/tf/html/c++/classtf\_1\_1Transform.html][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][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][StampedTransform]

### **3.监听程序** ###

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

#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 node;
    
        // 通过服务调用，产生第二只乌龟turtle2
        ros::service::waitForService("spawn");
        ros::ServiceClient add_turtle =
        node.serviceClient<turtlesim::Spawn>("spawn");
        turtlesim::Spawn srv;
        add_turtle.call(srv);
    
        // 定义turtle2的速度控制发布器
        ros::Publisher turtle_vel =
        node.advertise<geometry_msgs::Twist>("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<turtlesim::Spawn>("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<turtlesim::Spawn>("spawn");  
>     turtlesim::Spawn srv;  
>     add\_turtle.call(srv);

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

> // 定义turtle2的速度控制发布器  
>     ros::Publisher turtle\_vel =  
>     node.advertise<geometry\_msgs::Twist>("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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 2][]

角度系数为8

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 3][]

[https_blog.csdn.net_zhanghm1995_article_details_84644984]: https://blog.csdn.net/zhanghm1995/article/details/84644984
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200602230717439.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20200531231305394.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3,size_16,color_FFFFFF,t_70
[http_docs.ros.org_jade_api_tf_html_c_classtf_1_1Transform.html]: http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transform.html
[http_docs.ros.org_jade_api_tf_html_c_classtf_1_1Quaternion.html]: http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Quaternion.html
[StampedTransform]: http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1StampedTransform.html
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 2]: https://img-blog.csdnimg.cn/20200601234546518.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 3]: https://img-blog.csdnimg.cn/20200602000035971.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3,size_16,color_FFFFFF,t_70