Преобразование между осевыми соглашениями — ROS и OSVR

Короче говоря, я интегрирую две системы, которые имеют разные соглашения по осям.

В OSVR x — это право, y — вверх, а z — близко.

В ROS x вперед, y слева, z вверх.

Мне нужно конвертировать между ними. Без этого в ротации происходит странное поведение. Например, реальная высота отображается как высота в ROS, если смотреть на север, но если смотреть на запад, реальная высота становится отклонением в ROS.

Я написал некоторый код C ++ для выполнения преобразования, но оба они не совпадают.

namespace osvr_axis_conversion {

void osvrPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg) { // osvr to ros
geometry_msgs::PoseStamped outputMsg;

outputMsg.header = msg->header;
outputMsg.pose.orientation.x = msg->pose.orientation.y;
outputMsg.pose.orientation.y = msg->pose.orientation.z;
outputMsg.pose.orientation.z = msg->pose.orientation.x;
outputMsg.pose.orientation.w = msg->pose.orientation.w;

osvrPosePub.publish(outputMsg);
ros::spinOnce();
}

void osvrTwistCallback(const geometry_msgs::TwistStampedConstPtr &msg) { // osvr to ros
geometry_msgs::TwistStamped outputMsg;

//outputMsg = *msg;
//outputMsg.twist = msg->twist;
outputMsg.header = msg->header;
outputMsg.twist.angular.x = msg->twist.angular.y;
outputMsg.twist.angular.y = msg->twist.angular.z;
outputMsg.twist.angular.z = msg->twist.angular.x;
osvrTwistPub.publish(outputMsg);
ros::spinOnce();
}

void odometryCallback(const nav_msgs::OdometryConstPtr &msg) { // ros to osvr
// Do the reverse of the above!
nav_msgs::Odometry outputMsg;
outputMsg = *msg;

outputMsg.pose.pose.position.x = msg->pose.pose.position.z;
outputMsg.pose.pose.position.y = msg->pose.pose.position.x;
outputMsg.pose.pose.position.z = msg->pose.pose.position.y;

outputMsg.pose.pose.orientation.x = msg->pose.pose.orientation.z;
outputMsg.pose.pose.orientation.y = msg->pose.pose.orientation.x;
outputMsg.pose.pose.orientation.z = msg->pose.pose.orientation.y;
outputMsg.pose.pose.orientation.w = msg->pose.pose.orientation.w;

outputMsg.twist.twist.linear.x = msg->twist.twist.linear.z;
outputMsg.twist.twist.linear.y = msg->twist.twist.linear.x;
outputMsg.twist.twist.linear.z = msg->twist.twist.linear.y;

outputMsg.twist.twist.angular.x = msg->twist.twist.angular.z;
outputMsg.twist.twist.angular.y = msg->twist.twist.angular.x;
outputMsg.twist.twist.angular.z = msg->twist.twist.angular.y;

// IMPORTANT: We're not handling the covariance here, so it's wrong
//   in the output! This is being sent to the OSVR driver which doesn't
//   use the covariance data.

odometryPub.publish(outputMsg);
ros::spinOnce();
}

}

Как я могу выполнить преобразование между двумя соглашениями по осям так, чтобы поза была одинаковой между датчиком на гарнитуре OSVR и датчиком, прикрепленным к нему в ROS?