ROS教程——1.6 如何使用TF变换

转载自:ROS教程——1.6 如何使用TF变换

ROS教程——1.6 如何使用TF变换

原创 huicanlin Robot404 2019-07-03 15:10

1.6.1 ROS的变换系统(TF)

(1)TF简介

什么是TF。机器人系统通常具有许多随时间变化的坐标系,例如世界坐标系、基座坐标系、夹持器坐标系、头部坐标系等。TF是一个变换系统,它允许在一个坐标系中进行计算,然后在任何所需的时间点将它们转换到另一个坐标系。TF能够回答以下问题:地图坐标系中机器人基座的当前位姿是什么?相对于机器人基座,夹持器中物体的位姿是什么?5秒前,头部坐标系在世界坐标系的什么地方?

TF的优点。分布式系统,即没有单点故障;转换多次时没有数据丢失;没有坐标系之间的中间数据转换的计算成本;用户无需担心其数据的起始坐标系;有关过去位置的信息进行存储并且可访问(在本地记录开始后)。

TF节点。有两种类型的tf节点:发布器 - 发布坐标系之间的变换转换到/tf;订阅器 - 订阅/ tf并缓存所有数据直至缓存极限。

TF变换树。TF在坐标系之间构建变换树,支持多个非连接树,仅在同一树中变换有效,见图1。

图片

图1 Transform Tree

变换树示例。如图2-图3,即为Nao机器人的TF变换树。

图片

图2 Nao机器人及其坐标系

图片

图3 Nao机器人的变换树

(2)如何使用TF树

假设我们希望robot2根据robot1的激光数据进行导航,给定TF树,见图4-图5。

图片

图4 如何使用TF

图片

图5 Inverse Transform and Forward Transform

TF Demo。根据如下命令,启动turtle_tf_demo:

$ roslaunch turtle_tf turtle_tf_demo.launch

现在,我们可以看到,在一个窗口中的两只乌龟,一只跟随着另一只。

该启动文件的内容如下:

<launch>

  <!-- Turtlesim Node-->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
  <!-- Axes -->
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>

  <node name="turtle1_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle1" />
  </node>
  <node name="turtle2_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle2" />
  </node>
  <node name="turtle_pointer" pkg="turtle_tf" type="turtle_tf_listener.py" respawn="false" output="screen" >
  </node>

</launch>

由此可知,同时启动了turtle_tf_listener和turtle_teleop_key,因此,使用键盘箭头键在turtlesim中驱使一开始就位于中心的乌龟,示例轨迹如下:

图片

图6 TF Demo

该Demo使用TF库创建3个坐标系,分别是世界坐标系、turtle1坐标系和turtle2坐标系;包含一个TF广播器(broadcaster)发布turtle坐标系,一个TF订阅器(listener)计算两个turtle坐标系之间的距离。

(3)TF命令行工具

view_frames:可视化完整的坐标变换树;

通过view_frames创建一个由TF使用ROS广播的坐标系变换树:

图片

图7 运行view_frames

查看该变换树的命令如下:

$ evince frames.pdf

图片

图8 查看变换数

tf_monitor:监视坐标系之间的变换;

打印当前坐标系变换树信息到控制台:

图片

图9 tf_monitor

tf_echo:将指定的变换输出到屏幕;

tf_echo通过ROS广播任意两个坐标系之间的变换,使用示例:

$ rosrun tf tf_echo [reference_frame] [target_frame]

让我们看一下turtle2坐标系相对于turtle1坐标系的变换,它相当于:

图片

命令如下:

$ rosrun tf tf_echo turtle1 turtle2

当驱动乌龟时,TF变换将随着两只乌龟相对移动而改变:

图片

图10 tf echo

roswtf:使用tfwtf插件,帮助我们跟踪TF的问题;

static_transform_publisher:一个用于发送静态变换的命令行工具。

(4)旋转的表示

旋转有多种表示方式:

欧拉角:分别绕Z、Y和X轴的偏航角、俯仰角和滚转角;旋转矩阵;四元数;

图片

图片

图11 旋转的表示

四元数

在数学中,四元数是一个扩展复数的数字系统,四元数乘法的基本公式(Hamilton,1843):

图片

四元数可用于理论和应用数学,特别是涉及3D旋转的计算,例如计算机图形和计算机视觉。

四元数和空间旋转

3D中的任何旋转都可以表示为矢量

图片

(欧拉轴)和标量

图片

(旋转角度)的组合;围绕某个轴经过角度

