[codex] Add CUDA MPPI diagnostics and benchmark scenarios

include/cuda_mppi_controller/mppi_gpu.hpp

@@ -83,6 +83,10 @@ struct MppiResult
   float vy = 0.0f;
   float w = 0.0f;
   float best_cost = 0.0f;   // min sampled trajectory cost (collision diagnosis)
+  float mean_cost = 0.0f;   // mean sampled trajectory cost from the final iteration
+  int sampled_rollouts = 0;
+  int valid_rollouts = 0;    // sampled trajectories with no collision-cost hit
+  float valid_rollout_ratio = 0.0f;
   bool all_colliding = false;
   bool retreating = false;   // true when command is a recovery back-out action
 };

python/core/include/cuda_mppi_controller/mppi_gpu.hpp

@@ -83,6 +83,10 @@ struct MppiResult
   float vy = 0.0f;
   float w = 0.0f;
   float best_cost = 0.0f;   // min sampled trajectory cost (collision diagnosis)
+  float mean_cost = 0.0f;   // mean sampled trajectory cost from the final iteration
+  int sampled_rollouts = 0;
+  int valid_rollouts = 0;    // sampled trajectories with no collision-cost hit
+  float valid_rollout_ratio = 0.0f;
   bool all_colliding = false;
   bool retreating = false;   // true when command is a recovery back-out action
 };

python/core/src/mppi_gpu.cu

@@ -707,7 +707,19 @@ MppiResult MppiGpu::computeInternal(
   }
 
   MppiResult res;
+  res.sampled_rollouts = K;
+  double cost_sum = 0.0;
+  int valid_rollouts = 0;
+  for (const float cost : im.h_costs) {
+    cost_sum += cost;
+    if (cost < mp.collision_cost) {
+      ++valid_rollouts;
+    }
+  }
   res.best_cost = min_cost;
