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

引子

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

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 yfloat64 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: 100stamp: secs: 1723624279nsecs: 917443332frame_id: "base_link"child_frame_id: "laser"transform: translation: x: 0.2y: 0.0z: 0.5rotation: x: 0.0y: 0.0z: 0.0w: 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