Axis conversion between OSVR and ROS

asked 2017-04-27 22:16:54 -0500

natejgardner gravatar image

updated 2017-04-27 22:40:48 -0500

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();
}

}
edit retag flag offensive close merge delete

Comments

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.

gvdhoorn gravatar imagegvdhoorn ( 2017-04-28 03:31:38 -0500 )edit