的旋转可由单位矢量定义:

图片

该四元数表示为:

图片

四元数提供了一种简单的方法,用4个数字表示绕轴旋转的角度,可以使用简单的公式将相应的旋转应用于位置矢量:

http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

使用四元数的优点:非奇异性、比矩阵更紧凑(和更快)。

(5)rviz和tf

让我们用rviz来看看上述示例中乌龟坐标系通过rviz的-d选项,使用turtle_tf配置文件启动rviz:

$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

在左侧栏中,您将看到由tf广播的坐标系;

请注意,固定坐标系是/world,不随时间移动;

当驱动乌龟时,发现rviz中的坐标系发生了变化,在左侧栏选中"Show Names"和"Show Arrows"将更加直观。

图片

图12 rviz和tf

(6)广播TF变换

TF广播器将坐标系的相对位姿发送给系统的其余部分,一个系统可以有多个广播器,每个广播器提供机器人不同部分的信息。接下来,我们将编写代码来重现上述tf Demo。

写一个广播器。首先创建一个名为lecture1-6的新包,它依赖于tf,roscpp,rospy和turtlesim:

$ cd ~/catkin_ws/src/ros_tutorial/
$ catkin_create_pkg lecture1-6 tf roscpp rospy turtlesim

lecture1-6/src添加一个名为tf_broadcaster.cpp的新源文件,内容如下:

#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;
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    tf::Quaternion quaternion;
    transform.setRotation(tf::createQuaternionFromYaw(msg->theta));
    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];

    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    ros::spin();
    return 0;
};

发送变换

使用TransformBroadcaster发送变换需要4个参数:变换对象;时间戳,通常我们用当前时间,ros::Time::now();所创建链接的父坐标系名称,在本例中为“world”;所创建链接的子坐标系名称,本例中为乌龟本身名称。

示例如下:

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

创建并运行广播器

在CMakeLists.txt文件中插入:

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

通过catkin_make构建包:

$ cd ~/catkin_ws/
$ catkin_make

/lecture1-6/launch下创建启动文件tf_demo.launch,文件内容如下:

<launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!-- tf broadcaster node -->
    <node pkg="lecture1-6" type="tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
  </launch>

运行启动文件:

$ cd ~/catkin_ws/src
$ roslaunch lecture1-6 tf_demo.launch

检查结果

使用tf_echo工具检查乌龟位姿是否发布到tf

$ rosrun tf tf_echo /world /turtle1

图片

图13 检查结果

(7)监听TF变换

TF监听器接收并缓存在系统中广播的所有坐标系,并查询坐标系之间的指定变换,接下来我们将创建一个TF监听器,它将监听来自TF广播器的变换,使用以下代码将tf_listener.cpp添加到lecture1-6/src中:

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

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

    ros::NodeHandle node;

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

    ros::Publisher turtle_vel =
       node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    tf::TransformListener listener;
    ros::Rate rate(10.0);

    while (node.ok()){
        tf::StampedTransform transform;
        try {
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0) );
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        } catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
        }

        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4 * 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;
};

代码简述,要使用TransformListener,我们需要包含tf / transform_listener.h头文件;一旦创建了监听器,它就开始接收变换,并缓存最近10秒钟的变换信息;TransformListener对象应限定为持久化,否则其缓存将无法填充,几乎每个查询都将失败;一种常见的方法是使TransformListener对象成为类的成员变量。

TransformListener的核心方法:LookupTransform(), WaitForTransform(), CanTransform()。

LookupTransform(): 获取两个坐标系之间的变换;

要查询侦听器以获取指定变换,需要传递4个参数:变换源自哪个坐标系;变换的目标坐标系;想要的变换的时间。提供ros :: Time(0)将提供最新的变换。存储变换结果的对象。

例如:

listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);

WaitForTransform(): 等待直到超时或转换可用;

CanTransform(): 测试变换是否为坐标框系之间进行转换。

创建并运行监听器

将以下行添加到lecture1-6/CMakeLists.txt:

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

通过调用catkin_make来构建包:

$ cd ~/catkin_ws
$ catkin_make

Launch 文件

将以下行添加到lecture1-6/tf_demo.launch:

    <!-- Second broadcaster node -->
    <node pkg="lecture1-6" type="tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />
          
    <!-- tf listener node -->
    <node pkg="lecture1-6" type="tf_listener" name="listener" />

此时的tf_demo.launch文件:

<launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!-- tf broadcaster node -->
    <node pkg="lecture1-6" type="tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
          
    <!-- Second broadcaster node -->
    <node pkg="lecture1-6" type="tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />
          
    <!-- tf listener node -->
    <node pkg="lecture1-6" type="tf_listener" name="listener" />
</launch>

检查结果

要查看是否有效,只需使用箭头键驱动第一只乌龟(确保终端窗口处于活动状态,而不是模拟器窗口),然后您将看到第二只乌龟跟随第一只乌龟。对比(2)如何使用TF树可知通过代码实现了相应的功能。

1.6.2 获取机器人定位信息

典型的TF坐标系

图片

图14 典型的TF坐标系

odom: 仅使用测距测量的自洽坐标系;

base_footprint: 机器人在地面以上零高度的基础;

base_link: 机器人的基座,位于机器人旋转中心;

base_laser_link: 激光传感器的位置;

Turtlebot TF坐标系

图片

图15 Turtleot的TF坐标系

现在,我们通过一个示例,演示如何使用TF确定机器人在世界坐标系的当前位置。首先,更改Gazebo中TurtleBot的初始位置(默认为x = 0,y = 0),我们通过设置环境变量ROBOT_INITIAL_POSE来更改初始位置,例如:

export ROBOT_INITIAL_POSE="-x -1 -y -2"

要使机器人处于其自己坐标系中的位置(即,相对于其在地图上的起始位置),创建一个从/map坐标系到/base_footprint坐标系的监听器,代码如下:

#include <ros/ros.h>
#include <tf/transform_listener.h>

using namespace std;

int main(int argc, char** argv){
    ros::init(argc, argv, "robot_location");
    ros::NodeHandle node;

    tf::TransformListener listener;
    ros::Rate rate(2.0);

    listener.waitForTransform("/map", "/base_footprint", ros::Time(0), ros::Duration(10.0) );

    while (ros::ok()){
        tf::StampedTransform transform;
        try {

            listener.lookupTransform("/map", "/base_footprint", ros::Time(0), transform);
            double x = transform.getOrigin().x();
            double y = transform.getOrigin().y();

            cout << "Current position: (" << x << "," << y << ")" << endl;
        } catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
        }
        rate.sleep();
    }
    return 0;
}

静态变换发布者

为了获取机器人在全局坐标系中的位置,需要由某个节点发布map-> odom变换;此变换通常由ROS的地图构建节点或定位节点之一发布(下一教程);在假设机器人的定位准确时,可以在这些坐标系之间发布静态(固定)变换:

<launch>
  <!-- Publish a static transformation between /map and /odom -->
  <node name="tf" pkg="tf" type="static_transform_publisher" args="-1 -2 0 0 0 0 /map /odom 100" />
</launch>

监听从/map到/base_footprint的变换,以便在地图坐标系获取机器人的位置:

    while (ros::ok()){
        tf::StampedTransform transform;
        try {
            listener.lookupTransform("/map", "/base_footprint", ros::Time(0), transform);
            double x = transform.getOrigin().x();
            double y = transform.getOrigin().y(); 
            cout << "Current position: (" << x << "," << y << ")" << endl;
        } catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
        }
        rate.sleep();
    } 

图片

图16 新的TF变换树

<launch>
  <param name="/use_sim_time" value="true"/>  

  <!-- Run Gazebo with turtlebot -->
  <include file="$(find turtlebot_gazebo)/launch/turtlebot_world.launch"/>

  <!-- Publish a static transformation between /odom and /map -->
  <node name="tf" pkg="tf" type="static_transform_publisher" args="-1 -2 0 0 0 0 /map /odom 100" />

  <!-- Run node -->
  <node name="robot_location" pkg="lecture1-6" type="robot_location" output="screen" />

</launch>

lecture1-6/CMakeLists.txt文件中加入:

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

使用catkin_make创建包:

$ cd ~/catkin_ws/
$ catkin_make

启动:

 $ roslaunch lecture1-6 robot_location.launch

运行正确将得到:

图片

图17 Turtleot的TF坐标系

在rviz中查看TF变换运行rviz

$ roslaunch turtlebot_rviz_launchers view_robot.launch

单击TF复选框

图片

图18 在rviz中查看TF变换

1.6.3 练习

(1)编写函数,将机器人在世界中的当前位置(x,y)转换为机器人网格图中所在的位置单元格坐标(i,j),反之亦然;此处,考虑地图分辨率。

(2)输出机器人初始单元格位置。


评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值