资讯详情

TF坐标变换

在机器人系统中，有多个传感器，如激光雷达、摄像头等。有些传感器可以感知机器人周围物体的方向(或坐标、横向、纵向、高度的距离信息)，帮助机器人定位障碍物，直接将物体相对于传感器的方向信息等同于物体相对于机器人系统或机器人其他组件的方向信息。显示不好，中间需要一个转换过程。具体描述如下：

场景1:雷达和汽车

现有的移动机器人底盘在底盘上安装了雷达。雷达已知与底盘的偏移量相比。现在雷达检测到障碍物信息，分别获得坐标(x,y,z)，该坐标以雷达为参考系，如何将该坐标转换为以汽车为参考系的坐标？

场景2:一带机械臂机器人(如:PR2)需要夹住目标，目前机器人头部摄像头可以检测到目标的坐标(x,y,z)，然而，坐标以摄像头为参考系统，实际目标是机械臂夹具。目前，我们需要将坐标转换为相对于机械臂夹具的坐标。如何实现这一过程？

当然，根据我们在高中学到的知识，在明确不同坐标系之间的相对关系后，可以实现不同坐标系之间任何坐标点的转换，但计算实现更常用，算法有点复杂，所以在 ROS 相关模块直接包装在中间： 坐标变换(TF)。

TransForm Frame,坐标变换

ROS 物体通过坐标系开标，右手坐标系确切标定。

在 ROS 用于实现不同坐标系之间的点或向量转换。

在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2.后者更简洁高效，tf对应的常用功能包有：

tf2_geometry_msgs:可以将ROS消息转换成tf2消息。

tf2: 包装坐标变换的常用消息。

tf2_ros:为tf2提供了roscpp和rospy绑定，包装常用的坐标变换API。

1 坐标msg消息

订阅发布模型中的数据载体 msg 这是一个重要的实现。首先，我们需要了解坐标转换的常用实现 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped

前者用于传输坐标系中坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息

1.geometry_msgs/TransformStamped

命令行输入：rosmsg info geometry_msgs/TransformStamped

std_msgs/Header header                     #头信息   uint32 seq                                #|-- 序列号   time stamp                                #|-- 时间戳   string frame_id                            #|-- 坐标 ID string child_frame_id                    #子坐标系 id geometry_msgs/Transform transform        #坐标信息   geometry_msgs/Vector3 translation        #偏移量     float64 x                                #|-- X 方向偏移     float64 y                                #|-- Y 方向偏移     float64 z                                #|-- Z 方向偏移   geometry_msgs/Quaternion rotation        #四元数     float64 x                                     float64 y                                     float64 z                                     float64 w

2.geometry_msgs/PointStamped

命令行输入：rosmsg info geometry_msgs/PointStamped

std_msgs/Header header                    #头   uint32 seq                                #|-- 序号   time stamp                                #|-- 时间戳   string frame_id                            #|-- 坐标系 id geometry_msgs/Point point                #点坐标   float64 x                                    #|-- x y z 坐标   float64 y   float64 z

2 静态坐标变换

静态坐标变换是指两个坐标系之间的相对位置是固定的。

现有机器人模型的核心组成包括主体和雷达，每个对应的坐标系。坐标系的原点位于主体和雷达的物理中心。已知雷达原点与主原点的位移关系如下： x 0.2 y0.0 z0.5.目前雷达检测到一个障碍物，雷达坐标系中障碍物的坐标是 (2.0 3.0 5.0)，与主体坐标相比，这个障碍物有多少？

  1. 坐标系的相对关系可以通过出版商发布
  2. 订阅者，订阅到发布的坐标系的相对关系，然后将坐标点信息(可以写死)传入，然后借助 tf 实现坐标变换，输出结果

C 与 Python 实现一致的流程

  1. 新建功能包，添加依赖
  2. 实现编写发布方
  3. 编写订阅方实现
  4. 执行并检查结果

1.创建功能包 tf01_static

创建项目的功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方 demo01_static_pub.cpp

