orientation ground truth

[hosh0425]

I need to log ground truth of quadrotor (x,y,yaw). on topic /ground_truth_to_tf/pose robot pose are okey but to calculate robot yaw we need to use orientation. in orientation x and y are

x: -6.87593260918e-20
y: 1.25863102164e-19
approximatly zero and thus the yaw(calculate throw atan2(2*(q0q3+q1q2)),1-2*(q2q2+q3q3)) will be 0 or pi.
what's the solution?

[wilselby]

Hello,
Which launch file are you using? The topic "quad/ground_truth/pose" is of type geometry_msgs/PoseStamped. This message type provides position measurements x, y, and z. It also provides orientation as a quaternion using variables x, y, z, and w under orientation. You can convert the oreintation using the getRPY() function. See an example in the quad_controller.cpp file. A code snippet is provided below as well.

  //Convert quaternion to Euler angles
  tf:quaternionMsgToTF(current_gps.pose.pose.orientation, q);
  tf::Matrix3x3(q).getRPY(gps_roll, gps_pitch, gps_yaw);