ROS学习记录11——tf坐标变换与多海龟跟随

零.前言

关于tf的内容,我也没有彻底理解透彻,只能讲述个大概,若有错误,请评论区斧正,谢谢。
在《机器人基础》这个课里,有推导关于机器人坐标变换的说明,当然,我没学,学了也因为当时数学不好没听懂,所以这篇文章也不会讲太多数学层面的东西。

一.概念

1.1 引言

我们知道,物体的位置是相对的。
那么在描述一个系统中例如三个物体的位置关系,我们可以通过假设A为原点,计算B相对于A的坐标,C相对于A的坐标。这样可以通过A+坐标的方式表示B,C的位置。
当然,此时我们也可以把B当作坐标原点,根据刚才的坐标,我们可以得到反推得到A相对于B的坐标,又已知AC间的坐标关系,那么就可以通过代数计算得到C相对于B的坐标。
ROS中的坐标也是如此!

1.2 思想

ROS中,所有的坐标关系是通过一颗树形结构组成的,就像这样:
ROS学习记录11——tf坐标变换与多海龟跟随
这样做的好处是:假设A为我们认为的参照物,把它看作为原点,那么BD一起向某个方向移动了相同的距离。那么我们用绝对坐标的方式需要会改变B和D这两个物体的坐标。
但是通过这种树形结构的方式,我们只需要记住D和B的相对位置,那么我们只需要更改B相对于A的位置,在需要时,即可计算出D的位置,这样可以有效地简化坐标系变化时的坐标变动的数目,每个目标只用专心处理自己和目标坐标系的关系,不用考虑其他坐标系的关系。

1.3 概念

两个坐标系间的tf消息组成是这样的:

    header: 
      seq: 0
      stamp: 
        secs: 1621956538
        nsecs: 108681917
      frame_id: "parent"
    child_frame_id: "child"
    transform: 
      translation: 
        x: -0.45
        y: 0.2
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0

描述的是从frame_id变换到child_frame_id的坐标关系。很多很多这样的消息就可以构成一棵tf消息树。完成整个系统的坐标描述
上面还有roscore运行的时间戳,能够记录一段时间的实时坐标消息。

1.4 使用

那么我们需要做的:

  • 广播tf变换
  • 监听tf变换

通过广播,把变动的两个坐标系的关系发布出去,然后由ros底层提供处理,然后并通过话题发布出来,然后需要用到坐标系时,监听某两个坐标系间的关系即可(当然,这个关系可能不是直接得出,得经过计算)。不过,复杂的计算和其他问题的处理我们不需要关心。我们只需要发布,监听即可。

1.5 特点

优点:

  1. 各种数值计算的细节,你不用考虑,tf库可以帮你
  2. 接口很简洁,会广播和监听就行;
  3. 问题找的很准,那就是需要维护坐标系之间的关系,尤其是父子坐标系的关系
  4. 提供了很多工具程序
  5. 考虑了与时间相关的变换

缺点:

  1. 树的结构很简单,但有时候很笨拙。对于同级的坐标系,就需要从下到上找共同先辈,然后从这个先辈再往下找,进而确定二者的关系。
  2. 每个订阅器要想获得某两个坐标系的关系都要搜索同一颗树,这样的开销太大,主要是网络传输的负荷比较大。
  3. 很难满足实时性的要求,这一点比较显然。这也是为什么TF会将每个变换存10秒钟的数据
  4. 虽然整体比较容易上手但是很多细节不易理解。比如,now()和time(0);比如,技术文档里的一些术语名词;比如,采用了机器人里的习惯,与飞行器,惯导,车辆里的习惯区别较大,使用时不能想当然。

二.代码实现

2.1 广播程序

    header: 
      seq: 0
      stamp: 
        secs: 1621956538
        nsecs: 108681917
      frame_id: "parent"
    child_frame_id: "child"
    transform: 
      translation: 
        x: -0.45
        y: 0.2
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0

