C++ 轴约定之间的转换-ROS和OSVR
长话短说,我正在集成两个具有不同轴约定的系统 在OSVR中,x是正确的,y是向上的,z是接近的 在ROS中,x向前,y向左,z向上 我需要在他们之间转换。如果不这样做,旋转中会出现奇怪的行为。例如,当面向北方时,真实俯仰在ROS中显示为俯仰,但当面向西方时,真实俯仰在ROS中变为偏航C++ 轴约定之间的转换-ROS和OSVR,c++,transform,axis,ros,osvr,C++,Transform,Axis,Ros,Osvr,长话短说,我正在集成两个具有不同轴约定的系统 在OSVR中,x是正确的,y是向上的,z是接近的 在ROS中,x向前,y向左,z向上 我需要在他们之间转换。如果不这样做,旋转中会出现奇怪的行为。例如,当面向北方时,真实俯仰在ROS中显示为俯仰,但当面向西方时,真实俯仰在ROS中变为偏航 我编写了一些C++代码来执行转换,但这两个代码仍然不匹配。 namespace osvr_axis_conversion { void osvrPoseCallback(const geometry_msgs::
我编写了一些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中附加在其上的传感器之间的姿势相同?这个问题可能会更好地发布在:您可以制作旋转器(或矩阵)并应用它。在你的例子中,你需要两次旋转——ROS到OSVR需要绕Y旋转90度,X旋转90度,OSVR到ROS需要反向旋转。之后,只需申请位置和旋转。