Es/alp 123 core struct operator wrap around #29
Conversation
…wrap-around' into ES/ALP-123-core-struct-operator-wrap-around
| } | ||
|
|
||
| /** | ||
| * @brief wraps angle in radians into [-pi, pi] |
There was a problem hiding this comment.
I just realized we didn't put in the param doc strings in this file. Please make a low-priority ticket that adds the missing comments to these functions.
| /** | ||
| * @brief wraps angle in radians into [-pi, pi] | ||
| */ | ||
| static float wrapAngle( float angle_rad ){ |
There was a problem hiding this comment.
I don't think this function needs to be static
| angle_rad -= 2.0 * M_PI; | ||
| } | ||
|
|
||
| if (std::fabs(angle_rad - M_PI) < 1e-4) { |
There was a problem hiding this comment.
Why are we wrapping small values around pi and negative pi? This is dangerous. Robot's pose can be potentially switching between -pi and pi.
See this post on stackoverflow for a better way to handle angle wrapping. Also check out places in the code base where I implemented angle wrapping.
| * @brief wraps angle in radians into [-pi, pi] | ||
| */ | ||
| static float wrapAngle( float angle_rad ){ | ||
| while (angle_rad < -M_PI) { |
There was a problem hiding this comment.
This is introducing unnecessary loops. This function should be a very simple O(1) arithmetic function.
| pose.theta_rad = M_PI; | ||
| struct Pose2D copied = {0, 0, 0.0}; | ||
| copied = pose; | ||
| REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(-M_PI, 0.001)); |
There was a problem hiding this comment.
This and the unit test below don't make a lot of sense. The test expects the function to switch sign while in fact it should be the same? Unittests should be straight to the point and logical, because they ensure the quality of the code base. It is very important to make sure your tests make logical sense. Tests should be there to correct code or provide some indication of the correctness instead of matching the result of the code blindly.
-added wrapAngle function to wrap angle within [-pi, pi]
-added unit tests for wrapAngle