From ff53e762066b9aecc86160a2d54c5d2a6f66a1d4 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 10 Oct 2023 15:22:11 -0700 Subject: [PATCH 01/12] adding regenerate noise param + adding docs --- nav2_mppi_controller/CMakeLists.txt | 5 +++- nav2_mppi_controller/README.md | 5 ++++ .../tools/noise_generator.hpp | 13 +++++++---- .../src/critics/goal_angle_critic.cpp | 6 +---- .../src/critics/goal_critic.cpp | 6 +---- .../src/critics/obstacles_critic.cpp | 2 +- .../src/critics/prefer_forward_critic.cpp | 7 +----- .../src/critics/twirling_critic.cpp | 6 +---- nav2_mppi_controller/src/noise_generator.cpp | 23 +++++++++++++++---- nav2_mppi_controller/src/optimizer.cpp | 2 +- .../test/noise_generator_test.cpp | 11 +++++++-- 11 files changed, 52 insertions(+), 34 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 5cc8a8c0e9..1344f5cccb 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -6,7 +6,10 @@ add_definitions(-DXTENSOR_USE_XSIMD) set(XTENSOR_USE_TBB 0) set(XTENSOR_USE_OPENMP 0) +set(XTENSOR_USE_XSIMD 1) +# set(XTENSOR_DEFAULT_LAYOUT column_major) # row_major, column_major +# set(XTENSOR_DEFAULT_TRAVERSAL row_major) # row_major, column_major find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) @@ -76,7 +79,7 @@ set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) - target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS} ${OpenMP_INCLUDE_DIRS}) + target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd) ament_target_dependencies(${lib} ${dependencies_pkgs}) endforeach() diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 2557bcd7d6..44ed354910 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -29,6 +29,7 @@ This process is then repeated a number of times and returns a converged solution - Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors - High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide - Easily extensible to support modern research variants of MPPI +- Comes pre-tuned for good out-of-the-box behavior ## Configuration @@ -52,6 +53,8 @@ This process is then repeated a number of times and returns a converged solution | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | + | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | + #### Trajectory Visualizer | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -254,6 +257,8 @@ The most common parameters you might want to start off changing are the velocity If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. +By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. + ### Prediction Horizon, Costmap Sizing, and Offsets As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index 6eb1b36384..a811d998e4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -25,8 +25,9 @@ #include #include "nav2_mppi_controller/models/optimizer_settings.hpp" -#include -#include +#include "nav2_mppi_controller/tools/parameters_handler.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/state.hpp" namespace mppi { @@ -47,8 +48,12 @@ class NoiseGenerator * @brief Initialize noise generator with settings and model types * @param settings Settings of controller * @param is_holonomic If base is holonomic + * @param name Namespace for configs + * @param param_handler Get parameters util */ - void initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic); + void initialize( + mppi::models::OptimizerSettings & settings, + bool is_holonomic, const std::string & name, ParametersHandler * param_handler); /** * @brief Shutdown noise generator thread @@ -99,7 +104,7 @@ class NoiseGenerator std::thread noise_thread_; std::condition_variable noise_cond_; std::mutex noise_lock_; - bool active_{false}, ready_{false}; + bool active_{false}, ready_{false}, regenerate_noises_{false}; }; } // namespace mppi diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 62bfdf0676..604ee24eb1 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -35,11 +35,7 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - if (!utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.path)) { return; diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 852db11529..f61d6fd2b2 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -35,11 +35,7 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - if (!utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.path)) { return; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 80ac77e10f..90fa92a57a 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -152,7 +152,7 @@ void ObstaclesCritic::score(CriticData & data) } if (!trajectory_collide) {all_trajectories_collide = false;} - raw_cost[i] = static_cast(trajectory_collide ? collision_cost_ : traj_cost); + raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost; } data.costs += xt::pow( diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 18b6900177..88bf7e8cdf 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -33,12 +33,7 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 28eb71b48b..aa7cc2e6b5 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -31,11 +31,7 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) { + if (!enabled_ || utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) { return; } diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 09ee4ab92d..b7c452aa6a 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -23,12 +23,22 @@ namespace mppi { -void NoiseGenerator::initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic) +void NoiseGenerator::initialize( + mppi::models::OptimizerSettings & settings, bool is_holonomic, + const std::string & name, ParametersHandler * param_handler) { settings_ = settings; is_holonomic_ = is_holonomic; active_ = true; - noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + + auto getParam = param_handler->getParamGetter(name); + getParam(regenerate_noises_, "regenerate_noises", false); + + if (regenerate_noises_) { + noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + } else { + generateNoisedControls(); + } } void NoiseGenerator::shutdown() @@ -44,7 +54,7 @@ void NoiseGenerator::shutdown() void NoiseGenerator::generateNextNoises() { // Trigger the thread to run in parallel to this iteration - // to generate the next iteration's noises. + // to generate the next iteration's noises (if applicable). { std::unique_lock guard(noise_lock_); ready_ = true; @@ -76,7 +86,12 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h xt::noalias(noises_wz_) = xt::zeros({settings_.batch_size, settings_.time_steps}); ready_ = true; } - noise_cond_.notify_all(); + + if (regenerate_noises_) { + noise_cond_.notify_all(); + } else { + generateNoisedControls(); + } } void NoiseGenerator::noiseThread() diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 6d0a3ceb6c..8eb01578d8 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -49,7 +49,7 @@ void Optimizer::initialize( getParams(); critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_); - noise_generator_.initialize(settings_, isHolonomic()); + noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_); reset(); } diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 3788f2b8a3..92510d7736 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -42,13 +42,20 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) settings.batch_size = 100; settings.time_steps = 25; - generator.initialize(settings, false); + auto node = std::make_shared("node"); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); + ParametersHandler handler(node); + + generator.initialize(settings, false, "test_name", &handler); + generator.reset(settings, false); generator.shutdown(); } TEST(NoiseGeneratorTest, NoiseGeneratorMain) { // Tests shuts down internal thread cleanly + auto node = std::make_shared("node"); + ParametersHandler handler(node); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; @@ -70,7 +77,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) state.reset(settings.batch_size, settings.time_steps); // Request an update with no noise yet generated, should result in identical outputs - generator.initialize(settings, false); + generator.initialize(settings, false, "test_name", &handler); generator.reset(settings, false); // sets initial sizing and zeros out noises generator.setNoisedControls(state, control_sequence); EXPECT_EQ(state.cvx(0), 0); From c4c6a741380e41fcf8052867b69428b78da13da9 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 10 Oct 2023 15:28:56 -0700 Subject: [PATCH 02/12] fix tests --- nav2_mppi_controller/src/critics/prefer_forward_critic.cpp | 4 +++- nav2_mppi_controller/src/critics/twirling_critic.cpp | 4 +++- nav2_mppi_controller/test/noise_generator_test.cpp | 3 ++- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 88bf7e8cdf..08e755dd6a 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -33,7 +33,9 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index aa7cc2e6b5..6b492d13ba 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -31,7 +31,9 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - if (!enabled_ || utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) + { return; } diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 92510d7736..db8667e940 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -43,7 +43,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) settings.time_steps = 25; auto node = std::make_shared("node"); - node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); ParametersHandler handler(node); generator.initialize(settings, false, "test_name", &handler); @@ -55,6 +55,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) { // Tests shuts down internal thread cleanly auto node = std::make_shared("node"); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); ParametersHandler handler(node); NoiseGenerator generator; mppi::models::OptimizerSettings settings; From 8c449cf58e067a571fd5bc7da739ee553f2a99fd Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 10 Oct 2023 16:52:42 -0700 Subject: [PATCH 03/12] remove unnecessary normalization --- nav2_mppi_controller/src/optimizer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 8eb01578d8..3405566d84 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -278,8 +279,7 @@ void Optimizer::integrateStateVelocities( auto traj_y = xt::view(trajectory, xt::all(), 1); auto traj_yaws = xt::view(trajectory, xt::all(), 2); - xt::noalias(traj_yaws) = - utils::normalize_angles(xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw); + xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0 + initial_yaw); auto && yaw_cos = xt::xtensor::from_shape(traj_yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(traj_yaws.shape()); @@ -307,10 +307,10 @@ void Optimizer::integrateStateVelocities( models::Trajectories & trajectories, const models::State & state) const { - const double initial_yaw = tf2::getYaw(state.pose.pose.orientation); + const float initial_yaw = tf2::getYaw(state.pose.pose.orientation); xt::noalias(trajectories.yaws) = - utils::normalize_angles(xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw); + xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw; const auto yaws_cutted = xt::view(trajectories.yaws, xt::all(), xt::range(0, -1)); From 11858ec5f8890c67a57287e37c2291cf16af9af0 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Oct 2023 17:06:05 -0700 Subject: [PATCH 04/12] Update optimizer.cpp --- nav2_mppi_controller/src/optimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3405566d84..1dd8420230 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -269,7 +269,7 @@ void Optimizer::integrateStateVelocities( xt::xtensor & trajectory, const xt::xtensor & sequence) const { - double initial_yaw = tf2::getYaw(state_.pose.pose.orientation); + float initial_yaw = tf2::getYaw(state_.pose.pose.orientation); const auto vx = xt::view(sequence, xt::all(), 0); const auto vy = xt::view(sequence, xt::all(), 2); From 870f467ef498d60dbb5c2efcac6fcfbf4bf93b60 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 11 Oct 2023 16:05:10 -0700 Subject: [PATCH 05/12] adding refactored path alignment critic --- .../nav2_mppi_controller/tools/utils.hpp | 17 +++++ .../src/critics/path_align_critic.cpp | 67 ++++++++++--------- 2 files changed, 51 insertions(+), 33 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 6baa27873e..7f3be8f42e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -688,6 +688,23 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) return first_after_inversion; } +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + */ +inline size_t findClosestPathPt(const std::vector & vec, float dist) +{ + auto iter = std::lower_bound(vec.begin(), vec.end(), dist); + if (iter == vec.begin()) { + return 0; + } + if (dist - *(iter - 1) < *iter - dist) { + return iter - 1 - vec.begin(); + } + return iter - vec.begin(); +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 4ff12d4e55..2c3e16897c 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -54,7 +54,8 @@ void PathAlignCritic::score(CriticData & data) // Don't apply when first getting bearing w.r.t. the path utils::setPathFurthestPointIfNotSet(data); - if (*data.furthest_reached_path_point < offset_from_furthest_) { + const size_t path_segments_count = *data.furthest_reached_path_point; // up to furthest only + if (path_segments_count < offset_from_furthest_) { return; } @@ -80,49 +81,49 @@ void PathAlignCritic::score(CriticData & data) const size_t batch_size = T_x.shape(0); const size_t time_steps = T_x.shape(1); - const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); - const size_t path_segments_count = data.path.x.shape(0) - 1; auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); - if (path_segments_count < 1) { - return; + // Find integrated distance in the path + std::vector path_integrated_distances(path_segments_count, 0.0f); + float dx = 0.0f, dy = 0.0f; + for (unsigned int i = 1; i != path_segments_count; i++) { + dx = P_x(i) - P_x(i - 1); + dy = P_y(i) - P_y(i - 1); + float curr_dist = sqrtf(dx * dx + dy * dy); + path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist; } - float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; - float min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; - + float traj_integrated_distance = 0.0f; + float summed_path_dist = 0.0f, dyaw = 0.0f; + float num_samples = 0.0f; + size_t path_pt; for (size_t t = 0; t < batch_size; ++t) { - summed_dist = 0.0f; + traj_integrated_distance = 0.0f; + summed_path_dist = 0.0f; + num_samples = 0.0f; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - min_dist_sq = std::numeric_limits::max(); - min_s = 0; - - // Find closest path segment to the trajectory point - for (size_t s = 0; s < path_segments_count - 1; s++) { - xt::xtensor_fixed> P; - dx = P_x(s) - T_x(t, p); - dy = P_y(s) - T_y(t, p); - if (use_path_orientations_) { - dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); - dist_sq = dx * dx + dy * dy + dyaw * dyaw; - } else { - dist_sq = dx * dx + dy * dy; - } - if (dist_sq < min_dist_sq) { - min_dist_sq = dist_sq; - min_s = s; - } - } + dx = T_x(t, p) - T_x(t, p - trajectory_point_step_); + dy = T_y(t, p) - T_y(t, p - trajectory_point_step_); + traj_integrated_distance += sqrtf(dx * dx + dy * dy); + path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance); // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles - if (min_s != 0 && (*data.path_pts_valid)[min_s]) { - summed_dist += sqrtf(min_dist_sq); + if ((*data.path_pts_valid)[path_pt]) { + dx = P_x(path_pt) - T_x(t, p); + dy = P_y(path_pt) - T_y(t, p); + num_samples += 1.0f; + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(t, p)); + summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); + } else { + summed_path_dist += sqrtf(dx * dx + dy * dy); + } } } - - cost[t] = summed_dist / traj_pts_eval; + if (num_samples > 0) { + cost[t] = summed_path_dist / num_samples; + } } data.costs += xt::pow(std::move(cost) * weight_, power_); From a16ddc10fbfb02dbe1097f32b6e88f66c5222b79 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 11 Oct 2023 16:15:59 -0700 Subject: [PATCH 06/12] fix visualization bug --- nav2_mppi_controller/src/optimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 1dd8420230..3b053f4577 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -279,7 +279,7 @@ void Optimizer::integrateStateVelocities( auto traj_y = xt::view(trajectory, xt::all(), 1); auto traj_yaws = xt::view(trajectory, xt::all(), 2); - xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0 + initial_yaw); + xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw; auto && yaw_cos = xt::xtensor::from_shape(traj_yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(traj_yaws.shape()); From 9c1cbae366acfbc5fbe08bb07cdcc302f52e4287 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 12 Oct 2023 10:48:20 -0700 Subject: [PATCH 07/12] speed up another 30% --- .../nav2_mppi_controller/tools/utils.hpp | 4 +-- .../src/critics/path_align_critic.cpp | 25 +++++++++---------- 2 files changed, 14 insertions(+), 15 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 7f3be8f42e..81926e7682 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -693,9 +693,9 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) * @param vec Vect to check * @return dist Distance to look for */ -inline size_t findClosestPathPt(const std::vector & vec, float dist) +inline size_t findClosestPathPt(const std::vector & vec, float dist, size_t init = 0) { - auto iter = std::lower_bound(vec.begin(), vec.end(), dist); + auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist); if (iter == vec.begin()) { return 0; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 2c3e16897c..b47ae916ce 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -71,16 +71,12 @@ void PathAlignCritic::score(CriticData & data) } } - const auto & T_x = data.trajectories.x; - const auto & T_y = data.trajectories.y; - const auto & T_yaw = data.trajectories.yaws; - const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points - const size_t batch_size = T_x.shape(0); - const size_t time_steps = T_x.shape(1); + const size_t batch_size = data.trajectories.x.shape(0); + const size_t time_steps = data.trajectories.x.shape(1); auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); // Find integrated distance in the path @@ -96,25 +92,28 @@ void PathAlignCritic::score(CriticData & data) float traj_integrated_distance = 0.0f; float summed_path_dist = 0.0f, dyaw = 0.0f; float num_samples = 0.0f; - size_t path_pt; + size_t path_pt = 0; for (size_t t = 0; t < batch_size; ++t) { traj_integrated_distance = 0.0f; summed_path_dist = 0.0f; num_samples = 0.0f; + const auto T_x = xt::view(data.trajectories.x, t, xt::all()); + const auto T_y = xt::view(data.trajectories.y, t, xt::all()); + const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all()); for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - dx = T_x(t, p) - T_x(t, p - trajectory_point_step_); - dy = T_y(t, p) - T_y(t, p - trajectory_point_step_); + dx = T_x(p) - T_x(p - trajectory_point_step_); + dy = T_y(p) - T_y(p - trajectory_point_step_); traj_integrated_distance += sqrtf(dx * dx + dy * dy); - path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance); + path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance, path_pt); // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles if ((*data.path_pts_valid)[path_pt]) { - dx = P_x(path_pt) - T_x(t, p); - dy = P_y(path_pt) - T_y(t, p); + dx = P_x(path_pt) - T_x(p); + dy = P_y(path_pt) - T_y(p); num_samples += 1.0f; if (use_path_orientations_) { - dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(t, p)); + dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p)); summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); } else { summed_path_dist += sqrtf(dx * dx + dy * dy); From 38bccaa9fa6d7f0f0575f0e5c19284abab00b72a Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 12 Oct 2023 11:30:27 -0700 Subject: [PATCH 08/12] remove a little jitter --- .../include/nav2_mppi_controller/tools/utils.hpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 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 81926e7682..4a67bc276c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -308,14 +308,16 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) const auto dists = dx * dx + dy * dy; - size_t max_id_by_trajectories = 0; + size_t max_id_by_trajectories = 0, min_id_by_path = 0; float min_distance_by_path = std::numeric_limits::max(); + float cur_dist = 0.0f; for (size_t i = 0; i < dists.shape(0); i++) { - size_t min_id_by_path = 0; + min_id_by_path = 0; for (size_t j = 0; j < dists.shape(1); j++) { - if (dists(i, j) < min_distance_by_path) { - min_distance_by_path = dists(i, j); + cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; min_id_by_path = j; } } From d0421332f4d591102ba638a1b3ba076eade3a4cb Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 12 Oct 2023 11:47:55 -0700 Subject: [PATCH 09/12] a few more small optimizaitons --- .../src/critics/path_align_critic.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index b47ae916ce..2b6c9139b1 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -92,6 +92,7 @@ void PathAlignCritic::score(CriticData & data) float traj_integrated_distance = 0.0f; float summed_path_dist = 0.0f, dyaw = 0.0f; float num_samples = 0.0f; + float Tx = 0.0f, Ty = 0.0f; size_t path_pt = 0; for (size_t t = 0; t < batch_size; ++t) { traj_integrated_distance = 0.0f; @@ -99,20 +100,22 @@ void PathAlignCritic::score(CriticData & data) num_samples = 0.0f; const auto T_x = xt::view(data.trajectories.x, t, xt::all()); const auto T_y = xt::view(data.trajectories.y, t, xt::all()); - const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all()); for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - dx = T_x(p) - T_x(p - trajectory_point_step_); - dy = T_y(p) - T_y(p - trajectory_point_step_); + Tx = T_x(p); + Ty = T_y(p); + dx = Tx - T_x(p - trajectory_point_step_); + dy = Ty - T_y(p - trajectory_point_step_); traj_integrated_distance += sqrtf(dx * dx + dy * dy); path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance, path_pt); // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles if ((*data.path_pts_valid)[path_pt]) { - dx = P_x(path_pt) - T_x(p); - dy = P_y(path_pt) - T_y(p); + dx = P_x(path_pt) - Tx; + dy = P_y(path_pt) - Ty; num_samples += 1.0f; if (use_path_orientations_) { + const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all()); dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p)); summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); } else { From 8d7bf3e82c25a9d8f521d6b65e0efe48d30c374d Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 12 Oct 2023 14:43:23 -0700 Subject: [PATCH 10/12] fixing unit tests --- nav2_mppi_controller/src/critics/path_align_critic.cpp | 3 ++- nav2_mppi_controller/test/critics_tests.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 2b6c9139b1..343f57e3c0 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -106,7 +106,8 @@ void PathAlignCritic::score(CriticData & data) dx = Tx - T_x(p - trajectory_point_step_); dy = Ty - T_y(p - trajectory_point_step_); traj_integrated_distance += sqrtf(dx * dx + dy * dy); - path_pt = utils::findClosestPathPt(path_integrated_distances, traj_integrated_distance, path_pt); + path_pt = utils::findClosestPathPt( + path_integrated_distances, traj_integrated_distance, path_pt); // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 8b5efae8da..ae037141d0 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -586,8 +586,8 @@ TEST(CriticTests, PathAlignCritic) path.x(21) = 0.9; generated_trajectories.x = 0.66 * xt::ones({1000, 30}); critic.score(data); - // 0.04 * 1000 * 10 weight * 4 num pts eval / 4 normalization term - EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2); + // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term + EXPECT_NEAR(xt::sum(costs, immediate)(), 6600.0, 1e-2); // provide state pose and path far enough to enable, with data to pass condition // but path is blocked in collision From e7c4dd3800747eca0a742aac08fa990498439922 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 12 Oct 2023 15:03:59 -0700 Subject: [PATCH 11/12] retain legacy critic --- nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/critics.xml | 4 + .../critics/path_align_critic.hpp | 2 +- .../critics/path_align_legacy_critic.hpp | 60 ++++++++ .../src/critics/path_align_critic.cpp | 2 +- .../src/critics/path_align_legacy_critic.cpp | 137 ++++++++++++++++++ nav2_mppi_controller/test/critics_tests.cpp | 104 +++++++++++++ 7 files changed, 308 insertions(+), 2 deletions(-) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index af64e1b1ed..29dfde9672 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -78,6 +78,7 @@ add_library(mppi_critics SHARED src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp + src/critics/path_align_legacy_critic.cpp src/critics/path_follow_critic.cpp src/critics/path_angle_critic.cpp src/critics/prefer_forward_critic.cpp diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 669944f2fe..790568e796 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -17,6 +17,10 @@ mppi critic for aligning to path + + mppi critic for aligning to path (legacy) + + mppi critic for tracking the path in the correct heading diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index a549995557..f7ad2fda6a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp new file mode 100644 index 0000000000..65d9d0d61e --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::PathAlignLegacyCritic + * @brief Critic objective function for aligning to the path. Note: + * High settings of this will follow the path more precisely, but also makes it + * difficult (or impossible) to deviate in the presence of dynamic obstacles. + * This is an important critic to tune and consider in tandem with Obstacle. + * This is the initial 'Legacy' implementation before replacement Oct 2023. + */ +class PathAlignLegacyCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to trajectories path alignment + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + size_t offset_from_furthest_{0}; + int trajectory_point_step_{0}; + float threshold_to_consider_{0}; + float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 343f57e3c0..4b4d93abf3 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp new file mode 100644 index 0000000000..a04f4a9fa9 --- /dev/null +++ b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" + +#include +#include + +namespace mppi::critics +{ + +using namespace xt::placeholders; // NOLINT +using xt::evaluation_strategy::immediate; + +void PathAlignLegacyCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 10.0); + + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07); + getParam(offset_from_furthest_, "offset_from_furthest", 20); + getParam(trajectory_point_step_, "trajectory_point_step", 4); + getParam( + threshold_to_consider_, + "threshold_to_consider", 0.5); + getParam(use_path_orientations_, "use_path_orientations", false); + + RCLCPP_INFO( + logger_, + "ReferenceTrajectoryCritic instantiated with %d power and %f weight", + power_, weight_); +} + +void PathAlignLegacyCritic::score(CriticData & data) +{ + // Don't apply close to goal, let the goal critics take over + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { + return; + } + + // Don't apply when first getting bearing w.r.t. the path + utils::setPathFurthestPointIfNotSet(data); + if (*data.furthest_reached_path_point < offset_from_furthest_) { + return; + } + + // Don't apply when dynamic obstacles are blocking significant proportions of the local path + utils::setPathCostsIfNotSet(data, costmap_ros_); + const size_t closest_initial_path_point = utils::findPathTrajectoryInitialPoint(data); + unsigned int invalid_ctr = 0; + const float range = *data.furthest_reached_path_point - closest_initial_path_point; + for (size_t i = closest_initial_path_point; i < *data.furthest_reached_path_point; i++) { + if (!(*data.path_pts_valid)[i]) {invalid_ctr++;} + if (static_cast(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) { + return; + } + } + + const auto & T_x = data.trajectories.x; + const auto & T_y = data.trajectories.y; + const auto & T_yaw = data.trajectories.yaws; + + const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points + const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points + const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points + + const size_t batch_size = T_x.shape(0); + const size_t time_steps = T_x.shape(1); + const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); + const size_t path_segments_count = data.path.x.shape(0) - 1; + auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); + + if (path_segments_count < 1) { + return; + } + + float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; + float min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; + + for (size_t t = 0; t < batch_size; ++t) { + summed_dist = 0.0f; + for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { + min_dist_sq = std::numeric_limits::max(); + min_s = 0; + + // Find closest path segment to the trajectory point + for (size_t s = 0; s < path_segments_count - 1; s++) { + xt::xtensor_fixed> P; + dx = P_x(s) - T_x(t, p); + dy = P_y(s) - T_y(t, p); + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); + dist_sq = dx * dx + dy * dy + dyaw * dyaw; + } else { + dist_sq = dx * dx + dy * dy; + } + if (dist_sq < min_dist_sq) { + min_dist_sq = dist_sq; + min_s = s; + } + } + + // The nearest path point to align to needs to be not in collision, else + // let the obstacle critic take over in this region due to dynamic obstacles + if (min_s != 0 && (*data.path_pts_valid)[min_s]) { + summed_dist += sqrtf(min_dist_sq); + } + } + + cost[t] = summed_dist / traj_pts_eval; + } + + data.costs += xt::pow(std::move(cost) * weight_, power_); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::PathAlignLegacyCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index ae037141d0..397a6f06ae 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -606,3 +606,107 @@ TEST(CriticTests, PathAlignCritic) critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } + +TEST(CriticTests, PathAlignLegacyCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + PathAlignLegacyCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // 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(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable + // but data furthest point reached is 0 and offset default is 20, so returns + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // but with empty trajectories and paths, should still be zero + *data.furthest_reached_path_point = 21; + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // and with a valid path to pass invalid path condition + state.pose.pose.position.x = 0.0; + data.path_pts_valid.reset(); // Recompute on new path + path.reset(22); + path.x(0) = 0; + path.x(1) = 0.1; + path.x(2) = 0.2; + path.x(3) = 0.3; + path.x(4) = 0.4; + path.x(5) = 0.5; + path.x(6) = 0.6; + path.x(7) = 0.7; + path.x(8) = 0.8; + path.x(9) = 0.9; + path.x(10) = 0.9; + path.x(11) = 0.9; + path.x(12) = 0.9; + path.x(13) = 0.9; + path.x(14) = 0.9; + path.x(15) = 0.9; + path.x(16) = 0.9; + path.x(17) = 0.9; + path.x(18) = 0.9; + path.x(19) = 0.9; + path.x(20) = 0.9; + path.x(21) = 0.9; + generated_trajectories.x = 0.66 * xt::ones({1000, 30}); + critic.score(data); + // 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term + EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2); + + // provide state pose and path far enough to enable, with data to pass condition + // but path is blocked in collision + auto * costmap = costmap_ros->getCostmap(); + // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution + for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m + for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m + costmap->setCost(i, j, 254); + } + } + + data.path_pts_valid.reset(); // Recompute on new path + costs = xt::zeros({1000}); + path.x = 1.5 * xt::ones({22}); + path.y = 1.5 * xt::ones({22}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); +} From b7d7fc22a5f88737f0688b592f81213dbad93fb8 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 12 Oct 2023 15:06:53 -0700 Subject: [PATCH 12/12] adding tests for legacy --- nav2_mppi_controller/LICENSE.md | 1 + nav2_mppi_controller/README.md | 2 ++ nav2_mppi_controller/test/critics_tests.cpp | 1 + 3 files changed, 4 insertions(+) diff --git a/nav2_mppi_controller/LICENSE.md b/nav2_mppi_controller/LICENSE.md index da24c0064f..57a5719427 100644 --- a/nav2_mppi_controller/LICENSE.md +++ b/nav2_mppi_controller/LICENSE.md @@ -2,6 +2,7 @@ MIT License Copyright (c) 2021-2022 Fast Sense Studio Copyright (c) 2022-2023 Samsung Research America +Copyright (c) 2023 Open Navigation LLC Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 3872b72610..13c314205b 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -119,6 +119,8 @@ This process is then repeated a number of times and returns a converged solution | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | +Note: There is a "Legacy" version of this critic also available with the same parameters of an old formulation pre-October 2023. + #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 397a6f06ae..f88249aed1 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -25,6 +25,7 @@ #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" +#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp"