сегодня я более внимательно посмотрел на вычисление кватерниона, используемое в файле "RosInertialUnit.cpp" как части контроллера ROS по умолчанию.
Я хотел опробовать InterialUnit, используя "keyboard_teleop.wbt""- world и добавил датчик к роботу 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;
Это та же реализация, что и в этой статье в википедии .