7.ROS的TF坐标变换（一）：TF简介及静态坐标变换代码讲解
目录
1 什么是ROS的TF坐标变换TransForm Frame
2 坐标变换的msg信息geometry_msgs/TransformStamped与geometry_msgs/PointStamped
3 静态坐标变换
3.1 C++ clion实现静态坐标变换
3.1.1 CMakeLists.txt配置
3.1.2 package.xml配置
3.1.3 发布节点建立
3.1.4 接收节点建立
3.1.5 运行结果
3.1.6 补充 （简单做法）
1 什么是ROS的TF坐标变换TransForm Frame
在机器人系统中，配备了多种传感器，例如激光雷达和摄像头等。这些传感器能够感知机器人周围的物体位置，包括坐标、横向、纵向以及高度的距离信息。它们的作用是协助机器人准确定位障碍物。然而，并非所有传感器都能直接提供物体相对于机器人系统或其他组件的方位信息。虽然可以获取物体相对特定传感器的方位信息，但这并不等同于物体相对于整个机器人系统或其他组件的方位信息。在信息显示方面存在限制，因为这需要经历一定的转换过程。更详细地描述如下：
场景1：雷达与小车现有一移动式机器人底盘，在底盘上安装了一雷达，雷达相对于底盘的偏移量已知，现雷达检测到一障碍物信息，获取到坐标分别为(x,y,z)，该坐标是以雷达为参考系的，如何将这个坐标转换成以小车为参考系的坐标呢？
场景2:现有一带机械臂的机器人(比如:PR2)需要夹取目标物，当前机器人头部摄像头可以探测到目标物的坐标(x,y,z)，不过该坐标是以摄像头为参考系的，而实际操作目标物的是机械臂的夹具，当前我们需要将该坐标转换成相对于机械臂夹具的坐标，这个过程如何实现？
根据我们高中学习的知识，在明确了不同坐标系之间的的相对关系，就可以实现任何坐标点在不同坐标系之间的转换，但是该计算实现是较为常用的，且算法也有点复杂，因此在 ROS 中直接封装了相关的模块: 坐标变换(TF)。
在ROS中通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。
在 ROS 中用于实现不同坐标系之间的点或向量的转换。
在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
tf2: 封装了坐标变换的常用消息。
tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。
2 坐标变换的msg信息geometry_msgs/TransformStamped与geometry_msgs/PointStamped
在坐标变幻中常用的msg是
geometry_msgs/TransformStamped和geometry_msgs/PointStamped。我们可以打开终端用rosmsg info查看：这里的frame_id是被参考的坐标系，child_frame_id是另外的坐标系，transform是child_frame_id相对于frame_id的偏移量。
这里的frame_id是指我的坐标点是以哪个坐标系为参考的。xyz就是坐标点的值
前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。
3 静态坐标变换
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。
比如我们手持扫描设备中的相机和雷达之间就是不会变换的。
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
实现分析:
- 坐标系相对关系，可以通过发布方发布
- 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出
实现流程:
- 新建功能包，添加依赖
- 编写发布方实现
- 编写订阅方实现
- 执行并查看结果
这里我们用clion实现。
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配置
这些都是最简单的，如果不会配置建议回炉重造：
test 0.0.0 A test haha HITLHW BSD-3 catkin roscpp roscpp rospy rospy pcl_conversions pcl_conversions std_msgs std_msgs sensor_msgs sensor_msgs geometry_msgs geometry_msgs tf2 tf2 tf2_ros tf2_ros tf2_geometry_msgs tf2_geometry_msgs
3.1.3 发布节点建立
建立发布节点：
OK，CMake已经帮我们配置好了。
报错了把！！哈哈，CMake配置错了对把！
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 坐标系的位置信息 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建静态坐标转换广播器 4.创建坐标系信息 5.广播器发布坐标系信息 6.spin()// // 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; }也就是说我们在这里
首先：创建TF订阅节点，将订阅的信息缓存到buffer中
tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer);第二：生成一个坐标点，需要指定他在哪个坐标系下。
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;在雷达坐标系（laser）下的坐标为（1，2，7.3）
第三：获得这个点在base_link下的坐标并打印：
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());OK，我们已经实现了。
下面是需要注意的问题。
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})
3.1.6 补充 （简单做法）
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、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也建议使用该种方式直接实现静态坐标系相对信息发布。
我们建立摄像头相对于底盘的参数：
我们查看一下：