#include "ros/ros.h" #include "tf2_ros/static_transform_broadcaster.h" #include "geometry_msgs/TransformStamped.h"    #include "tf2/LinearMath/Quaternion.h"        //设置欧拉角 /*      静态坐标变换发布方：         发布关于 laser 坐标系位置信息       实现流程:         1.包含头文件         2.初始化 ROS 节点         3.创建静态坐标转换广播器         4.创建坐标系信息         5.广播器发布坐标系信息         6.spin() */  int main(int argc, char *argv[]) {     setlocale(LC_ALL,"");     ros::init(argc,argv,"static_pub");     ros::NodeHandle nh;     //3 创建静态坐标转换广播器     tf2_ros::StaticTransformBroadcaster pub;     //4 组织发布的消息     geometry_msgs::TransformStamped tfs;     tfs.header.stamp =ros::Time::now();     tfs.header.frame_id = "base_link"; ////参考相对坐标系中的那个     tfs.child_frame_id = "laser";     tfs.transform.translation.x = 0.2;     tfs.transform.translation.y = 0.0;     tfs.transform.translation.z = 0.5;     ///需要根据欧拉角转换     tf2::Quaternion qtn;//创建 四元数  对象     //向该对象设置欧拉角，这个对象可以将欧拉角转换为四元     qtn.setRPY(0,0,0)；//欧拉角单位为弧度     tfs.transform.rotation.x = qtn.getX();     tfs.transform.rotation.y = qtn.getY();     tfs.transform.rotation.z = qtn.getZ();     tfs.transform.rotation.w = qtn.getW();     ///发布数据     pub.sendTransform(tfs);     ros::spin();     return 0; }

/p>

 3.订阅方   demo02_static_sub.cpp

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"   //监听
#include "tf2_ros/buffer.h"              //将订阅的数据缓存，与transform_listener.h一起使用
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  //注意: 调用 transform 必须包含该头文件
/*
  订阅方：订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换

  流程：
       1.包含头文件
       2.编码、初始化、NodeHandle
       3创建订阅对象；--->订阅坐标系相对关系
       4创建一个坐标点数据
       5转换算法，需要调用TF内置实现
       6最后输出
*/

int main(int argc, char *argv[])
{
    //2编码、初始化、NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    //3创建订阅对象；--->订阅坐标系相对关系
    //3-1创建一个buffer缓存
    tf2_ros::Buffer buffer;
    //3-2再创建监听对象(监听对象可以将订阅的数据传入buffer)
    tf2_ros::TransformListener listener(buffer);
    //4创建一个坐标点数据
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    //添加休眠
    //ros::Duration(2).sleep();
    //5转换算法，需要调用TF内置实现
    ros::Rate rate(10);
    while(ros::ok())
    {
        //核心代码   将ps转换成相对于base_link的坐标点
        geometry_msgs::PointStamped ps_out;
        /*
        调用了buffer的转换函数transform
        参数1：被转换的坐标点
        参数2：目标坐标系
        返回值：输出的坐标点
        ps1:调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
        ps2:运行时存在的问题：抛出一个异常 base_link不存在
             原因：订阅数据是一个耗时操作，可能在调用transform转换函数时，坐标系的
                   相对关系还没有订阅，因此出现异常

             解决：
                 方案1：在调用转换函数前，执行休眠
                 方案2：进行异常处理
        */
        try
        {
            ps_out = buffer.transform(ps,"base_link");
            //6最后输出
            ROS_INFO("转换后的坐标值：(%.2f,%.2f,%.2f),参考的坐标系:%s",
                     ps_out.point.x,
                     ps_out.point.y,
                     ps_out.point.z,
                     ps_out.header.frame_id.c_str()
                     );
        }
        catch(const std::exception& e)
        {
            //std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%S",e.what());
        }
        
        
        rate.sleep();
        ros::spinOnce();
    }
    

    return 0;
}

先知道坐标系间转换，再推出坐标点的转换

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，控制台将输出，坐标转换后的结果。

补充1:

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser

也建议使用该种方式直接实现静态坐标系相对信息发布。

补充2:

可以借助于rviz显示坐标系关系，具体操作:

  • 新建窗口输入命令:rviz;
  • 在启动的 rviz 中设置Fixed Frame 为 base_link;
  • 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系。

3 动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

  1. 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点

  2. 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度

  3. 将 pose 信息转换成 坐标系相对信息并发布

C++ 与 Python 实现流程一致

  1. 新建功能包，添加依赖

  2. 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)

  3. 创建坐标相对关系订阅方

  4. 执行

