ROS中的TF坐标变换：从传感器数据到机器人系统的精准定位-CSDN博客

1、ROS的TF坐标变换

在机器人系统中，配备了多种传感器，例如激光雷达和摄像头等。这些传感器能够感知机器人周围的物体位置，包括坐标、横向、纵向以及高度的距离信息。它们的作用是协助机器人准确定位障碍物。然而，并非所有传感器都能直接提供物体相对于机器人系统或其他组件的方位信息。虽然可以获取物体相对特定传感器的方位信息，但这并不等同于物体相对于整个机器人系统或其他组件的方位信息。在信息显示方面存在限制，因为这需要经历一定的转换过程。更详细地描述如下：

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

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

2、坐标变换的msg信息

在坐标变幻中常用的msg是geometry_msgs/TransformStamped和geometry_msgs/PointStamped。我们可以打开终端用rosmsg info查看：

这里的frame_id是被参考的坐标系，child_frame_id是另外的坐标系，transform是child_frame_id相对于frame_id的偏移量。

这里的frame_id是指我的坐标点是以哪个坐标系为参考的。xyz就是坐标点的值

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

3、静态坐标变换

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

比如我们手持扫描设备中的相机和雷达之间就是不会变换的。

3.1 C++ clion实现静态坐标变换

3.1.1 CMakeLists.txt配置

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

cmake_minimum_required(VERSION 2.8.3)
project(test)
 
######################
### Cmake flags
######################
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")
 
find_package(catkin REQUIRED COMPONENTS
    roscpp
    rospy
    roslib
    # msg
    std_msgs
    sensor_msgs
    tf2
    tf2_ros
    tf2_geometry_msgs
    geometry_msgs
     
)
 
catkin_package()
 
include_directories(${catkin_INCLUDE_DIRS})
 
add_executable(testing main.cpp)
 
target_link_libraries(testing ${catkin_LIBRARIES})

3.1.2 package.xml配置

<?xml version="1.0"?>
<package>
  <name>test</name>
  <version>0.0.0</version>
  <description>A test</description>
  <maintainer email="haha@nefu.com">haha</maintainer>
  <author>HITLHW</author>
  <license>BSD-3</license>
 
 
  <buildtool_depend>catkin</buildtool_depend>
 
  <build_depend>roscpp</build_depend>
  <run_depend>roscpp</run_depend>
  <build_depend>rospy</build_depend>
  <run_depend>rospy</run_depend>
 
  <build_depend>pcl_conversions</build_depend>
  <run_depend>pcl_conversions</run_depend>
 
  <build_depend>std_msgs</build_depend>
  <run_depend>std_msgs</run_depend>
  <build_depend>sensor_msgs</build_depend>
  <run_depend>sensor_msgs</run_depend>
  <build_depend>geometry_msgs</build_depend>
  <run_depend>geometry_msgs</run_depend>
 
  <build_depend>tf2</build_depend>
  <run_depend>tf2</run_depend>
  <build_depend>tf2_ros</build_depend>
  <run_depend>tf2_ros</run_depend>
  <build_depend>tf2_geometry_msgs</build_depend>
  <run_depend>tf2_geometry_msgs</run_depend>
</package>

3.1.3 发布节点建立

建立发布节点：

cmake_minimum_required(VERSION 2.8.3)
project(test)
 
######################
### Cmake flags
######################
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")
 
find_package(catkin REQUIRED COMPONENTS
    roscpp
    rospy
    roslib
    # msg
    std_msgs
    sensor_msgs
    tf2
    tf2_ros
    tf2_geometry_msgs
    geometry_msgs
     
)
 
catkin_package()
 
include_directories(${catkin_INCLUDE_DIRS})
 
add_executable(testing main.cpp)
add_executable(static_pub static_pub.cpp)
 
 
target_link_libraries(testing ${catkin_LIBRARIES})
target_link_libraries(static_pub ${catkin_LIBRARIES})

我们的流程如下：

静态坐标变换发布方:
    发布关于 laser 坐标系的位置信息

//
// Created by liuhongwei on 23-12-1.
//
/*
    静态坐标变换发布方:
        发布关于 laser 坐标系的位置信息
    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建静态坐标转换广播器
        4.创建坐标系信息
        5.广播器发布坐标系信息
        6.spin()
*/
 
 
// 1.包含头文件
#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;
    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::spin();
    return 0;
}

注释的很清楚了！主体是base_link，雷达是laser。

我们执行这个节点：

看一下ROS话题：

已经有啦～

我们看看该话题的信息：

我们还可以通过rviz查看：

3.1.4 接收节点建立

//
// Created by liuhongwei on 23-12-1.
//
/*
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点
    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 TF 订阅节点
        4.生成一个坐标点(相对于子级坐标系)
        5.转换坐标点(相对于父级坐标系)
        6.spin()
*/
//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 订阅节点
    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),参考的坐标系是:%s",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;
}

3.1.5 运行结果

运行订阅发布节点：

成功，你学会了吗？？

最后的CMakeLists.txt如下：

cmake_minimum_required(VERSION 2.8.3)
project(test)
 
######################
### Cmake flags
######################
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")
 
find_package(catkin REQUIRED COMPONENTS
    roscpp
    rospy
    roslib
    # msg
    std_msgs
    sensor_msgs
    tf2
    tf2_ros
    tf2_geometry_msgs
    geometry_msgs
     
)
 
catkin_package()
 
include_directories(${catkin_INCLUDE_DIRS})
 
add_executable(testing main.cpp)
add_executable(static_pub static_pub.cpp)
add_executable(static_sub static_sub.cpp)
 
 
target_link_libraries(testing ${catkin_LIBRARIES})
target_link_libraries(static_pub ${catkin_LIBRARIES})
target_link_libraries(static_sub ${catkin_LIBRARIES})