ROS欧拉角和四元素互转

为了方便计算通常旋转会采用四元素来表示，如果采用欧拉角计算会出现万向节锁（Gimbal Lock）的问题，但不直观，某些使用场景下需要相互转换。因此记录ros中相互转换的方法

1,四元素转欧拉角(即posetopose2d）

void PoseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D& pose2D)
  {
    // use tf-pkg to convert angles
    tf2::Transform pose_tf;

    // convert geometry_msgs::PoseStamped to tf2::Transform
    tf2::convert(pose, pose_tf);

    // now get Euler-Angles from pose_tf
    double useless_pitch, useless_roll, yaw;
    pose_tf.getBasis().getEulerYPR(yaw, useless_pitch, useless_roll);

    // normalize angle
    yaw = angles::normalize_angle(yaw);

    // and set to pose2D
    pose2D.x = pose.position.x;
    pose2D.y = pose.position.y;
    pose2D.theta = yaw;

  }```
  ## 2，欧拉角转四元素（即pose2dtopose）
  

```cpp
 void Pose2DToPose(geometry_msgs::Pose& pose, const geometry_msgs::Pose2D pose2D)
  {
    // use tf-pkg to convert angles
    tf2::Quaternion frame_quat;

    // transform angle from euler-angle to quaternion representation
    frame_quat.setRPY(0.0, 0.0, pose2D.theta);

    // set position
    pose.position.x = pose2D.x;
    pose.position.y = pose2D.y;
    pose.position.z = 0.0;

    // set quaternion
    pose.orientation.x = frame_quat.x();
    pose.orientation.y = frame_quat.y();
    pose.orientation.z = frame_quat.z();
    pose.orientation.w = frame_quat.w();

  }