From 847f8375f65b992b820bdef885abf15981152ae2 Mon Sep 17 00:00:00 2001 From: Ayush1285 Date: Wed, 2 Oct 2024 20:08:33 +0530 Subject: [PATCH] Fixed all the unit tests and critic tests, all unit tests passing locally Signed-off-by: Ayush1285 --- .../nav2_mppi_controller/tools/utils.hpp | 29 ++++--- .../src/critics/constraint_critic.cpp | 19 ++-- .../src/critics/goal_angle_critic.cpp | 6 +- .../src/critics/path_align_critic.cpp | 7 +- .../src/critics/path_angle_critic.cpp | 15 ++-- .../src/critics/twirling_critic.cpp | 4 +- .../src/critics/velocity_deadband_critic.cpp | 12 +-- .../src/trajectory_visualizer.cpp | 4 +- nav2_mppi_controller/test/critics_tests.cpp | 86 +++++++++---------- .../test/noise_generator_test.cpp | 30 +++---- nav2_mppi_controller/test/utils_test.cpp | 10 ++- 11 files changed, 116 insertions(+), 106 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 8f07837c2cc..26afa8b7c1b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -265,13 +265,12 @@ inline bool withinPositionGoalTolerance( template auto normalize_angles(const T & angles) { - // Eigen::ArrayXXf theta = (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; - // }); - // return ((theta < 0.0f).select(theta + M_PIF, theta - M_PIF)).eval(); - auto theta = angles - (M_PIF * ((angles + M_PIF_2) * (1.0f / M_PIF)).floor()); - return theta; + 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; } /** @@ -303,11 +302,12 @@ auto shortest_angular_distance( */ inline size_t findPathFurthestReachedPoint(const CriticData & data) { - const auto traj_x = data.trajectories.x.col(-1); - const auto traj_y = data.trajectories.y.col(-1); + int traj_cols = data.trajectories.x.cols(); + const auto traj_x = data.trajectories.x.col(traj_cols - 1); + const auto traj_y = data.trajectories.y.col(traj_cols - 1); - const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1) - traj_x; - const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1) - traj_y; + const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x; + const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y; const auto dists = dx * dx + dy * dy; @@ -453,8 +453,9 @@ inline void savitskyGolayFilter( const models::OptimizerSettings & settings) { // Savitzky-Golay Quadratic, 9-point Coefficients - Eigen::Array filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0}; - filter /= 231.0; + Eigen::Array filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, + -21.0f}; + filter /= 231.0f; const unsigned int num_sequences = control_sequence.vx.size() - 1; @@ -464,7 +465,7 @@ inline void savitskyGolayFilter( } auto applyFilter = [&](const Eigen::Array & data) -> float { - return (data * filter).sum(); + return (data * filter).eval().sum(); }; auto applyFilterOverAxis = diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 0d2f61c78f5..54ea40f2c56 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -49,10 +49,10 @@ void ConstraintCritic::score(CriticData & data) if (diff != nullptr) { if (power_ > 1u) { data.costs += (((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx). - max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).pow(power_); + max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).pow(power_).eval(); } else { - data.costs += ((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx). - max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_; + data.costs += (((((data.state.vx - max_vel_).max(0.0f) + (min_vel_ - data.state.vx). + max(0.0f)) * data.model_dt).rowwise().sum().eval()) * weight_).eval(); } return; } @@ -69,10 +69,10 @@ void ConstraintCritic::score(CriticData & data) auto vel_total = (data.state.vx.square() + data.state.vy.square()).sqrt() * sgn; if (power_ > 1u) { data.costs += ((std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total). - max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); + max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_).eval(); } else { - data.costs += (std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total). - max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_; + data.costs += ((std::move((vel_total - max_vel_).max(0.0f) + (min_vel_ - vel_total). + max(0.0f)) * data.model_dt).rowwise().sum().eval() * weight_).eval(); } return; } @@ -86,10 +86,11 @@ void ConstraintCritic::score(CriticData & data) auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz.abs())).max(0.0f); if (power_ > 1u) { data.costs += ((std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + - out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).pow(power_); + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * + weight_).pow(power_).eval(); } else { - data.costs += (std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + - out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_; + data.costs += ((std::move((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + + out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() * weight_).eval(); } return; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index e0036a8534c..c5643580117 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -46,10 +46,10 @@ void GoalAngleCritic::score(CriticData & data) if(power_ > 1u) { data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_).pow(power_); + rowwise().mean()) * weight_).pow(power_).eval(); } else { - data.costs += ((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). - rowwise().mean()) * weight_; + data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). + rowwise().mean()) * weight_).eval(); } } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 73be9db274f..19e4ecbd326 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -66,8 +66,9 @@ void PathAlignCritic::score(CriticData & data) } } - const size_t batch_size = data.trajectories.x.cols(); + const size_t batch_size = data.trajectories.x.rows(); Eigen::ArrayXf cost(data.costs.rows()); + cost.setZero(); // Find integrated distance in the path std::vector path_integrated_distances(path_segments_count, 0.0f); @@ -151,9 +152,9 @@ void PathAlignCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += (std::move(cost) * weight_).pow(power_); + data.costs += (cost * weight_).pow(power_).eval(); } else { - data.costs += std::move(cost) * weight_; + data.costs += (cost * weight_).eval(); } } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 692b436ff0c..de3b0cf53f6 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -97,17 +97,18 @@ void PathAngleCritic::score(CriticData & data) throw nav2_core::ControllerException("Invalid path angle mode!"); } - int && rightmost_idx = data.trajectories.y.cols() - 1; + 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); auto yaws_between_points = diff_y.binaryExpr( - diff_x, [&](const float & y, const float & x){return atan2f(y, x);}); + 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 yaws = utils::shortest_angular_distance( - data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); + rightmost_yaw, yaws_between_points).abs(); if (power_ > 1u) { data.costs += (yaws * weight_).pow(power_); } else { @@ -117,11 +118,12 @@ 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( - data.trajectories.yaws.col(rightmost_idx), yaws_between_points).abs(); + rightmost_yaw, yaws_between_points).abs(); auto yaws_between_points_corrected = utils::point_corrected_yaws(yaws, yaws_between_points); auto corrected_yaws = utils::shortest_angular_distance( - data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); + rightmost_yaw, yaws_between_points_corrected).abs(); if (power_ > 1u) { data.costs += (corrected_yaws * weight_).pow(power_); } else { @@ -131,10 +133,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 corrected_yaws = utils::shortest_angular_distance( - data.trajectories.yaws.col(rightmost_idx), yaws_between_points_corrected).abs(); + rightmost_yaw, yaws_between_points_corrected).abs(); if (power_ > 1u) { data.costs += (corrected_yaws * weight_).pow(power_); } else { diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 0a897626dda..7fd838e8eed 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -39,9 +39,9 @@ void TwirlingCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += Eigen::pow((Eigen::abs(data.state.wz)).rowwise().mean().eval() * weight_, power_); + data.costs += ((data.state.wz.abs().rowwise().mean()) * weight_).pow(power_).eval(); } else { - data.costs += (Eigen::abs(data.state.wz)).rowwise().mean().eval() * weight_; + data.costs += ((data.state.wz.abs().rowwise().mean()) * weight_).eval(); } } diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp index 5befdf34eaa..c2d7e140947 100644 --- a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -49,12 +49,12 @@ void VelocityDeadbandCritic::score(CriticData & data) data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * - data.model_dt).rowwise().sum() * weight_).pow(power_); + data.model_dt).rowwise().sum() * weight_).pow(power_).eval(); } else { - data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * - data.model_dt).rowwise().sum() * weight_; + data.model_dt).rowwise().sum() * weight_).eval(); } return; } @@ -63,12 +63,12 @@ void VelocityDeadbandCritic::score(CriticData & data) data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * - data.model_dt).rowwise().sum() * weight_).pow(power_); + data.model_dt).rowwise().sum() * weight_).pow(power_).eval(); } else { - data.costs += (((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + + data.costs += ((((fabs(deadband_velocities_[0]) - data.state.vx.abs()).max(0.0f) + (fabs(deadband_velocities_[1]) - data.state.vy.abs()).max(0.0f) + (fabs(deadband_velocities_[2]) - data.state.wz.abs()).max(0.0f)) * - data.model_dt).rowwise().sum() * weight_; + data.model_dt).rowwise().sum() * weight_).eval(); } return; } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 5d4df563e86..ece39717686 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -110,8 +110,8 @@ void TrajectoryVisualizer::add( const float shape_1 = static_cast(n_cols); points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); - for (size_t i = 0; i != n_rows; i += trajectory_step_) { - for (size_t j = 0; j != n_cols; j += time_step_) { + for (size_t i = 0; i < n_rows; i += trajectory_step_) { + for (size_t j = 0; j < n_cols; j += time_step_) { const float j_flt = static_cast(j); float blue_component = 1.0f - j_flt / shape_1; float green_component = j_flt / shape_1; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index d5660c88fd4..9402343f330 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -66,6 +67,10 @@ TEST(CriticTests, ConstraintsCritic) costmap_ros->on_configure(lstate); models::State state; + // provide velocities in constraints, should not have any costs + state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); + state.vy = Eigen::ArrayXXf::Zero(1000, 30); + state.wz = Eigen::ArrayXXf::Ones(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; @@ -86,41 +91,34 @@ TEST(CriticTests, ConstraintsCritic) EXPECT_TRUE(critic.getMinVelConstraint() < 0.0); // Scoring testing - - // provide velocities in constraints, should not have any costs - state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); - state.vy = Eigen::ArrayXXf::Zero(1000, 30); - state.wz = Eigen::ArrayXXf::Ones(1000, 30); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // provide out of maximum velocity constraint - auto last_batch_traj_in_full = state.vx.row(-1); - last_batch_traj_in_full = 0.60 * Eigen::ArrayXf::Ones(30); + state.vx.row(999).setConstant(0.60f); critic.score(data); EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 EXPECT_NEAR(costs(999), 1.2, 0.01); - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // provide out of minimum velocity constraint - auto first_batch_traj_in_full = state.vx.row(0); - first_batch_traj_in_full = -0.45 * Eigen::ArrayXf::Ones(30); + state.vx.row(1).setConstant(-0.45f); critic.score(data); EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2 EXPECT_NEAR(costs(1), 1.2, 0.01); - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // Now with ackermann, all in constraint so no costs to score - state.vx = 0.40 * Eigen::ArrayXXf::Ones(1000, 30); - state.wz = 1.5 * Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setConstant(0.40f); + state.wz.setConstant(1.5f); data.motion_model = std::make_shared(¶m_handler, node->get_name()); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // Now violating the ackermann constraints - state.wz = 2.5 * Eigen::ArrayXXf::Ones(1000, 30); + state.wz.setConstant(2.5f); critic.score(data); EXPECT_GT(costs.sum(), 0); // 4.0 weight * 0.1 model_dt * (0.2 - 0.4/2.5) * 30 timesteps = 0.48 @@ -142,6 +140,7 @@ TEST(CriticTests, GoalAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -160,7 +159,6 @@ TEST(CriticTests, GoalAngleCritic) // provide state poses and path too far from `threshold_to_consider` to consider state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 10.0; path.y(9) = 0.0; path.yaws(9) = 3.14; @@ -194,6 +192,7 @@ TEST(CriticTests, GoalCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -212,20 +211,19 @@ TEST(CriticTests, GoalCritic) // provide state poses and path far, should not trigger state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 10.0; path.y(9) = 0.0; critic.score(data); EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Should all be 0 * 1000 - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // provide state pose and path close path.x(9) = 0.5; path.y(9) = 0.0; critic.score(data); EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight - EXPECT_NEAR(costs.sum(), 2500.0, 1e-6); // should be 2.5 * 1000 + EXPECT_NEAR(costs.sum(), 2500.0, 1e-3); // should be 2.5 * 1000 } TEST(CriticTests, PathAngleCritic) @@ -244,6 +242,7 @@ TEST(CriticTests, PathAngleCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -264,7 +263,6 @@ TEST(CriticTests, PathAngleCritic) // provide state poses and path close, within pose tolerance so won't do anything state.pose.pose.position.x = 0.0; state.pose.pose.position.y = 0.0; - path.reset(10); path.x(9) = 0.15; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -286,7 +284,7 @@ TEST(CriticTests, PathAngleCritic) // Set mode to no directional preferences + reset costs critic.setMode(1); - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ @@ -310,7 +308,7 @@ TEST(CriticTests, PathAngleCritic) // Set mode to consider path directionality + reset costs critic.setMode(2); - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // provide state pose and path close but outside of tol. with more than PI/2 angular diff. path.x(6) = 1.0; // angle between path point and pose < max_angle_to_furthest_ @@ -359,6 +357,7 @@ TEST(CriticTests, PreferForwardCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -378,19 +377,18 @@ TEST(CriticTests, PreferForwardCritic) // provide state poses and path far away, not within positional tolerances state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 10.0; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all forward motion path.x(9) = 0.15; - state.vx = Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setOnes(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0f, 1e-6f); // provide state pose and path close to trigger behavior but with all reverse motion - state.vx = -1.0 * Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setConstant(-1.0f); critic.score(data); EXPECT_GT(costs.sum(), 0.0f); EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length @@ -412,6 +410,7 @@ TEST(CriticTests, TwirlingCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(10); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -432,28 +431,28 @@ TEST(CriticTests, TwirlingCritic) // provide state poses and path far away, not within positional tolerances state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 10.0; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // provide state pose and path close to trigger behavior but with no angular variation path.x(9) = 0.15; - state.wz = Eigen::ArrayXXf::Zero(1000, 30); + state.wz.setZero(); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); // Provide nearby with some motion - auto traj_view = state.wz.row(0); - traj_view = 10.0; + state.wz.row(0).setConstant(10.0f); critic.score(data); EXPECT_NEAR(costs(0), 100.0, 1e-6); // (mean(10.0) * 10.0 weight - costs = Eigen::ArrayXf::Zero(1000); + costs.setZero(); // Now try again with some wiggling noise - traj_view = Eigen::ArrayXf::Random(30).abs() / 2.0f; + std::mt19937 engine; + std::normal_distribution normal_dist = std::normal_distribution(0.0f, 0.5f); + state.wz.row(0) = Eigen::ArrayXf::NullaryExpr(30, [&] () {return normal_dist(engine);}); critic.score(data); - EXPECT_NEAR(costs(0), 3.3, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight + EXPECT_NEAR(costs(0), 2.581, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight } TEST(CriticTests, PathFollowCritic) @@ -472,6 +471,7 @@ TEST(CriticTests, PathFollowCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(6); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -492,7 +492,6 @@ TEST(CriticTests, PathFollowCritic) // provide state poses and path close within positional tolerances state.pose.pose.position.x = 2.0; - path.reset(6); path.x(5) = 1.7; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -520,6 +519,7 @@ TEST(CriticTests, PathAlignCritic) models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); models::Path path; + path.reset(6); Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = @@ -540,7 +540,6 @@ TEST(CriticTests, PathAlignCritic) // provide state poses and path close within positional tolerances state.pose.pose.position.x = 1.0; - path.reset(10); path.x(9) = 0.85; critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); @@ -585,7 +584,7 @@ TEST(CriticTests, PathAlignCritic) path.x(19) = 0.9; path.x(20) = 0.9; path.x(21) = 0.9; - generated_trajectories.x = 0.66 * Eigen::ArrayXXf::Ones(1000, 30); + generated_trajectories.x.setConstant(0.66f); critic.score(data); // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term EXPECT_NEAR(costs.sum(), 6600.0, 1e-2); @@ -601,9 +600,9 @@ TEST(CriticTests, PathAlignCritic) } data.path_pts_valid.reset(); // Recompute on new path - costs = Eigen::ArrayXf::Zero(1000); - path.x = 1.5 * Eigen::ArrayXf::Ones(22); - path.y = 1.5 * Eigen::ArrayXf::Ones(22); + costs.setZero(); + path.x.setConstant(1.5f); + path.y.setConstant(1.5f); critic.score(data); EXPECT_NEAR(costs.sum(), 0.0, 1e-6); } @@ -622,6 +621,7 @@ TEST(CriticTests, VelocityDeadbandCritic) costmap_ros->on_configure(lstate); models::State state; + state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; @@ -642,16 +642,16 @@ TEST(CriticTests, VelocityDeadbandCritic) // Scoring testing // provide velocities out of deadband bounds, should not have any costs - state.vx = 0.80 * Eigen::ArrayXXf::Ones(1000, 30); - state.vy = 0.60 * Eigen::ArrayXXf::Ones(1000, 30); - state.wz = 0.80 * Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setConstant(0.80f); + state.vy.setConstant(0.60f); + state.wz.setConstant(0.80f); critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); // Test cost value - state.vx = 0.01 * Eigen::ArrayXXf::Ones(1000, 30); - state.vy = 0.02 * Eigen::ArrayXXf::Ones(1000, 30); - state.wz = 0.021 * Eigen::ArrayXXf::Ones(1000, 30); + state.vx.setConstant(0.01f); + state.vy.setConstant(0.02f); + state.wz.setConstant(0.021f); critic.score(data); // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 EXPECT_NEAR(costs(1), 19.845, 0.01); diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 0e3735c654b..f70f047d19f 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -86,9 +86,9 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) EXPECT_EQ(state.cvx(0), 0); EXPECT_EQ(state.cvy(0), 0); EXPECT_EQ(state.cwz(0), 0); - EXPECT_EQ(state.cvx(9), 9); - EXPECT_EQ(state.cvy(9), 9); - EXPECT_EQ(state.cwz(9), 9); + EXPECT_EQ(state.cvx(0, 9), 9); + EXPECT_EQ(state.cvy(0, 9), 9); + EXPECT_EQ(state.cwz(0, 9), 9); // Request an update with noise requested generator.generateNextNoises(); @@ -97,16 +97,16 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) EXPECT_NE(state.cvx(0), 0); EXPECT_EQ(state.cvy(0), 0); // Not populated in non-holonomic EXPECT_NE(state.cwz(0), 0); - EXPECT_NE(state.cvx(9), 9); - EXPECT_EQ(state.cvy(9), 9); // Not populated in non-holonomic - EXPECT_NE(state.cwz(9), 9); + EXPECT_NE(state.cvx(0, 9), 9); + EXPECT_EQ(state.cvy(0, 9), 9); // Not populated in non-holonomic + EXPECT_NE(state.cwz(0, 9), 9); EXPECT_NEAR(state.cvx(0), 0, 0.3); EXPECT_NEAR(state.cvy(0), 0, 0.3); EXPECT_NEAR(state.cwz(0), 0, 0.3); - EXPECT_NEAR(state.cvx(9), 9, 0.3); - EXPECT_NEAR(state.cvy(9), 9, 0.3); - EXPECT_NEAR(state.cwz(9), 9, 0.3); + EXPECT_NEAR(state.cvx(0, 9), 9, 0.3); + EXPECT_NEAR(state.cvy(0, 9), 9, 0.3); + EXPECT_NEAR(state.cwz(0, 9), 9, 0.3); // Test holonomic setting generator.reset(settings, true); // Now holonomically @@ -116,16 +116,16 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) EXPECT_NE(state.cvx(0), 0); EXPECT_NE(state.cvy(0), 0); // Now populated in non-holonomic EXPECT_NE(state.cwz(0), 0); - EXPECT_NE(state.cvx(9), 9); - EXPECT_NE(state.cvy(9), 9); // Now populated in non-holonomic - EXPECT_NE(state.cwz(9), 9); + EXPECT_NE(state.cvx(0, 9), 9); + EXPECT_NE(state.cvy(0, 9), 9); // Now populated in non-holonomic + EXPECT_NE(state.cwz(0, 9), 9); EXPECT_NEAR(state.cvx(0), 0, 0.3); EXPECT_NEAR(state.cvy(0), 0, 0.3); EXPECT_NEAR(state.cwz(0), 0, 0.3); - EXPECT_NEAR(state.cvx(9), 9, 0.3); - EXPECT_NEAR(state.cvy(9), 9, 0.3); - EXPECT_NEAR(state.cwz(9), 9, 0.3); + EXPECT_NEAR(state.cvx(0, 9), 9, 0.3); + EXPECT_NEAR(state.cvy(0, 9), 9, 0.3); + EXPECT_NEAR(state.cwz(0, 9), 9, 0.3); generator.shutdown(); } diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 7e17606b5c7..2e9b633205b 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -177,7 +178,7 @@ TEST(UtilsTests, AnglesTests) } } - auto norm_ang = normalize_angles(angles); + auto norm_ang = normalize_angles(angles).eval(); for (unsigned int i = 0; i != norm_ang.size(); i++) { EXPECT_TRUE((norm_ang(i) >= -M_PIF) && (norm_ang(i) <= M_PIF)); } @@ -185,7 +186,7 @@ TEST(UtilsTests, AnglesTests) // Test shortest angular distance Eigen::ArrayXf zero_angles(100); zero_angles.setZero(); - auto ang_dist = shortest_angular_distance(angles, zero_angles); + auto ang_dist = shortest_angular_distance(angles, zero_angles).eval(); for (unsigned int i = 0; i != ang_dist.size(); i++) { EXPECT_TRUE((ang_dist(i) >= -M_PIF) && (ang_dist(i) <= M_PIF)); } @@ -330,7 +331,10 @@ TEST(UtilsTests, SmootherTest) noisey_sequence.wz = 0.3 * Eigen::ArrayXf::Ones(30); // Make the sequence noisy - auto noises = ((Eigen::ArrayXf::Random(30)).abs()) / 5.0f; + std::mt19937 engine; + std::normal_distribution normal_dist = std::normal_distribution(0.0f, 0.2f); + auto noises = Eigen::ArrayXf::NullaryExpr( + 30, [&] () {return normal_dist(engine);}); noisey_sequence.vx += noises; noisey_sequence.vy += noises; noisey_sequence.wz += noises;