Axis conversion between OSVR and ROS
I'm using the vrpn_client_ros
package to connect an OSVR headset to ROS. However, the OSVR uses a different axis convention than ROS (much like camera optical frames).
In OSVR, X is right, Y is up, and Z is near.
In ROS, X is forward, Y is left, and Z is up.
This is leading to very strange behavior when I compare orientation between the two. Roll, pitch, and yaw on the OSVR seem to change depending on which direction it's facing, where pitch when facing north becomes yaw when facing west, and negative pitch when facing south. Moreover, it's very clear that rotation axes are very different between the headset IMU and camera odometry.
How can I convert the OSVR axes into ROS axes such that pose and twist from the OSVR headset IMU are equivalent to pose and twist from my camera odometry?
I have some code that swaps the axes to no avail. I've tried several combinations in desperation, but I always get the same rotation behavior.
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();
}
}
I don't know about your problem per se, but just wanted to mention that
pose.orientation
is a Quaternion, not an RPY vector. I don't think reshuffling the entries of a Quaternion results in what you are after.