【自动驾驶】ROS中的TF坐标变换（一）：静态坐标变换

颢师傅

于 2024-08-15 23:15:00 发布

阅读量423

点赞数 14

文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/hh1357102/article/details/141188690

版权

引子

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

ros中的右手坐标系

tf:TransForm Frame,坐标变换
在这里插入图片描述
坐标系:ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。

补充：

欧拉角及四元数理解

欧拉角：
对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。而坐标系则固定于刚体，随着刚体的旋转而旋转。
在这里插入图片描述

在空间中会有n+1个坐标系，其中只有一个坐标系起到标定作用，也就是说“其他n个坐标系全都是基于该坐标系找到自己在空间中的位置的”。只有大家都知道了自己在空间中的具体位置，坐标转换才可以顺利进行下去。

在描述机器人运动时，我们常常提及“位姿”，其实位姿是一个合成词，我们可以将其拆解为“位置+姿态”。位置就是指“机器人某个运动关节/测量传感器在世界坐标系中的具体位置，姿态就是”基于该点的坐标系相较于世界坐标系所进行的旋转“。
四元数就比较复杂了，可以参考：
解析四元数
 四元数的应用
四元数。就是通过四元数连乘的形式表达机器人末端相对于基坐标系的关系，或者先有关系，再通过求逆的运算求解每个关节应页的位置关系。四元数只能表示旋转，不能表示平移。

旋转

坐标变换，当没有平移，只有旋转操作时，可以理解为一个向量在原坐标系中的投影，又在另一个坐标系中的投影：
其中：

在这里插入图片描述
是坐标变换的旋转矩阵，将向量P在原B坐标系变换至A坐标系中。其组成是【X Y Z】即单位化的B坐标系的轴在A坐标系中的投影。

平移操作

在这里插入图片描述

复合操作

在这里插入图片描述

运行坐标变换的例子

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch

启动乌龟跟随例子窗口。

坐标转换

ros中封装了现成的坐标变换模块。
在坐标转换实现中常用的 msg:

（1）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

translation指child_frame_id 相对于frame_id 的变化

（2）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

x,y,z是坐标点的具体坐标值。
具体结构都可以用rosmsg info查看

静态坐标变换-发布坐标系信息

静态坐标变换
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。
坐标系相对关系，可以通过发布方发布
订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出。
场景：
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

创建功能包

 catkin_create_pkg tfdemo_publisher std_msgs rospy roscpp tf2 tf2_ros tf2_geometry_msgs geometry_msgs

除了基本依赖以外，新增了若干坐标系相关的依赖。
编写c++文件

// 1.包含头文件
// 2.设置编码，节点初始化
//主要是发布两个坐标系间的相对关系
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
//引入四元数包
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"static_brocast");
    // 3.创建静态坐标转换广播器，专用于发布静态坐标
    tf2_ros::StaticTransformBroadcaster broadcaster;
    // 4.创建坐标系信息
    geometry_msgs::TransformStamped ts;
    //----设置头信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";//相对坐标系关系中被参考的那一个
    //----设置子级坐标系
    ts.child_frame_id = "laser";//就是另一个坐标系，子集
    //----设置子级相对于父级的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    //----设置四元数:将 欧拉角数据转换成四元数
    tf2::Quaternion qtn;
    //使用该对象设置欧拉角，然后调用函数将欧拉角转换为四元数，这里欧拉角刚好都是0，这里的单位都是弧度
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 5.广播器发布坐标系信息
    broadcaster.sendTransform(ts);
    //静态的发布一次即可，当ROS节点调用ros::spin()时，它会一直等待直到有消息到达该节点。当节点接收到消息时，
    //它将调用与该消息相关联的回调函数进行处理。这使得ROS节点能够响应其他节点的请求，同时保持事件循环处于活动状态。
    //也就是维持一个事件循环
    ros::spin();
    return 0;
}

还有cmakelist文件：

add_executable(${PROJECT_NAME}_node src/demo01.cpp)
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
)

编译并运行工程：

source /root/work/ws/devel/setup.bash
roscore
rosrun tfdemo_publisher tfdemo_publisher_node

如果roscore提示已启动，可使用killall -9 rosmaster杀掉重启。
查询当前话题：

rostopic list

得到：

/rosout
/rosout_agg
/tf_static

查看：

rostopic echo  /tf_static

得到：

transforms: 
  - 
    header: 
      seq: 100
      stamp: 
        secs: 1723624279
        nsecs: 917443332
      frame_id: "base_link"
    child_frame_id: "laser"
    transform: 
      translation: 
        x: 0.2
        y: 0.0
        z: 0.5
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

或者在rviz里也可以查看。

静态坐标变换-订阅坐标系信息

订阅demo01内的坐标系相对关系，传入坐标点，再进行转换。

添加cpp订阅者主文件

为了简便，该订阅者cpp文件与发布者在同一个包的同一个src下，在这里插入图片描述


//1.包含头文件
#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" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点,listener订阅数据，并缓存至Buffer当中
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 1;
        point_laser.point.y = 2;
        point_laser.point.z = 7.3;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常.....");
        }

        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

修改cmakelist

新增节点2即可：

add_executable(${PROJECT_NAME}_node src/demo01.cpp)
add_executable(${PROJECT_NAME}_node2 src/demo02.cpp)

target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_node2
  ${catkin_LIBRARIES}
)

文件编译报错的解决方案

如果碰到：./boost/thread/pthread/thread_data.hpp:60:5: error: missing binary operator before token “(“报错
在这里插入图片描述
即可编译通过。

运行程序进行测试

source /root/work/ws/devel/setup.bash
roscore
rosrun tfdemo_publisher tfdemo_publisher_node
rosrun tfdemo_publisher tfdemo_publisher_node2

在这里插入图片描述

颢师傅

关注

14
点赞
踩
22

收藏

觉得还不错? 一键收藏
打赏
0
评论
【自动驾驶】ROS中的TF坐标变换（一）：静态坐标变换

机器人系统上，有多个传感器，如激光雷达、摄像头等，有的传感器是可以感知机器人周边的物体方位(或者称之为:坐标，横向、纵向、高度的距离信息)的，以协助机器人定位障碍物，可以直接将物体相对该传感器的方位信息，等价于物体相对于机器人系统或机器人其它组件的方位信息吗？现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。
复制链接

扫一扫