Skip to content

Es/alp 123 core struct operator wrap around #29

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
eslaght wants to merge 5 commits into
base: dev
Choose a base branch
from
Open

Es/alp 123 core struct operator wrap around #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
01e29a4
Implemented wrap angle function and testing
eslaght Jul 25, 2024
50b706b
Implemented wrap angle function and testing
eslaght Jul 25, 2024
1e37e60
Merge remote-tracking branch 'origin/ES/ALP-123-core-struct-operator-…
eslaght Jul 29, 2024
28c1254
missing parenthesis
eslaght Jul 29, 2024
7345187
update
eslaght Jul 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 24 additions & 2 deletions include/core-structs.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

#include <optional>
#include <Eigen/Dense>
#include <math.h>

/** A point (position only) in the 2D plane. */
struct Point2D {
Expand All @@ -26,14 +27,15 @@ struct Pose2D {
float y; // y-coordinate (scaled in meters)
float theta_rad; // heading (radians) wrapped into [-pi, pi]


/**
* @brief overloaded copy assignment operator
*/
struct Pose2D& operator=(const struct Pose2D& other) {
if (this != &other) {
this->x = other.x;
this->y = other.y;
this->theta_rad = other.theta_rad;
this->theta_rad = wrapAngle(other.theta_rad);
}
return *this;
}
Expand All @@ -44,9 +46,29 @@ struct Pose2D {
struct Pose2D& operator+=(const Eigen::Vector3f& rhs) {
x += rhs[0];
y += rhs[1];
theta_rad += rhs[2];
theta_rad = wrapAngle(theta_rad + rhs[2]);
return *this;
}

/**
* @brief wraps angle in radians into [-pi, pi]
Copy link
Collaborator

@WillWu88 WillWu88 Aug 7, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

*/
static float wrapAngle( float angle_rad ){
Copy link
Collaborator

@WillWu88 WillWu88 Aug 7, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think this function needs to be static

while (angle_rad < -M_PI) {
Copy link
Collaborator

@WillWu88 WillWu88 Aug 7, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is introducing unnecessary loops. This function should be a very simple O(1) arithmetic function.

angle_rad += 2.0 * M_PI;
}
while (angle_rad > M_PI) {
angle_rad -= 2.0 * M_PI;
}

if (std::fabs(angle_rad - M_PI) < 1e-4) {
Copy link
Collaborator

@WillWu88 WillWu88 Aug 7, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

angle_rad = M_PI;
} else if (std::fabs(angle_rad + M_PI) < 1e-4f) {
angle_rad = -M_PI;
}

return angle_rad;
}
};

/**
Expand Down
88 changes: 88 additions & 0 deletions src/FastSLAM/math-util_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,3 +171,91 @@ TEST_CASE( "Test CDF table generation" ){
REQUIRE( cdf.empty() );
}
}

TEST_CASE( "Test wrap around angle" ){

struct Pose2D pose = {1, 0, 0};

SECTION( "positive overflow with sum operator" ){
Eigen::Vector3f del(0.0f, 0.0f, 3.0f * M_PI_2);
pose += del;
REQUIRE_THAT(pose.theta_rad, Catch::Matchers::WithinRel((-M_PI_2), 0.001));
}

SECTION( "negative overflow with sum operator" ){
Eigen::Vector3f del(0.0f, 0.0f, -4.0f * M_PI_2);
pose += del;
REQUIRE_THAT(pose.theta_rad, Catch::Matchers::WithinAbs(-0.0, 0.001));
}

SECTION( "theta is slightly over pi with sum operator" ){
Eigen::Vector3f del(0.0f, 0.0f, M_PI + 0.1);
pose += del;
REQUIRE_THAT(pose.theta_rad, Catch::Matchers::WithinRel(-M_PI + 0.1, 0.001));
}

SECTION( "theta is slightly under pi with sum operator" ){
Eigen::Vector3f del(0.0f, 0.0f, M_PI - 0.1);
pose += del;
REQUIRE_THAT(pose.theta_rad, Catch::Matchers::WithinRel(M_PI - 0.1, 0.001));
}

SECTION( "sum theta is -pi sum operator" ){
struct Pose2D add = {0.0f, 0.0f, -M_PI_2};
Eigen::Vector3f delta(0.0f, 0.0f, -M_PI_2);
add += delta;
REQUIRE_THAT(add.theta_rad, Catch::Matchers::WithinRel(M_PI, 0.001));
}

SECTION( "sum theta is pi sum operator" ){
struct Pose2D add = {0.0f, 0.0f, M_PI_2};
Eigen::Vector3f delta(0.0f, 0.0f, M_PI_2);
add += delta;
REQUIRE_THAT(add.theta_rad, Catch::Matchers::WithinRel(-M_PI, 0.001));
}

SECTION( "positive overflow with copy assignment operator" ){
pose.theta_rad = 3.0 * M_PI;
struct Pose2D copied = {0, 0, 0};
copied = pose;
REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel((-M_PI), 0.001));
}

SECTION( "negative overflow with copy assignment operator" ){
pose.theta_rad = -5.0f * M_PI_2;
struct Pose2D copied;
copied = pose;
REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(-M_PI_2, 0.001));
}

SECTION( "theta is slightly over pi with sum operator" ){
pose.theta_rad = M_PI + 0.01;
struct Pose2D copied;
copied = pose;
REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(-M_PI + 0.01, 0.001));
}

SECTION( "theta is slightly under -pi with sum operator" ){
pose.theta_rad = -M_PI - 0.001;
struct Pose2D copied;
copied = pose;
REQUIRE_THAT(copied.theta_rad, Catch::Matchers::WithinRel(M_PI - 0.001, 0.001));
}

SECTION( "copy asssignment with theta is pi" ){
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));
Copy link
Collaborator

@WillWu88 WillWu88 Aug 7, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

}

SECTION( "copy asssignment with theta is -pi" ){
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));
}

}