Webots ROS 默认控制器 "RosInertialUnit.cpp" 中的四元数计算

Question

今天我仔细研究了 "RosInertialUnit.cpp" 文件中作为默认 ROS 控制器一部分的四元数计算。

我想尝试使用 "keyboard_teleop.wbt" - 世界的 InterialUnit 并将传感器添加到 Pioneer 机器人。

然后我将场景树中给出的机器人旋转值（轴 + 角度格式）与 ROS 中传感器的输出（方向转换为四元数）进行比较。您可以在下面的屏幕截图中看到两者：

在我看来，四元数输出与场景树中给定的值不匹配。使用 MATLAB 的函数 "quat = axang2quat(axang)" 时，我会为上面的示例获得以下内容：

quat = 0.7936    0.0131   -0.6082    0.0104  % w x y z

与ROS消息对比，y和z调换了。我不太确定这是否是故意的（也许是不同的约定？）。我不想立即开始拉取请求，但想在此之前讨论这个问题。

我正在 "RosInertialUnit.cpp" 的更改版本中测试以下实现，这给了我预期的结果（与在 MATLAB 中计算的结果相同）。

  double halfRoll = mInertialUnit->getRollPitchYaw()[0] * 0.5;   // turning around x
  double halfPitch = mInertialUnit->getRollPitchYaw()[2] * 0.5;  // turning around y
  double halfYaw = mInertialUnit->getRollPitchYaw()[1] * 0.5;    // turning around z
  double cosYaw = cos(halfYaw);
  double sinYaw = sin(halfYaw);
  double cosPitch = cos(halfPitch);
  double sinPitch = sin(halfPitch);
  double cosRoll = cos(halfRoll);
  double sinRoll = sin(halfRoll);

  value.orientation.x = cosYaw * cosPitch * sinRoll - sinYaw * sinPitch * cosRoll;
  value.orientation.y = sinYaw * cosPitch * sinRoll + cosYaw * sinPitch * cosRoll;
  value.orientation.z = sinYaw * cosPitch * cosRoll - cosYaw * sinPitch * sinRoll;
  value.orientation.w = cosYaw * cosPitch * cosRoll + sinYaw * sinPitch * sinRoll;

这与 this wikipedia article 中使用的实现相同。