diff --git a/include/cuda_mppi_controller/mppi_gpu.hpp b/include/cuda_mppi_controller/mppi_gpu.hpp index 4f635148..3cfc571d 100644 --- a/include/cuda_mppi_controller/mppi_gpu.hpp +++ b/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 }; diff --git a/python/core/include/cuda_mppi_controller/mppi_gpu.hpp b/python/core/include/cuda_mppi_controller/mppi_gpu.hpp index 4f635148..3cfc571d 100644 --- a/python/core/include/cuda_mppi_controller/mppi_gpu.hpp +++ b/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 }; diff --git a/python/core/src/mppi_gpu.cu b/python/core/src/mppi_gpu.cu index 046ee5dc..5439122b 100644 --- a/python/core/src/mppi_gpu.cu +++ b/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(cost_sum / static_cast(K)); + res.valid_rollouts = valid_rollouts; + res.valid_rollout_ratio = static_cast(valid_rollouts) / static_cast(K); res.all_colliding = min_cost >= mp.collision_cost; if (res.all_colliding) { diff --git a/python/src/cudarobotics/bindings.cpp b/python/src/cudarobotics/bindings.cpp index c68dd78e..e65719b2 100644 --- a/python/src/cudarobotics/bindings.cpp +++ b/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); diff --git a/python/tests/test_import.py b/python/tests/test_import.py index 18077796..43d0f8c9 100644 --- a/python/tests/test_import.py +++ b/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( diff --git a/ros2_ws/src/cuda_mppi_controller/README.md b/ros2_ws/src/cuda_mppi_controller/README.md index e9483132..d50f0d2f 100644 --- a/ros2_ws/src/cuda_mppi_controller/README.md +++ b/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: diff --git a/ros2_ws/src/cuda_mppi_controller/config/cuda_mppi_params.example.yaml b/ros2_ws/src/cuda_mppi_controller/config/cuda_mppi_params.example.yaml index 71574ef8..490c3fcc 100644 --- a/ros2_ws/src/cuda_mppi_controller/config/cuda_mppi_params.example.yaml +++ b/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 diff --git a/ros2_ws/src/cuda_mppi_controller/include/cuda_mppi_controller/cuda_mppi_controller.hpp b/ros2_ws/src/cuda_mppi_controller/include/cuda_mppi_controller/cuda_mppi_controller.hpp index 6b9c5a10..21d9c532 100644 --- a/ros2_ws/src/cuda_mppi_controller/include/cuda_mppi_controller/cuda_mppi_controller.hpp +++ b/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 +#include #include #include @@ -48,6 +49,12 @@ 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. @@ -55,6 +62,11 @@ class CudaMppiController : public nav2_core::Controller 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 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); }; diff --git a/ros2_ws/src/cuda_mppi_controller/src/cuda_mppi_controller.cpp b/ros2_ws/src/cuda_mppi_controller/src/cuda_mppi_controller.cpp index 3798ac72..f63a318c 100644 --- a/ros2_ws/src/cuda_mppi_controller/src/cuda_mppi_controller.cpp +++ b/ros2_ws/src/cuda_mppi_controller/src/cuda_mppi_controller.cpp @@ -2,6 +2,7 @@ #include "cuda_mppi_controller/nav2_compat.hpp" #include +#include #include #include #include @@ -73,7 +74,7 @@ void requireNonNegative(const std::string & name, double value) } void validateControllerParams(const MppiParams & params, double lookahead_dist, - double transform_tolerance) + double transform_tolerance, double diagnostics_log_period) { requireParam(params.batch_size > 0, "batch_size", "must be greater than 0"); requireParam(params.time_steps > 0, "time_steps", "must be greater than 0"); @@ -114,11 +115,15 @@ void validateControllerParams(const MppiParams & params, double lookahead_dist, requirePositive("lookahead_dist", lookahead_dist); requireNonNegative("transform_tolerance", transform_tolerance); + requireNonNegative("diagnostics_log_period", diagnostics_log_period); } bool applyControllerParameter(const std::string & key, const rclcpp::Parameter & parameter, MppiParams & params, double & lookahead_dist, - double & transform_tolerance) + double & transform_tolerance, + double & diagnostics_log_period, + std::string & diagnostics_csv_path, + bool & optimizer_params_changed) { if (key == "batch_size") { params.batch_size = static_cast(parameter.as_int()); @@ -190,9 +195,16 @@ bool applyControllerParameter(const std::string & key, const rclcpp::Parameter & lookahead_dist = parameter.as_double(); } else if (key == "transform_tolerance") { transform_tolerance = parameter.as_double(); + } else if (key == "diagnostics_log_period") { + diagnostics_log_period = parameter.as_double(); + return true; + } else if (key == "diagnostics_csv_path") { + diagnostics_csv_path = parameter.as_string(); + return true; } else { return false; } + optimizer_params_changed = true; return true; } @@ -208,6 +220,8 @@ bool CudaMppiController::updateParamsFromNode( MppiParams next = params_; double next_lookahead_dist = lookahead_dist_; double next_transform_tolerance = transform_tolerance_; + double next_diagnostics_log_period = diagnostics_log_period_; + std::string next_diagnostics_csv_path = diagnostics_csv_path_; int batch_size = next.batch_size; int time_steps = next.time_steps; @@ -273,6 +287,8 @@ bool CudaMppiController::updateParamsFromNode( node->get_parameter(name_ + ".retreat_scale", retreat_scale); node->get_parameter(name_ + ".lookahead_dist", next_lookahead_dist); node->get_parameter(name_ + ".transform_tolerance", next_transform_tolerance); + node->get_parameter(name_ + ".diagnostics_log_period", next_diagnostics_log_period); + node->get_parameter(name_ + ".diagnostics_csv_path", next_diagnostics_csv_path); next.batch_size = batch_size; next.time_steps = time_steps; @@ -308,11 +324,14 @@ bool CudaMppiController::updateParamsFromNode( next.enable_retreat = enable_retreat; next.retreat_scale = static_cast(retreat_scale); - validateControllerParams(next, next_lookahead_dist, next_transform_tolerance); + validateControllerParams( + next, next_lookahead_dist, next_transform_tolerance, next_diagnostics_log_period); params_ = next; lookahead_dist_ = next_lookahead_dist; transform_tolerance_ = next_transform_tolerance; + diagnostics_log_period_ = next_diagnostics_log_period; + diagnostics_csv_path_ = next_diagnostics_csv_path; return true; } @@ -373,8 +392,11 @@ void CudaMppiController::configure( declare_param("retreat_scale", static_cast(params_.retreat_scale)); declare_param("lookahead_dist", lookahead_dist_); declare_param("transform_tolerance", transform_tolerance_); + declare_param("diagnostics_log_period", diagnostics_log_period_); + declare_param("diagnostics_csv_path", diagnostics_csv_path_); updateParamsFromNode(node); + diagnostics_csv_ = openDiagnosticsCsv(diagnostics_csv_path_); optimizer_ = std::make_unique(params_); @@ -386,30 +408,49 @@ void CudaMppiController::configure( MppiParams next_params = params_; double next_lookahead_dist = lookahead_dist_; double next_transform_tolerance = transform_tolerance_; + double next_diagnostics_log_period = diagnostics_log_period_; + std::string next_diagnostics_csv_path = diagnostics_csv_path_; bool changed = false; + bool optimizer_params_changed = false; for (const auto & parameter : parameters) { const std::string & full_name = parameter.get_name(); if (full_name.rfind(prefix, 0) != 0) { continue; } const std::string key = full_name.substr(prefix.size()); - changed = applyControllerParameter(key, parameter, next_params, next_lookahead_dist, - next_transform_tolerance) || + changed = applyControllerParameter( + key, parameter, next_params, next_lookahead_dist, next_transform_tolerance, + next_diagnostics_log_period, next_diagnostics_csv_path, optimizer_params_changed) || changed; } if (!changed) { return result; } try { - validateControllerParams(next_params, next_lookahead_dist, next_transform_tolerance); + validateControllerParams( + next_params, next_lookahead_dist, next_transform_tolerance, + next_diagnostics_log_period); std::unique_ptr next_optimizer; - if (optimizer_) { + if (optimizer_ && optimizer_params_changed) { next_optimizer = std::make_unique(next_params); } + DiagnosticsCsv next_diagnostics_csv; + const bool diagnostics_csv_changed = + next_diagnostics_csv_path != diagnostics_csv_path_; + if (diagnostics_csv_changed) { + next_diagnostics_csv = openDiagnosticsCsv(next_diagnostics_csv_path); + } params_ = next_params; lookahead_dist_ = next_lookahead_dist; transform_tolerance_ = next_transform_tolerance; - optimizer_ = std::move(next_optimizer); + diagnostics_log_period_ = next_diagnostics_log_period; + diagnostics_csv_path_ = next_diagnostics_csv_path; + if (optimizer_params_changed) { + optimizer_ = std::move(next_optimizer); + } + if (diagnostics_csv_changed) { + diagnostics_csv_ = std::move(next_diagnostics_csv); + } } catch (const std::exception & ex) { result.successful = false; result.reason = ex.what(); @@ -427,6 +468,7 @@ void CudaMppiController::cleanup() { param_callback_.reset(); optimizer_.reset(); + diagnostics_csv_ = DiagnosticsCsv{}; } void CudaMppiController::activate() @@ -452,6 +494,71 @@ void CudaMppiController::reset() } } +CudaMppiController::DiagnosticsCsv CudaMppiController::openDiagnosticsCsv( + const std::string & path) const +{ + DiagnosticsCsv output; + if (path.empty()) { + return output; + } + + output.file.open(path, std::ios::out | std::ios::app); + if (!output.file.is_open()) { + throw std::runtime_error( + "CudaMppiController: failed to open diagnostics_csv_path '" + path + "'"); + } + output.enabled = true; + output.file + << "stamp_sec,solve_ms,best_cost,mean_cost,sampled_rollouts,valid_rollouts," + << "valid_rollout_ratio,all_colliding,retreating,path_points,costmap_size_x," + << "costmap_size_y,cmd_v,cmd_vy,cmd_w\n"; + return output; +} + +void CudaMppiController::emitDiagnostics( + const MppiResult & result, double solve_ms, int path_points, + int costmap_size_x, int costmap_size_y) +{ + const auto node = node_.lock(); + const rclcpp::Time now = node ? node->now() : rclcpp::Clock(RCL_ROS_TIME).now(); + + if (diagnostics_log_period_ > 0.0 && node) { + const bool due = + !has_diagnostics_log_time_ || + (now - last_diagnostics_log_time_).seconds() >= diagnostics_log_period_; + if (due) { + last_diagnostics_log_time_ = now; + has_diagnostics_log_time_ = true; + RCLCPP_INFO( + logger_, + "CUDA MPPI diagnostics: solve=%.2f ms valid=%d/%d (%.1f%%) best=%.3f mean=%.3f " + "retreat=%s cmd=(%.3f, %.3f, %.3f)", + solve_ms, result.valid_rollouts, result.sampled_rollouts, + 100.0 * result.valid_rollout_ratio, result.best_cost, result.mean_cost, + result.retreating ? "true" : "false", result.v, result.vy, result.w); + } + } + + if (diagnostics_csv_.enabled && diagnostics_csv_.file.is_open()) { + diagnostics_csv_.file + << now.seconds() << ',' + << solve_ms << ',' + << result.best_cost << ',' + << result.mean_cost << ',' + << result.sampled_rollouts << ',' + << result.valid_rollouts << ',' + << result.valid_rollout_ratio << ',' + << (result.all_colliding ? 1 : 0) << ',' + << (result.retreating ? 1 : 0) << ',' + << path_points << ',' + << costmap_size_x << ',' + << costmap_size_y << ',' + << result.v << ',' + << result.vy << ',' + << result.w << '\n'; + } +} + std::vector CudaMppiController::extractLocalPath( const geometry_msgs::msg::PoseStamped & robot_pose, float & goal_x, float & goal_y, float & goal_yaw, bool & goal_is_final) @@ -541,22 +648,32 @@ geometry_msgs::msg::TwistStamped CudaMppiController::computeVelocityCommands( nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); MppiResult result; + int costmap_size_x = 0; + int costmap_size_y = 0; + double solve_ms = 0.0; { std::unique_lock lock(*costmap->getMutex()); + costmap_size_x = static_cast(costmap->getSizeInCellsX()); + costmap_size_y = static_cast(costmap->getSizeInCellsY()); + const auto solve_start = std::chrono::steady_clock::now(); result = optimizer_->compute( static_cast(pose.pose.position.x), static_cast(pose.pose.position.y), static_cast(tf2::getYaw(pose.pose.orientation)), costmap->getCharMap(), - static_cast(costmap->getSizeInCellsX()), - static_cast(costmap->getSizeInCellsY()), + costmap_size_x, + costmap_size_y, static_cast(costmap->getOriginX()), static_cast(costmap->getOriginY()), static_cast(costmap->getResolution()), path_xy.data(), static_cast(path_xy.size() / 2), goal_x, goal_y, goal_yaw, goal_is_final, footprint_xy.data(), static_cast(footprint_xy.size() / 2)); + const auto solve_end = std::chrono::steady_clock::now(); + solve_ms = std::chrono::duration(solve_end - solve_start).count(); } + emitDiagnostics( + result, solve_ms, static_cast(path_xy.size() / 2), costmap_size_x, costmap_size_y); if (result.all_colliding && !result.retreating) { throw NoValidControl( diff --git a/ros2_ws/src/cuda_mppi_controller/test/controller_benchmark.cpp b/ros2_ws/src/cuda_mppi_controller/test/controller_benchmark.cpp index e6c77763..e5b6c046 100644 --- a/ros2_ws/src/cuda_mppi_controller/test/controller_benchmark.cpp +++ b/ros2_ws/src/cuda_mppi_controller/test/controller_benchmark.cpp @@ -3,10 +3,12 @@ // nav2's controller_server loads them, driving the same unicycle plant // through synthetic costmaps. // -// Usage: controller_benchmark [scenario] -// scenario: wall_gap | narrow_corridor | u_turn | all | esdf | path_angle | curvature_speed +// Usage: controller_benchmark [scenario] [preset] +// scenario: wall_gap | narrow_corridor | u_turn | double_gap | moving_crossing +// | all | esdf | path_angle | curvature_speed // (default: wall_gap) -// writes /summary.csv and /traj_