Skip to content

Commit

Permalink
Added test for new functions
Browse files Browse the repository at this point in the history 
Signed-off-by: Ayush1285 <[email protected]>
  • Loading branch information
@Ayush1285
Ayush1285 committed Oct 20, 2024
1 parent 55f7fae commit 902a72d
Show file tree
Hide file tree
Showing 3 changed files with 80 additions and 27 deletions.
40 changes: 27 additions & 13 deletions nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -265,12 +265,10 @@ inline bool withinPositionGoalTolerance(
template<typename T>
auto normalize_angles(const T & angles)
{
return (angles + M_PIF).unaryExpr([&](const float x){
return (angles + M_PIF).unaryExpr([&](const float x) {
float remainder = std::fmod(x, 2.0f * M_PIF);
return remainder < 0.0f ? remainder + M_PIF : remainder - M_PIF;
});
// auto theta = angles - (M_PIF * ((angles + M_PIF_2) * (1.0f / M_PIF)).floor());
// return theta;
}

/**
Expand Down Expand Up @@ -639,29 +637,45 @@ inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
}
}

inline auto point_corrected_yaws(
const Eigen::ArrayXf & yaws, const Eigen::ArrayXf & yaws_between_points)
/**
* @brief Normalize the yaws between points on the basis of final yaw angle
* of the trajectory.
* @param last_yaws Final yaw angles of the trajectories.
* @param yaw_between_points Yaw angles calculated between x and y co-ordinates of the trajectories.
* @return Normalized yaw between points.
*/
inline auto normalize_yaws_between_points(
const Eigen::Ref<const Eigen::ArrayXf> & last_yaws,
const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
{
Eigen::ArrayXf yaws = utils::shortest_angular_distance(
last_yaws, yaw_between_points).abs();
int size = yaws.size();
Eigen::ArrayXf yaws_between_points_corrected(size);
for(int i = 0; i != size; i++) {
const float & yaw_between_points = yaws_between_points[i];
const float & yaw_between_point = yaw_between_points[i];
yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ?
yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF);
yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
}
return yaws_between_points_corrected;
}

inline auto point_corrected_yaws(
const Eigen::ArrayXf & yaws_between_points, const float & goal_yaw)
/**
* @brief Normalize the yaws between points on the basis of goal angle.
* @param goal_yaw Goal yaw angle.
* @param yaw_between_points Yaw angles calculated between x and y co-ordinates of the trajectories.
* @return Normalized yaw between points
*/
inline auto normalize_yaws_between_points(
const float & goal_yaw, const Eigen::Ref<const Eigen::ArrayXf> & yaw_between_points)
{
int size = yaws_between_points.size();
int size = yaw_between_points.size();
Eigen::ArrayXf yaws_between_points_corrected(size);
for(int i = 0; i != size; i++) {
const float & yaw_between_points = yaws_between_points[i];
const float & yaw_between_point = yaw_between_points[i];
yaws_between_points_corrected[i] = fabs(
angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ?
yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF);
angles::normalize_angle(yaw_between_point - goal_yaw)) < M_PIF_2 ?
yaw_between_point : angles::normalize_angle(yaw_between_point + M_PIF);
}
return yaws_between_points_corrected;
}
Expand Down
27 changes: 13 additions & 14 deletions nav2_mppi_controller/src/critics/path_angle_critic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,18 +97,18 @@ void PathAngleCritic::score(CriticData & data)
throw nav2_core::ControllerException("Invalid path angle mode!");
}

int rightmost_idx = data.trajectories.y.cols() - 1;
auto diff_y = goal_y - data.trajectories.y.col(rightmost_idx);
auto diff_x = goal_x - data.trajectories.x.col(rightmost_idx);
int last_idx = data.trajectories.y.cols() - 1;
auto diff_y = goal_y - data.trajectories.y.col(last_idx);
auto diff_x = goal_x - data.trajectories.x.col(last_idx);
auto yaws_between_points = diff_y.binaryExpr(
diff_x, [&](const float & y, const float & x){return atan2f(y, x);}).eval();

switch (mode_) {
case PathAngleMode::FORWARD_PREFERENCE:
{
auto rightmost_yaw = data.trajectories.yaws.col(rightmost_idx);
auto last_yaws = data.trajectories.yaws.col(last_idx);
auto yaws = utils::shortest_angular_distance(
rightmost_yaw, yaws_between_points).abs();
last_yaws, yaws_between_points).abs();
if (power_ > 1u) {
data.costs += (yaws * weight_).pow(power_);

Check warning on line 113 in nav2_mppi_controller/src/critics/path_angle_critic.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_mppi_controller/src/critics/path_angle_critic.cpp#L113 

Added line #L113 was not covered by tests
} else {
Expand All @@ -118,12 +118,11 @@ void PathAngleCritic::score(CriticData & data)
}
case PathAngleMode::NO_DIRECTIONAL_PREFERENCE:
{
auto rightmost_yaw = data.trajectories.yaws.col(rightmost_idx);
auto yaws = utils::shortest_angular_distance(
rightmost_yaw, yaws_between_points).abs();
auto yaws_between_points_corrected = utils::point_corrected_yaws(yaws, yaws_between_points);
auto last_yaws = data.trajectories.yaws.col(last_idx);
auto yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
yaws_between_points);
auto corrected_yaws = utils::shortest_angular_distance(
rightmost_yaw, yaws_between_points_corrected).abs();
last_yaws, yaws_between_points_corrected).abs();
if (power_ > 1u) {
data.costs += (corrected_yaws * weight_).pow(power_);

Check warning on line 127 in nav2_mppi_controller/src/critics/path_angle_critic.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_mppi_controller/src/critics/path_angle_critic.cpp#L127 

Added line #L127 was not covered by tests
} else {
Expand All @@ -133,11 +132,11 @@ void PathAngleCritic::score(CriticData & data)
}
case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS:
{
auto rightmost_yaw = data.trajectories.yaws.col(rightmost_idx);
auto yaws_between_points_corrected =
utils::point_corrected_yaws(yaws_between_points, goal_yaw);
auto last_yaws = data.trajectories.yaws.col(last_idx);
auto yaws_between_points_corrected = utils::normalize_yaws_between_points(goal_yaw,
yaws_between_points);
auto corrected_yaws = utils::shortest_angular_distance(
rightmost_yaw, yaws_between_points_corrected).abs();
last_yaws, yaws_between_points_corrected).abs();
if (power_ > 1u) {
data.costs += (corrected_yaws * weight_).pow(power_);

Check warning on line 141 in nav2_mppi_controller/src/critics/path_angle_critic.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_mppi_controller/src/critics/path_angle_critic.cpp#L141 

Added line #L141 was not covered by tests
} else {
Expand Down
40 changes: 40 additions & 0 deletions nav2_mppi_controller/test/utils_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -512,3 +512,43 @@ TEST(UtilsTests, ShiftColumnsByOnePlaceTest)
EXPECT_EQ(array_2d(1, 2), 7);
EXPECT_EQ(array_2d(2, 2), 11);
}

TEST(UtilsTests, NormalizeYawsBetweenPointsTest)
{
Eigen::ArrayXf last_yaws(10);
last_yaws.setZero();

Eigen::ArrayXf yaw_between_points(10);
yaw_between_points.setZero(10);

// Try with both angles 0
Eigen::ArrayXf yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
yaw_between_points);
EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));

// Try with yaw between points as pi/4
yaw_between_points.setConstant(M_PIF_2 / 2);
yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
yaw_between_points);
EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));

// Try with yaw between points as pi/2
yaw_between_points.setConstant(M_PIF_2);
yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
yaw_between_points);
EXPECT_TRUE(yaws_between_points_corrected.isApprox(yaw_between_points));

// Try with a few yaw betweem points more than pi/2
yaw_between_points[1] = 1.2 * M_PIF_2;
yaws_between_points_corrected = utils::normalize_yaws_between_points(last_yaws,
yaw_between_points);
EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3);
EXPECT_NEAR(yaws_between_points_corrected[0], yaw_between_points[0], 1e-3);
EXPECT_NEAR(yaws_between_points_corrected[9], yaw_between_points[9], 1e-3);

// Try with goal angle 0
float goal_angle = 0;
yaws_between_points_corrected = utils::normalize_yaws_between_points(goal_angle,
yaw_between_points);
EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3);
}

0 comments on commit 902a72d

Please sign in to comment.