Add test for geopose heading #28091
Add test for geopose heading #28091
Conversation
Signed-off-by: Ryan Friedman <[email protected]>
Ryanf55
force-pushed
the
test-ros-geopose-heading
branch
from
b07a3c4
to
a24440b
Compare
| @@ -41,7 +47,7 @@ | |||
| CMAC_HEADING = 353 | |||
|
|
|||
|
|
|||
| def ros_quat_to_heading(quat): | |||
| def ros_quat_to_heading_deg(quat): | |||
| # By default, scipy follows scalar-last order – (x, y, z, w) | |||
There was a problem hiding this comment.
scipy is a large library to pull in just for quaternions and rotations. Have you thought about transforms3d (https://matthew-brett.github.io/transforms3d/)?
There was a problem hiding this comment.
Yea, I just saw scipy was already used a bunch of places, including in the setup script, and thought the team would prefer a library they are familiar with.
Your link looks like a much more focused library, but the apt version is out of date:
matthew-brett/transforms3d#65
| @@ -57,10 +63,28 @@ def validate_position_cmac(position): | |||
| and math.isclose(position.altitude, CMAC_ABS_ALT, abs_tol=1.0) | |||
| ) | |||
|
|
|||
| def wrap_360(angle): | |||
There was a problem hiding this comment.
Could use the same calc as AP_Math here?
float wrap_360(const float angle)
{
float res = fmodf(angle, 360.0f);
if (res < 0) {
res += 360.0f;
}
return res;
}The calc does not wrap 360 deg to 0, not sure if that's an issue (means the function is not invertible).
There was a problem hiding this comment.
I pulled this from pymavlink but added recursion so you can handle multi-wrap. If the initial heading was directly north, the comparison would get tricky, but this util is only used in this hard-coded cmac test.
|
|
||
| Per ROS REP-103, the quaternion should report 0 when the vehicle faces east, | ||
| and 90 degrees when facing north. | ||
| Because CMAC is NNW, we expect ~97 degees. |
There was a problem hiding this comment.
./Tools/autotest/locations.txt has this for CMAC. Heading is 353 => rotation = 97 as described.
CMAC=-35.363261,149.165230,584,353There was a problem hiding this comment.
Yea, I just had to add a tolerance because the data is through the EAHRS with noise added. Peter mentioned to set the EKF type to match the SIM so it's exact, but we don't depend on mavproxy or support params yet, so I left that out of scope and used a tolerance instead.
Purpose
Add a test to prove the reported heading on the geopose interface is REP-103 compliant.
This matches the proposed clarifications here, and can be used as an example in the REP.
ros-infrastructure/rep#407