1.创建功能包   tf02_dynamic

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方      demo01_dynamic_pub.cpp

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
    发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
    准备：
       话题：/turtle1/pose
       消息：/turtlesim/Pose

    流程：
       1.包含头文件
       2设置编码、初始化、NodeHandle
       3创建订阅对象、订阅/turtle/pose
       4回调函数处理订阅的消息，将位姿信息转换成坐标相对关系并发布
       5spin()
*/

void doPose(const turtlesim::Pose::ConstPtr& pose){
     //获取位姿信息，转换成坐标系相对关系，并发布
     //1创建TF发布对象    创建 TF 广播器
     static tf2_ros::TransformBroadcaster pub;
     //2组织被发布的数据   创建 广播的数据(通过 pose 设置)
     geometry_msgs::TransformStamped ts;
     //  |----头设置
     ts.header.frame_id = "world";
     ts.header.stamp = ros::Time::now();
     //  |----坐标系 ID
     ts.child_frame_id ="turtle1";
     //坐标系偏移量设置   二维实现，pose 中没有z，z 是 0
     ts.transform.translation.x = pose->x;
     ts.transform.translation.y = pose->y;
     ts.transform.translation.z = 0;
     //坐标系四元数
     /*
      位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是2D，没有翻滚与俯仰角度，
      所以可以得出乌龟的欧拉角：0 0 theta
    */
     tf2::Quaternion qtn;
     qtn.setRPY(0,0,pose->theta);

     ts.transform.rotation.x = qtn.getX();
     ts.transform.rotation.y = qtn.getY();
     ts.transform.rotation.z = qtn.getZ();
     ts.transform.rotation.w = qtn.getW();
     //3发布
     pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    //3创建订阅对象、订阅/turtle/pose
    ros::Subscriber sub = nh.subscribe("turtle1/pose",100,doPose);
    ros::spin();
    return 0;
}

 记得修改左上角的Fixed Frame 为world   还要添加TF才能显示

3.订阅方   demo02_dynamic_sub.cpp

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
/*
  订阅方：订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换

  流程：
       1.包含头文件
       2.编码、初始化、NodeHandle
       3创建订阅对象；--->订阅坐标系相对关系
       4创建一个坐标点数据
       5转换算法，需要调用TF内置实现
       6最后输出
*/

int main(int argc, char *argv[])
{
    //2编码、初始化、NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    //3创建订阅对象；--->订阅坐标系相对关系
    //3-1创建一个buffer缓存
    tf2_ros::Buffer buffer;
    //3-2再创建监听对象(监听对象可以将订阅的数据传入buffer)
    tf2_ros::TransformListener listener(buffer);
    //4创建一个坐标点数据
    geometry_msgs::PointStamped ps;
    //参考的坐标系
    ps.header.frame_id = "turtle1";
    //时间戳
    ps.header.stamp = ros::Time(0.0);
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    //添加休眠
    //ros::Duration(2).sleep();
    //5转换算法，需要调用TF内置实现
    ros::Rate rate(10);
    while(ros::ok())
    {
        //核心代码   将ps转换成相对于base_link的坐标点
        geometry_msgs::PointStamped ps_out;
        /*
        调用了buffer的转换函数transform
        参数1：被转换的坐标点
        参数2：目标坐标系
        返回值：输出的坐标点
        ps1:调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h

        */
        try
        {
            ps_out = buffer.transform(ps,"world");
            //6最后输出
            ROS_INFO("转换后的坐标值：(%.2f,%.2f,%.2f),参考的坐标系:%s",
                     ps_out.point.x,
                     ps_out.point.y,
                     ps_out.point.z,
                     ps_out.header.frame_id.c_str()
                     );
        }
        catch(const std::exception& e)
        {
            //std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%S",e.what());
        }
        
        
        rate.sleep();
        ros::spinOnce();
    }
    

    return 0;
}

 相比于demo02_static_sub.cpp需要修改三个地方:ps.header.frame_id = "turtle1";ps.header.stamp = ros::Time(0.0);ps_out = buffer.transform(ps,"world");

依次启动：

  1. roscore
  2. rosrun turtlesim turtlesim_node
  3. rosrun tf02_dynamic demo01_dynamic_pub
  4. rosrun tf02_dynamic demo02_dynamic_sub
  5. rosrun turtlesim turtle_teleop_key

 

标签： 0280217518传感器tftf14传感器

锐单商城拥有海量元器件数据手册、 IC替代型号，打造 电子元器件IC百科大全！

锐单商城 - 一站式电子元器件采购平台