0

长话短说,我正在集成两个具有不同轴约定的系统。

在 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 中固定在其上的传感器之间的姿势相同?

4

0 回答 0