ROS 开发 roll pitch yaw欧拉角计算四元素 x y z w C++实现

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