以上是一个tf消息,我们分析该消息可以得到:我们可能需要修改以下内容的值:stamp frame_id child_frame_id translation: rotation:
通过查阅官方手册,我们可以得知:

  • br.sendTransform(); 该函数可以发送我们的tf变换的消息
  • tf::StampedTransform(transform, ros::Time::now(), child_frame_id, frame_id); 该函数可以创建一条我们需要发送的tf消息。他会自动更根据ros::Time::now()的时间,为我们添加stamp这样的时间戳。
  • transform是一个类,它包含了translationrotation这两个tf需要用到的元素。
  • transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) ) 可以修改transform.translation的值,参数为相对于父坐标系的x,y,z
  • transform.setRotation( tf::Quaternion(0, 0, 0, 1) )可以修改transform.rotation的值,参数为我们学过的2X2的旋转矩阵。
  • tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q);但是手动计算这个旋转矩阵太麻烦了,所以我们可以通过创建Quaternion对象,并通过rpy作为参数,实例化该对象,它会自动根据rpy的三个参数创建旋转矩阵。

整个结构像这样:
ROS学习记录11——tf坐标变换与多海龟跟随

至此,我们可以通过x,y,z,r,p,y这六个我们认识的参数来创建并广播我们的tf消息了。

2.2 监听程序

监听程序很简单:
listener.lookupTransform(child_frame_id, frame_id, ros::Time(0), transform);

  • 参数除了第三个ros::Time(0),其余的都在广播程序里讲过。
  • ros::Time(0)代表 (tf树由后台计算处理) 最新可用的tf树的时间。若此处使用
    整个ROS的时间系统是相对于独立的,使用的是ros的内部时间,所以要实现延迟跟随,得用该方法获得x秒前的时间ros::Time::now(); ros::Time past = now - ros::Duration(x);

三.实战——多海龟跟随

3.1 创建功能包

现在忽略掉工作空间、工作空间的环境配置。直接创建功能包:

catkin_create_pkg turtle_follow roscpp geometry_msgs tf turtlesim

3.2 创建乌龟发布节点

生成海龟的原理:是向Spwan这个服务提供一个请求,这个过程可以参考我的
这是海龟生成的服务Spwan.srv

float32 x
float32 y
float32 theta
string name # Optional.  A unique name will be created and returned if this is empty
---
string name

那么根据服务编程的原则,我们可以写出以下代码:

#include <ros/ros.h>
#include <turtlesim/Spawn.h>
 
int main(int argc, char** argv)
{
    int i;
    if (argc < 2)
    {
        ROS_ERROR("Turtle's name can't be empty");
        return -1;
    }
    ros::init(argc, argv, "turtle_new");
    ros::NodeHandle node;
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle_client = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv_msg;
    for (i = 1; i < argc; ++i)
    {
        srv_msg.request.x = i;
        srv_msg.request.y = i;
        srv_msg.request.theta = 0;
        srv_msg.request.name = *(argv + i);
        add_turtle_client.call(srv_msg);
    }
    return 0;
}

解释一下:

引用的库

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

检测调用该节点是否含有参数,若没有,则报警并退出:

    if (argc < 2)
    {
        ROS_ERROR("Turtle's name can't be empty");
        return -1;
    }

初始化节点:

    ros::init(argc, argv, "turtle_new");
    ros::NodeHandle node;

等待spawn服务创建(因为roslaunch里启动不分顺序,在这里必须等待,确保该服务存在),然后初始化一个客户端,并创建一条srv_msg服务的数据结构,准备访问服务:

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle_client = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv_msg;

通过循环,更改乌龟的初始坐标和名字,然后进行访问。(乌龟在二维平面上运动,所以只有x轴,y轴,和yaw角)

    for (i = 1; i < argc; ++i)
    {
        srv_msg.request.x = i;
        srv_msg.request.y = i;
        srv_msg.request.theta = 0;
        srv_msg.request.name = *(argv + i);
        add_turtle_client.call(srv_msg);
    }

将上面的总体代码创建为turtle_new.cpp
ROS学习记录11——tf坐标变换与多海龟跟随添加CMakeList:

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