+  res.mean_cost = static_cast<float>(cost_sum / static_cast<double>(K));
+  res.valid_rollouts = valid_rollouts;
+  res.valid_rollout_ratio = static_cast<float>(valid_rollouts) / static_cast<float>(K);
   res.all_colliding = min_cost >= mp.collision_cost;
 
   if (res.all_colliding) {

python/src/cudarobotics/bindings.cpp

@@ -422,6 +422,10 @@ class PyMppiPlanner
 
     nb::dict info;
     info["best_cost"] = result.best_cost;
+    info["mean_cost"] = result.mean_cost;
+    info["sampled_rollouts"] = result.sampled_rollouts;
+    info["valid_rollouts"] = result.valid_rollouts;
+    info["valid_rollout_ratio"] = result.valid_rollout_ratio;
     info["all_colliding"] = result.all_colliding;
     info["retreating"] = result.retreating;
     return nb::make_tuple(result.v, result.vy, result.w, info);
@@ -737,6 +741,10 @@ NB_MODULE(_cudarobotics, m)
     .def_rw("vy", &cr::MppiResult::vy)
     .def_rw("w", &cr::MppiResult::w)
     .def_rw("best_cost", &cr::MppiResult::best_cost)
+    .def_rw("mean_cost", &cr::MppiResult::mean_cost)
+    .def_rw("sampled_rollouts", &cr::MppiResult::sampled_rollouts)
+    .def_rw("valid_rollouts", &cr::MppiResult::valid_rollouts)
+    .def_rw("valid_rollout_ratio", &cr::MppiResult::valid_rollout_ratio)
     .def_rw("all_colliding", &cr::MppiResult::all_colliding)
     .def_rw("retreating", &cr::MppiResult::retreating);
 

python/tests/test_import.py

@@ -37,6 +37,15 @@ def test_mppi_planner_smoke():
     assert isinstance(vy, float)
     assert isinstance(w, float)
     assert isinstance(info, dict)
+    assert {
+        "best_cost",
+        "mean_cost",
+        "sampled_rollouts",
+        "valid_rollouts",
+        "valid_rollout_ratio",
+        "all_colliding",
+        "retreating",
+    }.issubset(info)
 
 
 def test_mppi_planner_cuda_dlpack_costmap_smoke():
@@ -67,6 +76,7 @@ def test_mppi_planner_cuda_dlpack_costmap_smoke():
     assert isinstance(vy, float)
     assert isinstance(w, float)
     assert isinstance(info, dict)
+    assert "valid_rollout_ratio" in info
 
 
 @pytest.mark.parametrize(

ros2_ws/src/cuda_mppi_controller/README.md

@@ -174,12 +174,20 @@ controller_server:
 | `yaw_goal_activation_dist` | 0.5 | [m] range to enable the yaw goal cost |
 | `lookahead_dist` | 3.0 | [m] global plan window fed to the GPU |
 | `transform_tolerance` | 0.1 | [s] TF lookup tolerance |
+| `diagnostics_log_period` | 0.0 | [s] periodic one-line solve/valid-rollout logging; 0 disables |
+| `diagnostics_csv_path` | `""` | optional per-cycle diagnostics CSV path |
 
 Parameters above are validated at configure time and during live ROS parameter
 updates. Invalid values, such as zero horizon length, non-positive model step,
 unknown motion models, or negative cost weights, are rejected before the GPU
 optimizer is rebuilt.
 
+Set `diagnostics_log_period` to a positive value for throttled controller logs,
+or set `diagnostics_csv_path` to capture one row per control cycle. The CSV
+includes solve time, best/mean rollout cost, valid rollout count and ratio,
+all-colliding/retreat flags, path window size, costmap size, and the selected
+command.
+
 ## Benchmark scenarios
 
 `controller_benchmark` runs closed-loop CPU vs GPU comparisons on synthetic maps:
@@ -188,12 +196,21 @@ optimizer is rebuilt.
 ros2 run cuda_mppi_controller controller_benchmark /tmp/bench wall_gap
 ros2 run cuda_mppi_controller controller_benchmark /tmp/bench narrow_corridor
 ros2 run cuda_mppi_controller controller_benchmark /tmp/bench u_turn
+ros2 run cuda_mppi_controller controller_benchmark /tmp/bench double_gap
+ros2 run cuda_mppi_controller controller_benchmark /tmp/bench moving_crossing quick
 ros2 run cuda_mppi_controller controller_benchmark /tmp/bench all
+ros2 run cuda_mppi_controller controller_benchmark /tmp/bench double_gap quick
+ros2 run cuda_mppi_controller controller_benchmark /tmp/bench double_gap cpu_gpu
 ros2 run cuda_mppi_controller controller_benchmark /tmp/bench esdf
 ros2 run cuda_mppi_controller controller_benchmark /tmp/bench path_angle
 ros2 run cuda_mppi_controller controller_benchmark /tmp/bench curvature_speed
 ```
 
+The optional preset is `full` by default. Use `quick` for GPU K=2,048/8,192
+smoke runs, or `cpu_gpu` for CPU K=2,000 vs GPU K=8,192. The `esdf`,
+`path_angle`, and `curvature_speed` benchmark families keep their fixed
+comparison sets.
+
 `all` also runs Ackermann/Omni GPU configs (`gpu_ackermann_K8192`, `gpu_omni_K8192`).
 `esdf` runs a GPU-only comparison of the default costmap critic against the
 optional distance-field clearance critic. Results:

ros2_ws/src/cuda_mppi_controller/config/cuda_mppi_params.example.yaml

@@ -42,3 +42,5 @@ controller_server:
       yaw_goal_activation_dist: 0.5
       lookahead_dist: 3.0       # [m] global plan window fed to the GPU
       transform_tolerance: 0.1
+      diagnostics_log_period: 0.0 # [s] 0 disables periodic solve/valid-rollout logs
+      diagnostics_csv_path: ""    # optional CSV trace path for per-cycle diagnostics

ros2_ws/src/cuda_mppi_controller/include/cuda_mppi_controller/cuda_mppi_controller.hpp

@@ -2,6 +2,7 @@
 #define CUDA_MPPI_CONTROLLER__CUDA_MPPI_CONTROLLER_HPP_
 
 #include <memory>
+#include <fstream>
 #include <string>
 #include <vector>
 
@@ -48,13 +49,24 @@ class CudaMppiController : public nav2_core::Controller
   void reset();
 
 private:
+  struct DiagnosticsCsv
+  {
+    std::ofstream file;
+    bool enabled = false;
+  };
+
   // Extract the local window of the global plan around the robot, transformed
   // into the costmap global frame. Returns flattened [x0,y0,x1,y1,...] points;
   // sets goal pose (window end) and whether it is the true final goal.
   std::vector<float> extractLocalPath(
     const geometry_msgs::msg::PoseStamped & robot_pose,
     float & goal_x, float & goal_y, float & goal_yaw, bool & goal_is_final);
 
+  DiagnosticsCsv openDiagnosticsCsv(const std::string & path) const;
+  void emitDiagnostics(
+    const MppiResult & result, double solve_ms, int path_points,
+    int costmap_size_x, int costmap_size_y);
+
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
   std::string name_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
@@ -67,6 +79,11 @@ class CudaMppiController : public nav2_core::Controller
 
   double lookahead_dist_ = 3.0;
   double transform_tolerance_ = 0.1;
+  double diagnostics_log_period_ = 0.0;
+  std::string diagnostics_csv_path_;
+  DiagnosticsCsv diagnostics_csv_;
+  rclcpp::Time last_diagnostics_log_time_{0, 0, RCL_ROS_TIME};
+  bool has_diagnostics_log_time_ = false;
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_;
   bool updateParamsFromNode(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
 };