无人车采用纯跟踪算法跟踪离线路径（ROS，C++）坐标系转换代码

最新推荐文章于 2024-07-25 11:27:31 发布

m0_46699649

最新推荐文章于 2024-07-25 11:27:31 发布

阅读量739

点赞数 29

文章标签：算法 c++ 开发语言自动驾驶

本文链接：https://blog.csdn.net/m0_46699649/article/details/139284042

版权

一：序言

坐标系准换在本次项目中是非常重要的，本篇文章结合代码对如果调用ROS中坐标准换接口进行详细讲述，参考链接如下：
ROS坐标转换工具
项目学习链接如下：
无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分

二：代码讲解

实现代码如下

tf2::Transform transform;
    transform.setOrigin(tf2::Vector3( 0,0,0));
    tf2::Quaternion q;
    q.setRPY(0, 0,  (standardAzimuth - 90) * (PII / 180));
    transform.setRotation(q);
    tf2::Vector3 state(enu_xyz[0],enu_xyz[1],0);
    state = transform * state;

这个代码完成了坐标系准换的功能
详细代码讲解如下：

 tf2::Transform transform;

tf2::Transform 是 ROS（机器人操作系统）中的一个类，用于表示二维或三维空间中的坐标变换。它基于刚体变换理论，可以进行平移和旋转操作。tf2::Transform transform; 创建了一个名为 transform 的 tf2::Transform 对象。这个对象用于表示一个坐标变换，包括平移和旋转。tf2::Transform 类是通过组合 tf2::Vector3 和 tf2::Quaternion 类实现的。tf2::Vector3 用于表示三维空间中的位置，而 tf2::Quaternion 用于表示旋转。transform 对象默认构造函数会将平移部分设为零向量 (0, 0, 0)，表示没有平移。旋转部分默认构造函数会将旋转设为单位四元数，表示没有旋转。transform 对象可以通过方法调用进行设置和获取平移和旋转的值。

transform.setOrigin(tf2::Vector3( 0,0,0));

transform.setOrigin(tf2::Vector3(0, 0, 0)); 这行代码用于设置 tf2::Transform 对象的平移部分，即设置坐标变换的起始点。下面对这行代码进行详细解释：tf2::Vector3 是 ROS 中的一个类，用于表示三维空间中的位置或向量。它接受三个参数，分别代表 X、Y 和 Z 轴的坐标值。tf2::Vector3(0, 0, 0) 创建了一个名为 tf2::Vector3 的对象，并将其初始化为 (0, 0, 0)，表示坐标变换的起始点的位置。transform.setOrigin() 是 tf2::Transform 类的一个方法，用于设置坐标变换对象的平移部分。它接受一个 tf2::Vector3 类型的参数，表示平移的向量。
transform.setOrigin(tf2::Vector3(0, 0, 0)) 这行代码将 tf2::Transform 对象的平移部分设置为原点 (0, 0, 0)，表示坐标变换将以原点为起始点进行平移操作。就是平移了（0,0,0）个单位.

tf2::Quaternion q;

tf2::Quaternion q; 这行代码创建了一个名为 q 的 tf2::Quaternion 对象。tf2::Quaternion 是 ROS 中的一个类，用于表示三维空间中的旋转。它是基于四元数表示的，可以描述旋转的轴和角度。tf2::Quaternion q; 声明并创建了一个名为 q 的 tf2::Quaternion 对象。此时，q 的值将使用默认构造函数进行初始化。
默认构造函数会将 q 的四元数值设置为 (0, 0, 0, 1)，表示没有旋转。这是一个单位四元数，它不会对任何点或向量产生旋转效果。tf2::Quaternion 类提供了一系列方法，可以用于设置和获取旋转的值，进行旋转操作，以及与其他四元数进行运算（如乘法、插值等）。

 q.setRPY(0, 0,  (standardAzimuth - 90) * (PII / 180));

-这行代码用于设置 tf2::Quaternion 对象的旋转部分，即根据给定的欧拉角设置旋转。q 是 tf2::Quaternion 对象，应该在代码的其他部分进行声明和实例化。setRPY() 是 tf2::Quaternion 类的一个方法，用于设置旋转部分。它接受三个参数，分别对应绕 X、Y 和 Z 轴的旋转角度。(standardAzimuth - 90) * (PII / 180) 是计算旋转角度的表达式。standardAzimuth 是一个变量，表示标准方位角（azimuth）的值。这个表达式将标准方位角减去 90 度，然后乘以 π/180，将角度转换为弧度。
setRPY(0, 0, (standardAzimuth - 90) * (PII / 180)) 将旋转角度设置为 (standardAzimuth - 90) * (PII / 180)，并应用于 q 对象的旋转部分。这样，q 的旋转部分将根据给定的欧拉角进行旋转。

transform.setRotation(q);

transform.setRotation(q); 这行代码用于将 tf2::Quaternion 对象 q 的旋转部分设置为一个 tf2::Transform 对象 transform 的旋转。
transform 是一个 tf2::Transform 对象，它包含了坐标变换的信息，包括平移和旋转。q 是一个tf2::Quaternion 对象，它表示旋转部分。通常，你可以通过欧拉角或其他方式创建和设置 q 的旋转部分。
setRotation(q) 是 tf2::Transform 类的一个方法，用于设置旋转部分。它接受一个 tf2::Quaternion 对象作为参数，并将该对象的旋转部分应用于 transform 对象的旋转部分。通过调用 transform.setRotation(q)，你可以将 q 的旋转部分设置为 transform 对象的旋转部分，从而实现坐标变换中的旋转操作。

tf2::Vector3 state(enu_xyz[0],enu_xyz[1],0);

tf2::Vector3 state(enu_xyz[0], enu_xyz[1], 0); 这行代码创建了一个名为 state 的 tf2::Vector3 对象，并将其初始化为 (enu_xyz[0], enu_xyz[1], 0)。
tf2::Vector3 是 ROS 中的一个类，用于表示三维空间中的向量。state 是一个名为 state 的 tf2::Vector3 对象，应该在代码的其他部分进行声明和实例化。(enu_xyz[0], enu_xyz[1], 0) 是一个初始化向量的表达式。enu_xyz 是一个数组或容器，其中包含了三个元素，对应于东北天坐标系 (ENU) 中的 X、Y 和 Z 坐标。

   state = transform * state;

state = transform * state; 这行代码将对 tf2::Transform 对象 transform 和 tf2::Vector3 对象 state 进行坐标变换操作，并将结果赋值给 state。
transform 是一个 tf2::Transform 对象，包含了坐标变换的信息，包括平移和旋转。state 是一个 tf2::Vector3 对象，表示三维空间中的向量。它可能包含了位置或方向等信息。
运算符用于执行坐标变换操作。在这里，transform * state 表示将 state 向量应用于 transform 坐标变换，得到一个新的向量。
state = transform * state 将变换后的向量赋值给 state 对象，覆盖原始的 state 值。通过执行 transform * state 这样的坐标变换操作，可以将一个向量从一个坐标系转换到另一个坐标系，考虑了平移和旋转的影响。