编译,然后在三个终端里按顺序运行:

roscore
rosrun turtlesim turtlesim_node
rosrun turtle_follow turtle_new A B C D E

得到结果:
ROS学习记录11——tf坐标变换与多海龟跟随
然后查看话题,成功创建五只乌龟,还有自带的turtle1,有强迫症的同学,可以通过编写kill服务去杀死那只turtle1
ROS学习记录11——tf坐标变换与多海龟跟随

3.2 创建坐标广播器

首先我们先看一下乌龟的姿态信息:rostopic echo /A/pose

x: 1.0
y: 1.0
theta: 0.0
linear_velocity: 0.0
angular_velocity: 0.0
---

这是我们创建时,相对于world这个坐标系的相对坐标:xy轴,y角,线速度,角速度。
所以,我们要将该/pose话题提供的信息,转换成tf消息并发出。代码如下:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
 
std::string turtle_name;
 
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    tf::Quaternion quaternion;
    
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    quaternion.setRPY(0, 0, msg->theta);
    transform.setRotation(quaternion);
 
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
 
int main(int argc, char** argv)
{

    ros::init(argc, argv, "turtle_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    }
    
    turtle_name = *(argv + 1); // = argv[1]
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
 
    ros::spin();
    
    return 0;
};

解释一下:
主函数里要求程序要有一个参数,也就是乌龟的名字。然后就是基础的节点编程的原理,乌龟发布的位置的话题是:/乌龟的名字/pose,我们需要订阅该话题,该话题发布后,我们就进入回调函数,并广播tf消息。

int main(int argc, char** argv)
{

    ros::init(argc, argv, "turtle_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    }
    
    turtle_name = *(argv + 1); // = argv[1]
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
 
    ros::spin();
    
    return 0;
};

回调函数里,创建了 一个广播器br,姿态信息transorm,和旋转矩阵quaternion
在2.1里详细的描写了该过程,就不赘述。

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    tf::Quaternion quaternion;
    
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    quaternion.setRPY(0, 0, msg->theta);
    transform.setRotation(quaternion);
 
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

将我们的代码写入turtle_broadcaster.cpp
ROS学习记录11——tf坐标变换与多海龟跟随
添加依赖:

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

编译运行:

roscore
rosrun turtlesim turtlesim_node
rosrun turtle_follow turtle_new A
rosrun turtle_follow turtle_broadcaster A

此时我们可以通过:rostopic echo /tf查看我们的tf树,或者使用rosrun tf tf_echo [reference_frame] [target_frame]查看任意两个坐标系的变换,或者使用rosrun rqt_tf_tree rqt_tf_tree在rqt里可视化查看:
ROS学习记录11——tf坐标变换与多海龟跟随

3.3 创建坐标监听器,并发布速度话题

rostopic echo /turtle1/cmd_vel看看/cmd_vel 这个话题是啥样的:
ROS学习记录11——tf坐标变换与多海龟跟随
无论怎么移动小乌龟,他只有x,z轴数据在变,所以我也懒得找文献是为什么了,直接给这两个速度赋值就行。
那么我们监听器的代码是:

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <string>
using namespace std;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtle_listener");
    ros::NodeHandle node;
    string follower_vel("/");
    if (argc != 3)
    {
        ROS_ERROR("Need two names of the turtles!");
        return -1;
    }
    follower_vel.append(argv[1]);
    follower_vel.append("/cmd_vel");
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>(follower_vel, 10);
    tf::TransformListener listener;
 
    ros::Rate rate(10.0);

    while (node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            listener.lookupTransform(argv[1], argv[2], ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
 
        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;
};

目前我们坐标系的命名因该是这样的:worldA……
速度消息发布的话题是这样的:/A/cmd_vel
等会儿我们处理的时候,最好按着格式来,一个符号都不要差(虽然有些是可以省略)。

解释一下代码, 这里我们除了用到Twist这个消息,还要使用string对字符串拼接.

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

初始化节点,并且初始化我们的/X/cmd_vel这个话题的字符串。因为是跟随,所以我们需要两个参数。

    ros::init(argc, argv, "turtle_listener");
    ros::NodeHandle node;
    string follower_vel("/");
    if (argc != 3)
    {
        ROS_ERROR("Need two names of the turtles!");
        return -1;
    }

添加字符串,完善follower_vel这个字符串,等会儿launch只输入乌龟名即可,并创建Twist消息的发布器和tf监听器

    follower_vel.append(argv[1]);
    follower_vel.append("/cmd_vel");
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>(follower_vel, 10);
    tf::TransformListener listener;

这个是获得两个坐标系的transform信息,用try起来是因为 本身这是个异步过错,容易出错。等会儿运行时就会看到真的会出错。

        tf::StampedTransform transform;
        try
        {
            listener.lookupTransform(argv[1], argv[2], ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

创建消息,通过坐标x,y得到角度差距,并求得y轴的角速度,和欧式距离下的线速度,并发布消息。

        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();

将我们的代码写入:
ROS学习记录11——tf坐标变换与多海龟跟随
添加依赖:

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

这样就完成了。

3.4 编写launch文件

在功能包下创建launch文件夹,并创建launch文件:
ROS学习记录11——tf坐标变换与多海龟跟随

 <launch>
    <!-- 海龟仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 创建一个乌龟节点 -->
    <node pkg="turtle_follow" type="turtle_new" args="A" name="turtle_new" />

    <!-- 创建两个乌龟的tf广播器 -->
    <node pkg="turtle_follow" type="turtle_broadcaster" args="A" name="turtle1_broadcaster" />
 	<node pkg="turtle_follow" type="turtle_broadcaster" args="turtle1" name="turtle2_broadcaster" />
 
    <!-- 第一个参数的乌龟跟随第二个参数的乌龟 -->
    <node pkg="turtle_follow" type="turtle_listener" args="A turtle1" name="listener1" />
 </launch>

这个launch文件就不需要太多的解释了,启动节点,添加参数,不懂的话可以看上一篇文章的内容。
然后运行看效果:

roslaunch turtle_follow follow.launch

看,因为tf坐标系还没来得及建立而产生的错误:
ROS学习记录11——tf坐标变换与多海龟跟随
如果没有一直出现红色的错误信息,说明代码无误,在这个终端用方向键移动乌龟:

ROS学习记录11——tf坐标变换与多海龟跟随
千辛万苦写出来的代码不来点儿好玩儿的?6只乌龟开火车!:

 <launch>
    <!-- 海龟仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 创建五个乌龟节点 -->
    <node pkg="turtle_follow" type="turtle_new" args="A B C D E" name="turtle_new" />

    <!-- 创建六个乌龟的tf广播器 -->
    <node pkg="turtle_follow" type="turtle_broadcaster" args="A" name="turtle1_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="B" name="turtle2_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="C" name="turtle3_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="D" name="turtle4_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="E" name="turtle5_broadcaster" />
 	<node pkg="turtle_follow" type="turtle_broadcaster" args="turtle1" name="turtle0_broadcaster" />
 
    <!-- 第一个参数的乌龟跟随第二个参数的乌龟 -->
    <node pkg="turtle_follow" type="turtle_listener" args="A turtle1" name="listener1" />
    <node pkg="turtle_follow" type="turtle_listener" args="B A" name="listener2" />
    <node pkg="turtle_follow" type="turtle_listener" args="C B" name="listener3" />
    <node pkg="turtle_follow" type="turtle_listener" args="D C" name="listener4" />
    <node pkg="turtle_follow" type="turtle_listener" args="E D" name="listener5" />
 </launch>

ROS学习记录11——tf坐标变换与多海龟跟随ROS学习记录11——tf坐标变换与多海龟跟随

四.后记

要深入学习tf,还是得多看官方文档、手册。接下来学习了gazebo后,就可以让机器人动起来了!

上一篇:【古月居ROS21讲】14 服务端Server的编程实现


下一篇:Alpha冲刺报告(1/12)(麻瓜制造者)