Conversion between axis conventions- ROS and OSVR

296 Views Asked by At

Long story short, I'm integrating two systems that have different axis conventions.

In OSVR, x is right, y is up, and z is near.

In ROS, x is forward, y is left, an z is up.

I need to convert between them. Without doing so, there is strange behavior in rotation. For example, real pitch shows as pitch in ROS when facing north, but when facing west, real pitch becomes yaw in ROS.

I wrote some C++ code to perform the conversion, but the two still don't match up.

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

}

How can I convert between the two axis conventions such that pose is the same between a sensor on the OSVR headset and a sensor affixed to it in ROS?

0

There are 0 best solutions below