diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 931af234887..c28c711dc50 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -59,6 +60,7 @@ target_link_libraries(mppi_controller PUBLIC nav2_ros_common::nav2_ros_common nav2_costmap_2d::layers nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 68db5b73719..630aa19f214 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -59,6 +59,7 @@ This process is then repeated a number of times and returns a converged solution | 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. | | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | + | publish_critics_stats | bool | Default false. Whether to publish statistics about each critic's performance. When enabled, publishes a `nav2_msgs::msg::CriticsStats` message containing critic names, whether they changed costs, and the sum of costs added by each critic. Useful for debugging and tuning critic behavior. | #### Trajectory Visualizer @@ -293,6 +294,7 @@ controller_server: |---------------------------|----------------------------------|-----------------------------------------------------------------------| | `trajectories` | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence | | `transformed_global_plan` | `nav_msgs/Path` | Part of global plan considered by local planner | +| `critics_stats` | `nav2_msgs/CriticsStats` | Statistics about each critic's performance (published when `publish_critics_stats` is enabled) | ## Notes to Users @@ -328,6 +330,26 @@ As you increase or decrease your weights on the Obstacle, you may notice the afo Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off. +### Critic costs debugging + +The `publish_critics_stats` parameter enables publishing of statistics about each critic's performance, which can be visualized using tools like PlotJuggler or Foxglove to analyze and debug critic behavior. + +The published `nav2_msgs::msg::CriticsStats` message contains the following fields: + +- **stamp**: Timestamp of when the statistics were computed +- **critics**: Array of critic names that were evaluated (e.g., "ConstraintCritic", "GoalCritic", "ObstaclesCritic") +- **changed**: Boolean array indicating whether each critic modified the trajectory costs. `true` means the critic added non-zero costs, `false` means it had no effect +- **costs_sum**: Array of the total cost contribution from each critic. This represents the sum of all costs added by that specific critic across all trajectory candidates + +This data is invaluable for understanding: +- Which critics are actively influencing trajectory selection +- The relative impact of each critic on the final trajectory choice +- Whether critics are working as expected or if parameter tuning is needed (e.g. `threshold_to_consider`) + +More detailed statistics could be added in the future. + +![critics_stats](media/critics_stats.png) + ### MFMA and AVX2 Optimizations This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index fed85867905..237649bedde 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -22,6 +22,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav2_msgs/msg/critics_stats.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -96,6 +97,9 @@ class CriticManager std::unique_ptr> loader_; Critics critics_; + nav2::Publisher::SharedPtr critics_effect_pub_; + bool publish_critics_stats_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/media/critics_stats.png b/nav2_mppi_controller/media/critics_stats.png new file mode 100644 index 00000000000..c373f8e0dcc Binary files /dev/null and b/nav2_mppi_controller/media/critics_stats.png differ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index a27716ab997..fb73fce776a 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -18,6 +18,7 @@ nav2_common nav2_core nav2_costmap_2d + nav2_msgs nav2_util nav_msgs pluginlib diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index d6acd320b05..305140e2e13 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -37,6 +37,7 @@ void CriticManager::getParams() auto node = parent_.lock(); auto getParam = parameters_handler_->getParamGetter(name_); getParam(critic_names_, "critics", std::vector{}, ParameterType::Static); + getParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static); } void CriticManager::loadCritics() @@ -46,6 +47,13 @@ void CriticManager::loadCritics() "nav2_mppi_controller", "mppi::critics::CriticFunction"); } + auto node = parent_.lock(); + if (publish_critics_stats_) { + critics_effect_pub_ = node->create_publisher( + "~/critics_stats"); + critics_effect_pub_->on_activate(); + } + critics_.clear(); for (auto name : critic_names_) { std::string fullname = getFullName(name); @@ -67,11 +75,44 @@ std::string CriticManager::getFullName(const std::string & name) void CriticManager::evalTrajectoriesScores( CriticData & data) const { - for (const auto & critic : critics_) { + std::unique_ptr stats_msg; + if (publish_critics_stats_) { + stats_msg = std::make_unique(); + stats_msg->critics.reserve(critics_.size()); + stats_msg->changed.reserve(critics_.size()); + stats_msg->costs_sum.reserve(critics_.size()); + } + + for (size_t i = 0; i < critics_.size(); ++i) { if (data.fail_flag) { break; } - critic->score(data); + + // Store costs before critic evaluation + Eigen::ArrayXf costs_before; + if (publish_critics_stats_) { + costs_before = data.costs; + } + + critics_[i]->score(data); + + // Calculate statistics if publishing is enabled + if (publish_critics_stats_) { + stats_msg->critics.push_back(critic_names_[i]); + + // Calculate sum of costs added by this individual critic + Eigen::ArrayXf cost_diff = data.costs - costs_before; + float costs_sum = cost_diff.sum(); + stats_msg->costs_sum.push_back(costs_sum); + stats_msg->changed.push_back(costs_sum != 0.0f); + } + } + + // Publish statistics if enabled + if (critics_effect_pub_) { + auto node = parent_.lock(); + stats_msg->stamp = node->get_clock()->now(); + critics_effect_pub_->publish(std::move(stats_msg)); } } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 93f7998c56a..14d1327508b 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" + "msg/CriticsStats.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/WaypointStatus.msg" diff --git a/nav2_msgs/msg/CriticsStats.msg b/nav2_msgs/msg/CriticsStats.msg new file mode 100644 index 00000000000..f02406228df --- /dev/null +++ b/nav2_msgs/msg/CriticsStats.msg @@ -0,0 +1,5 @@ +# Critics statistics message +builtin_interfaces/Time stamp +string[] critics +bool[] changed +float32[] costs_sum diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index b81cef7ce56..01fe9656d5b 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -76,6 +76,7 @@ controller_server: temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" + publish_critics_stats: true visualize: true regenerate_noises: true TrajectoryVisualizer: