Tags: SLAM

本篇博客是慕课《ROS机器人操作系统入门》的第四篇笔记,慕课网址是这里。本篇笔记主要记录tf和在C++和Python下的两种实现方式、需要注意的问题以及urdf

1.基本知识

(1)Transform

简单来说tf是ROS中用来处理坐标变换而生的一套工具,是Transform的缩写。tf也可以理解为一个Topic,这个Topic上的消息(Message)就是就是各坐标系(frame)间的变换关系,这些消息都有对应的发布者(Publisher)进行发布与更新。多个变换和坐标系组成树状的图,叫TF树,如下示意图所示。 树状图中每个节点表示一个坐标系(frame),对应机器人的一个link,节点之间的箭头则代表变换关系。各个节点之间必须是可以连通的,否则会报错。

在ROS里两个frame之间的变换是发布者(Publisher)发布的消息,这个消息的格式是在geometry_msgs/TransformStamped.msg中定义的,如下图所示。

而tf树的数据结构则定义在tf/tfMessage.msg(一代)或tf2_msgs/TFMessage.msg(二代),如下。 利用rostopic info /tf即可以知道当前的tf是什么格式。

(2)urdf

urdf全称Unified Robot Description Format,统一机器人描述格式,指定了一个用于描述机器人结构的规范,以xml文件格式储存。tf的维护、可视化、仿真等都会用到它。在urdf文件中有linkjointlink对应tf树的坐标系frame,joint对应tf树的坐标系间的变换transform。linkjoint的常用属性如下。

2.需求与描述

现有一个机器人,包含有一个RGB-D相机、一个LiDAR一个云台,云台上有一个相机。建立一个tf树来维护它们彼此之间的坐标变换关系,如下图所示。 trans_broadcaster节点用于发布消息,trans_listener节点用于接收消息。

3.Transform的C++实现

C++中提供了很多相关数据类型,如下: 注意tf::StampedTransform和上面提到的geometry_msgs/TransformStamped.msg的区别,前者是C++中的一个类,只能用于C++中,而后者则是ROS的消息,只依赖于ROS,与语言无关,都可以使用。

TransformBroadcaster类及相关成员函数。

TransformListener类及相关成员函数。 注意获取变换的时间,不能是ros::Time::now()。因为一个消息从发送到接收会有一定的延迟,如果选择现在的时间作为接收,则永远也收不到之前发送的消息,解决办法是ros::Time(0),表示接收最近一次发布的消息。

(1)创建Package

打开RoboWare,在”资源管理器”中找到src文件夹,右键单击选择”新建ROS包”,输入名称即可创建Package。创建完成后右键Package选择”编辑依赖的ROS包列表”,输入roscpptfgeometry_msgs(空格隔开)即可添加依赖。

(2)创建trans_broadcaster节点

在RoboWare的资源管理器中找到transform_learn,右键选择”新建Src文件夹”,在src文件夹上右键,然后选择”新建CPP源文件”,输入名称为trans_broadcaster.cpp并回车,然后在弹出的对话框中选择”加入到新的可执行文件中”即可。

(3)编写trans_broadcaster节点代码

对于一个Transform的发布者来说,其对外发布tf消息与之前的Topic类似。主要包含:初始化ROS节点(ros::init())、新建NodeHandle(ros::NodeHandle)、构造TransformBroadcaster(tf::TransformBroadcaster)、发布消息(broadcaster.sendTransform())、循环执行(ros::spin())。具体代码如下:

#include<ros/ros.h>
#include<tf/tf.h>   // 引入ROS的TF框架
#include<tf/transform_broadcaster.h>    // 用于引入广播者相关API



int main(int argc, char *argv[])
{
    // 初始化并指定节点名称
    ros::init(argc, argv, "trans_broadcaster");
    // 新建句柄
    ros::NodeHandle nh;
    // 新建广播对象,后续调用它的成员函数实现广播
    tf::TransformBroadcaster broadcaster;

    // 事先定义好各坐标系之间的变换关系
    tf::Transform tf_body_plat;
    // setOrigin用于设置变换的平移部分,只支持传入向量形式数据
    tf_body_plat.setOrigin(tf::Vector3(0.3,0.6,0.1));
    // setRotation用于设置变换的旋转部分,只支持传入四元数形式数据
    tf_body_plat.setRotation(tf::createIdentityQuaternion());

    tf::Transform tf_body_rgbd;
    tf_body_rgbd.setOrigin(tf::Vector3(0.4,0.6,0.2));
    tf_body_rgbd.setRotation(tf::createIdentityQuaternion());

    tf::Transform tf_body_laser;
    tf_body_laser.setOrigin(tf::Vector3(0.8,-0.2,0.3));
    tf_body_laser.setRotation(tf::createIdentityQuaternion());

    tf::Transform tf_plat_cam;
    tf_plat_cam.setOrigin(tf::Vector3(0.1,0.3,-0.2));
    tf_plat_cam.setRotation(tf::createIdentityQuaternion());

    // 构造Rate对象使循环按指定频率运行
    ros::Rate rate(1);
    while (nh.ok())
    {
        // broadcaster发送的是StampedTransform类型对象,因此需要对Transform对象转换
        // StampedTransform的构造函数四个参数分别是:
        // 第一个参数是构造好的Transform数据
        // 第二个参数是这个Transform所对应的时间
        // 第三个参数是这个Transform的源坐标系名称(from orign)
        // 第四个参数是这个Transform的目标坐标系名称(to target)
        tf::StampedTransform stf_body_rgbd = tf::StampedTransform(tf_body_rgbd,ros::Time::now(),"body","rgbd");
        tf::StampedTransform stf_body_plat = tf::StampedTransform(tf_body_plat,ros::Time::now(),"body","plat");
        tf::StampedTransform stf_body_laser = tf::StampedTransform(tf_body_laser,ros::Time::now(),"body","laser");
        tf::StampedTransform stf_plat_cam = tf::StampedTransform(tf_plat_cam,ros::Time::now(),"plat","cam");
        
        // 调用成员函数发送广播
        broadcaster.sendTransform(stf_body_rgbd);
        broadcaster.sendTransform(stf_body_plat);
        broadcaster.sendTransform(stf_body_laser);
        broadcaster.sendTransform(stf_plat_cam);
        
        // 输出部分提示信息
        std::cout<<"broadcasting transform......"<<std::endl;
        // 休眠等待
        rate.sleep();
    }
    return 0;
}
(4)创建trans_listener节点

在RoboWare的资源管理器中找到src文件夹,点击右键,然后选择”新建CPP源文件”,输入名称为trans_listener.cpp并回车,然后在弹出的对话框中选择”加入到新的可执行文件中”即可。

(5)编写trans_listener节点代码

对于一个Transform的接收者来说,其接收tf消息与之前的Topic类似。主要包含:初始化ROS节点(ros::init())、新建NodeHandle(ros::NodeHandle)、构造TransformListener(tf::TransformListener)、接收消息or应用变换(listener.lookupTransform()orlistener.transformPoint)。具体代码如下:

#include<ros/ros.h>
#include<tf/tf.h>   // 引入ROS的TF框架
#include<tf/transform_listener.h>   // 用于引入接收者相关API
#include<geometry_msgs/PointStamped.h>  // 代码中用到ROS中定义好的一些几何类型,如PointStamped

int main(int argc, char *argv[])
{
    // 初始化节点并指定名称
    ros::init(argc, argv, "trans_listener");
    // 新建句柄
    ros::NodeHandle nh;
    // 创建一个tf接收对象,后续调用该对象的成员函数进行接收
    tf::TransformListener listener;

    // 阻塞等待,直到指定的某两个frame连通,注意参数
    // 第一个参数是变换的目标坐标系(to)
    // 第二个参数是变换的源坐标系(from)
    // 第三个参数指定那一时刻的变换,如果写0则表示当前时刻
    // 第四个参数是阻塞时间长度
    listener.waitForTransform("body","cam",ros::Time(0),ros::Duration(4.0));

    // 新建测试点用于进行测试
    geometry_msgs::PointStamped point_in_rgbd;
    point_in_rgbd.header.frame_id = "rgbd";
    point_in_rgbd.header.stamp = ros::Time();
    point_in_rgbd.point.x = 0.4;
    point_in_rgbd.point.y = 0.5;
    point_in_rgbd.point.z = 0.3;

    // 构造Rate对象使循环按指定频率运行
    ros::Rate rate(1);
    while (nh.ok())
    {
        // 在正常运行的情况下利用try-catch语句尝试接收
        try
        {
            // 测试一,利用lookupTransform接收
            // 新建标记变换对象用于接收收到的变换
            tf::StampedTransform trans_body_laser;

            // 根据坐标名字查找变换,若找到赋值给传入的参数
            // 第一个参数是变换的目标坐标系(to)
            // 第二个参数是变换的源坐标系(from)
            // 第三个参数指定那一时刻的变换,如果写0则表示当前时刻。ros::Time(0)表示最近的一帧坐标变换,不能写成ros::Time::now()
            // 第四个参数表示用于接收变换数据的对象
            listener.lookupTransform("laser","body",ros::Time(0),trans_body_laser);

            // 获取变换的平移部分
            std::cout<<"translation body->laser:x="<<
            trans_body_laser.getOrigin().x()<<
            ",y="<<trans_body_laser.getOrigin().y()<<
            ",z="<<trans_body_laser.getOrigin().z()<<std::endl<<std::endl;
        
            // 测试二,利用transformPoint应用变换
            // 将rgbd下的点转换到cam下
            geometry_msgs::PointStamped tf_p_cam;
            listener.transformPoint("cam",point_in_rgbd,tf_p_cam);
            ROS_INFO("trans1:rgbd_point:(%f,%f,%f) -> cam_point(%f,%f,%f)",
            point_in_rgbd.point.x,point_in_rgbd.point.y,point_in_rgbd.point.z,
            tf_p_cam.point.x,tf_p_cam.point.y,tf_p_cam.point.z);
            
            // 再将刚刚转换的点转回rgbd下,以此验证是否转换正确闭合
            geometry_msgs::PointStamped tf_p_rgbd;
            listener.transformPoint("rgbd",tf_p_cam,tf_p_rgbd);
            ROS_INFO("trans2:cam_point:(%f,%f,%f) -> rgbd_point(%f,%f,%f)\n",
            tf_p_cam.point.x,tf_p_cam.point.y,tf_p_cam.point.z,
            tf_p_rgbd.point.x,tf_p_rgbd.point.y,tf_p_rgbd.point.z);
        }
        catch (tf::TransformException &ex)
        {
            // 如果失败输出错误信息
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
        }
        rate.sleep();
    }
    return 0;
}
(6)编译运行测试

最后在Catkin工作空间根目录下打开终端,使用catkin_make进行编译。然后先输入roscore打开Master,然后在新终端中输入rosrun transform_learn trans_broadcaster启动trans_broadcaster节点,新终端中输入rosrun transform_learn trans_broadcaster启动trans_broadcaster节点。如下: 可以看到成功运行了刚刚写的两个节点,并且实现了tf消息的收发。最后简单说一下,生成的两个节点的可执行文件放在了/root/catkin_ws/devel/lib/transform_learn里,直接在终端中运行这个可执行文件也是可以的(Master已经运行的情况下)。

4.Topic的Python实现

TF相关数据类型:

tf.transformations包提供了一些基本的变换运算。

tf.TransformListener类提供了接收者的相关函数。

tf.TransformBroadcaster类提供了发布者的相关函数。

(1)创建Package

打开RoboWare,在”资源管理器”中找到src文件夹,右键单击选择”新建ROS包”,输入名称即可创建Package。创建完成后右键Package选择”编辑依赖的ROS包列表”,输入rospytfgeometry_msgs(空格隔开)即可添加依赖。

(2)创建trans_broadcaster节点文件

根据之前的习惯,src文件夹里一般放C/C++文件,Python代码一般放在scripts文件夹里。在包名上右键点击,选择”新建文件夹”,输入名称为scripts,在scripts文件夹上右键选择”新建文件”,输入trans_broadcaster.py即可。

(3)编写trans_broadcaster代码

在Python中有两种发布方法:sendTransform()sendTransformMessage()。第一种Python代码如下:

# coding=utf-8
import rospy
import tf


if __name__ == "__main__":
    # 初始化并指定节点名称
    rospy.init_node("trans_broadcaster")
    # 新建广播对象,后续调用它的成员函数实现广播
    broadcaster = tf.TransformBroadcaster()
    
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        # 在Python中向量、点、四元数、矩阵都表示成类似数组形式,Tuple、List、Numpy Array通用
        # 调用成员函数发送广播
        # 第一个参数是以tuple表示的平移分量
        # 第二个参数是以tuple表示的旋转分量,四元数顺序是(x, y, z, w)
        # 第三个参数是变换对应的时间
        # 第四个参数是变换的目标坐标系(to target)
        # 第五个参数是变换的源坐标系(from origin)
        broadcaster.sendTransform((0.4,0.6,0.2),(0,0,0,1),rospy.Time.now(),"rgbd","body")
        broadcaster.sendTransform((0.3,0.6,0.1),(0,0,0,1),rospy.Time.now(),"plat","body")
        broadcaster.sendTransform((0.8,-0.2,0.3),(0,0,0,1),rospy.Time.now(),"laser","body")
        broadcaster.sendTransform((0.1,0.3,-0.2),(0,0,0,1),rospy.Time.now(),"cam","plat")
        
        print "broadcasting transform..."
        rate.sleep()

第二种如下:

# coding=utf-8
import rospy
import tf
import geometry_msgs.msg
import tf2_ros.transform_broadcaster

if __name__ == "__main__":
    # 初始化并指定节点名称
    rospy.init_node("trans_broadcaster")
    # 新建广播对象,后续调用它的成员函数实现广播
    broadcaster = tf.TransformBroadcaster()
    
    # 相比于第一种方法,先构建好Message然后再广播,这样写起来好看一些
    tf_body_plat = geometry_msgs.msg.TransformStamped()
    tf_body_plat.header.frame_id = "body"
    tf_body_plat.header.stamp = rospy.Time(0)
    tf_body_plat.child_frame_id = "plat"
    tf_body_plat.transform.translation.x = 0.3
    tf_body_plat.transform.translation.y = 0.6
    tf_body_plat.transform.translation.z = 0.1
    tf_body_plat.transform.rotation.w = 1
    tf_body_plat.transform.rotation.x = 0
    tf_body_plat.transform.rotation.y = 0
    tf_body_plat.transform.rotation.z = 0

    tf_body_rgbd = geometry_msgs.msg.TransformStamped()
    tf_body_rgbd.header.frame_id = "body"
    tf_body_rgbd.header.stamp = rospy.Time(0)
    tf_body_rgbd.child_frame_id = "rgbd"
    tf_body_rgbd.transform.translation.x = 0.4
    tf_body_rgbd.transform.translation.y = 0.6
    tf_body_rgbd.transform.translation.z = 0.2
    tf_body_rgbd.transform.rotation.w = 1
    tf_body_rgbd.transform.rotation.x = 0
    tf_body_rgbd.transform.rotation.y = 0
    tf_body_rgbd.transform.rotation.z = 0

    tf_body_laser = geometry_msgs.msg.TransformStamped()
    tf_body_laser.header.frame_id = "body"
    tf_body_laser.header.stamp = rospy.Time(0)
    tf_body_laser.child_frame_id = "laser"
    tf_body_laser.transform.translation.x = 0.8
    tf_body_laser.transform.translation.y = -0.2
    tf_body_laser.transform.translation.z = 0.3
    tf_body_laser.transform.rotation.w = 1
    tf_body_laser.transform.rotation.x = 0
    tf_body_laser.transform.rotation.y = 0
    tf_body_laser.transform.rotation.z = 0

    tf_plat_cam = geometry_msgs.msg.TransformStamped()
    tf_plat_cam.header.frame_id = "plat"
    tf_plat_cam.header.stamp = rospy.Time(0)
    tf_plat_cam.child_frame_id = "cam"
    tf_plat_cam.transform.translation.x = 0.1
    tf_plat_cam.transform.translation.y = 0.3
    tf_plat_cam.transform.translation.z = -0.2
    tf_plat_cam.transform.rotation.w = 1
    tf_plat_cam.transform.rotation.x = 0
    tf_plat_cam.transform.rotation.y = 0
    tf_plat_cam.transform.rotation.z = 0


    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        # 调用成员函数发送广播
        broadcaster.sendTransformMessage(tf_body_plat)
        broadcaster.sendTransformMessage(tf_body_laser)
        broadcaster.sendTransformMessage(tf_body_rgbd)
        broadcaster.sendTransformMessage(tf_plat_cam)
        
        print "broadcasting transform..."
        rate.sleep()
(4)创建trans_listener节点文件

scripts文件夹上右键选择”新建文件”,输入trans_listener.py即可。

(5)编写trans_listener代码

代码如下:

# coding=utf-8
import rospy
import tf
import geometry_msgs.msg


if __name__ == "__main__":
    # 初始化节点并指定名称
    rospy.init_node("trans_listener")
    # 创建一个tf接收对象,后续调用该对象的成员函数进行接收
    listener = tf.TransformListener()
    # 阻塞直到frame相通
    listener.waitForTransform("body", "cam", rospy.Time(), rospy.Duration(4.0))

    # 新建测试点用于进行测试
    point_in_rgbd = geometry_msgs.msg.PointStamped()
    point_in_rgbd.header.frame_id = "rgbd"
    point_in_rgbd.header.stamp = rospy.Time()
    point_in_rgbd.point.x = 0.4
    point_in_rgbd.point.y = 0.5
    point_in_rgbd.point.z = 0.3

    # 构造Rate对象使循环按指定频率运行
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
            # 测试一,利用lookupTransform接收,监听对应的tf,返回平移和旋转
            # ros::Time(0)表示最近的一帧坐标变换,不能写成ros::Time::now()
            (translation, rotation) = listener.lookupTransform(
                "laser", "body", rospy.Time(0))
            print "translation body->laser:", translation, "\n"

            # 测试二,利用transformPoint应用变换
            # 将rgbd下的点转换到cam下
            tf_p_cam = listener.transformPoint("cam", point_in_rgbd)
            rospy.loginfo("trans1:rgbd_point:(%f,%f,%f) -> cam_point(%f,%f,%f)",
                          point_in_rgbd.point.x, point_in_rgbd.point.y, point_in_rgbd.point.z,
                          tf_p_cam.point.x, tf_p_cam.point.y, tf_p_cam.point.z)

            # 再将刚刚转换的点转回rgbd下,以此验证是否转换正确闭合
            tf_p_rgbd = listener.transformPoint("rgbd", tf_p_cam)
            rospy.loginfo("trans2:cam_point:(%f,%f,%f) -> rgbd_point(%f,%f,%f)\n",
                          tf_p_cam.point.x, tf_p_cam.point.y, tf_p_cam.point.z,
                          tf_p_rgbd.point.x, tf_p_rgbd.point.y, tf_p_rgbd.point.z)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
        rate.sleep()
(6)运行测试

最后在终端中输入roscore启动Master,在scripts目录下打开新终端然后分别输入python trans_broadcaster1.py启动广播节点,python trans_listener.py启动接收节点。运行结果如下:

最后所有代码上传到了Github,点击查看

5.tf常用工具介绍

rosrun tf view_frames:订阅tf树5秒钟,然后保存成一个pdf文件。例如本文开头的树状图就是用这个工具生成的。

rosrun rqt_tf_tree rqt_tf_tree:动态查看当前的tf树。

rosrun tf tf_echo [reference_frame] [target_frame]:查看指定两个frame之间的变换关系。

本文作者原创,未经许可不得转载,谢谢配合

返回顶部