TF坐标变换

机器人系统上,有多个传感器,如激光雷达、摄像头等,有的传感器是可以感知机器人周边的物体方位(或者称之为:坐标,横向、纵向、高度的距离信息)的,以协助机器人定位障碍物,可以直接将物体相对该传感器的方位信息,等价于物体相对于机器人系统或机器人其它组件的方位信息吗?显示是不行的,这中间需要一个转换过程。更具体描述如下:

场景1:雷达与小车

现有一移动式机器人底盘,在底盘上安装了一雷达,雷达相对于底盘的偏移量已知,现雷达检测到一障碍物信息,获取到坐标分别为(x,y,z),该坐标是以雷达为参考系的,如何将这个坐标转换成以小车为参考系的坐标呢?

 场景2:现有一带机械臂的机器人(比如:PR2)需要夹取目标物,当前机器人头部摄像头可以探测到目标物的坐标(x,y,z),不过该坐标是以摄像头为参考系的,而实际操作目标物的是机械臂的夹具,当前我们需要将该坐标转换成相对于机械臂夹具的坐标,这个过程如何实现?

 当然,根据我们高中学习的知识,在明确了不同坐标系之间的的相对关系,就可以实现任何坐标点在不同坐标系之间的转换,但是该计算实现是较为常用的,且算法也有点复杂,因此在 ROS 中直接封装了相关的模块: 坐标变换(TF)。

概念

tf:TransForm Frame,坐标变换

坐标系:ROS 中是通过坐标系统开标定物体的,确切的将是通过右手坐标系来标定的。

作用

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

说明

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

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

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

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

1 坐标msg消息

订阅发布模型中数据载体 msg 是一个重要实现,首先需要了解一下,在坐标转换实现中常用的 msg:geometry_msgs/TransformStampedgeometry_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;
}

 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

 

  • 1
    点赞
  • 25
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值