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?
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?