ROS 开发 通过欧拉角计算四元素 C++实现
方法一:
传入 roll pitch yaw 欧拉角,数学计算得出, 返回x y z w四元素的值
void GetXYZWFromRPY(double roll, double pitch, double yaw, double& x, double& y, double& z, double& w)
{
roll /= 2;
pitch /= 2;
yaw /= 2;
x = sin(roll) * cos(pitch) * cos(yaw) - cos(roll) * sin(pitch) * sin(yaw);
y = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * cos(pitch) * sin(yaw);
z = cos(roll) * cos(pitch) * sin(yaw) - sin(roll) * sin(pitch) * cos(yaw);
w = cos(roll) * cos(pitch) * cos(yaw) + sin(roll) * sin(pitch) * sin(yaw);
}
方法二:调用ROS tf2类实现
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
x = q.x();
y = q.y();
z = q.z();
w = q.w();