feat(autoware_diffusion_planner): gradual drift and deviation on turns and reducing jitter (#3239)

* feat(mppi): fix for gradual drift

Co-authored-by: Cursor <cursoragent@cursor.com>

* using coloured noise; changed reference to be time-based tracking; param updates

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

* param update; removed unecessary projections

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

* new tunings after test

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

* fix to plots

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

* fixes

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

* updates to debug

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

* changes for adding lateral error to cost structure

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

* fixes

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

* fix

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

---------

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>
Co-authored-by: Cursor <cursoragent@cursor.com>

Author: PanConChicharron

planning/autoware_diffusion_planner/include/autoware/diffusion_planner/diffusion_planner_node.hpp

@@ -44,6 +44,7 @@
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <autoware_vehicle_msgs/msg/steering_report.hpp>
 #include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/float32_multi_array.hpp>
 #include <std_msgs/msg/float64.hpp>
 #include <std_srvs/srv/set_bool.hpp>
@@ -180,6 +181,9 @@ class DiffusionPlanner : public rclcpp::Node
     const autoware::mppi_optimizer::FirstOrderDubinsMppiDebug & debug, const std::string & frame_id,
     const rclcpp::Time & stamp);
 
+  /** Publish whether MPPI is currently applied to the output trajectory. */
+  void publish_mppi_enabled(bool enabled);
+
   /**
    * @brief Publish guidance triggered status as a debug message.
    * @param guidance_triggered Map of guidance name to triggered flags per batch.
@@ -232,6 +236,7 @@ class DiffusionPlanner : public rclcpp::Node
   rclcpp::Publisher<Trajectory>::SharedPtr pub_mppi_reference_trajectory_{nullptr};
   rclcpp::Publisher<Trajectory>::SharedPtr pub_mppi_optimized_trajectory_{nullptr};
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_mppi_markers_{nullptr};
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_mppi_enabled_{nullptr};
   rclcpp::Publisher<CandidateTrajectories>::SharedPtr pub_trajectories_{nullptr};
   rclcpp::Publisher<PredictedObjects>::SharedPtr pub_objects_{nullptr};
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_lane_marker_{nullptr};

planning/autoware_diffusion_planner/src/diffusion_planner_node.cpp

@@ -92,6 +92,9 @@ DiffusionPlanner::DiffusionPlanner(const rclcpp::NodeOptions & options)
   pub_mppi_optimized_trajectory_ =
     this->create_publisher<Trajectory>("~/debug/mppi/optimized_trajectory", 1);
   pub_mppi_markers_ = this->create_publisher<MarkerArray>("~/debug/mppi/markers", 1);
+  // Latched so late-joining debug tools see the current enable state immediately.
+  pub_mppi_enabled_ = this->create_publisher<std_msgs::msg::Bool>(
+    "~/debug/mppi/enabled", rclcpp::QoS{1}.transient_local());
   pub_trajectories_ = this->create_publisher<CandidateTrajectories>("~/output/trajectories", 1);
   pub_objects_ =
     this->create_publisher<PredictedObjects>("~/output/predicted_objects", rclcpp::QoS(1));
@@ -116,6 +119,7 @@ DiffusionPlanner::DiffusionPlanner(const rclcpp::NodeOptions & options)
     "~/debug/guidance_status", 1);
 
   set_up_params();
+  publish_mppi_enabled(params_.use_mppi_optimizer && !params_.shadow_mode);
   vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
 
   // Create core instance
@@ -394,6 +398,7 @@ SetParametersResult DiffusionPlanner::on_parameter(
       temp_params.line_string_max_step_m != previous_line_string_max_step_m;
     params_ = temp_params;
     core_->update_params(params_);
+    publish_mppi_enabled(params_.use_mppi_optimizer && !params_.shadow_mode);
 
     if (
       args_path_changed || model_paths_changed || batch_size_changed || dpm_solver_steps_changed ||
@@ -683,9 +688,11 @@ void DiffusionPlanner::on_timer()
           driving_along_targets, frame_context->ego_kinematic_state.pose.pose.position.z));
       record_section_time(
         *stop_watch_ptr_, "mppi_optimizer/optimize_trajectory", *diagnostics_inference_);
-      if (!params_.shadow_mode) {
+      const bool apply_mppi = !params_.shadow_mode;
+      if (apply_mppi) {
         planner_output.trajectory = mppi_result.trajectory;
       }
+      publish_mppi_enabled(apply_mppi);
 
       autoware_utils_debug::ScopedTimeTrack publish_debug_st(
         "mppi_optimizer/publish_debug", *time_keeper_);
@@ -698,12 +705,15 @@ void DiffusionPlanner::on_timer()
       record_section_time(
         *stop_watch_ptr_, "mppi_optimizer/publish_debug", *diagnostics_inference_);
     } catch (const std::runtime_error & e) {
+      publish_mppi_enabled(false);
       RCLCPP_ERROR_STREAM(get_logger(), "MPPI optimization failed: " << e.what());
       diagnostics_inference_->update_level_and_message(DiagnosticStatus::ERROR, e.what());
       diagnostics_inference_->publish(frame_time);
       return;
     }
     record_section_time(*stop_watch_ptr_, "mppi_optimizer", *diagnostics_inference_);
+  } else {
+    publish_mppi_enabled(false);
   }
 
   publish_guidance_status(planner_output.guidance_triggered, frame_time);
@@ -765,6 +775,13 @@ void DiffusionPlanner::publish_guidance_status(
   pub_guidance_status_->publish(msg);
 }
 
+void DiffusionPlanner::publish_mppi_enabled(bool enabled)
+{
+  std_msgs::msg::Bool msg;
+  msg.data = enabled;
+  pub_mppi_enabled_->publish(msg);
+}
+
 void DiffusionPlanner::publish_mppi_debug(
   const autoware::mppi_optimizer::FirstOrderDubinsMppiDebug & debug, const std::string & frame_id,
   const rclcpp::Time & stamp)

planning/autoware_mppi_optimizer/CMakeLists.txt

@@ -85,6 +85,7 @@ if(CUDA_AVAIL AND TRT_AVAIL)
     autoware_vehicle_msgs::autoware_vehicle_msgs__rosidl_generator_cpp
     geometry_msgs::geometry_msgs__rosidl_generator_cpp
     nav_msgs::nav_msgs__rosidl_generator_cpp
+    ${CUDA_cufft_LIBRARY}
   )
   set_target_properties(first_order_dubins_mppi PROPERTIES
     POSITION_INDEPENDENT_CODE ON

planning/autoware_mppi_optimizer/README.md

@@ -69,6 +69,7 @@ Various features can be disabled by changing the following parameters set in `mp
 ignore_obstacles: true
 ignore_drivable_area: true
 force_cold_start_each_step: true
+use_last_control_as_nominal: true
 ```
 
 Then rebuild / restart the diffusion planner and compare live MPPI to offline retune.
@@ -79,7 +80,9 @@ Notes:
 - `ignore_drivable_area` is retained as an ablation flag; on this stack boundary crash is already
   disabled in the cost (`isEgoOutsideDrivableArea` always false).
 - `force_cold_start_each_step` only resets tracking counters / arc-length (control is already
-  re-seeded from the reference via `updateImportanceSampler(u_nom)` each cycle).
+  re-seeded via `updateImportanceSampler(u_nom)` each cycle).
+- `use_last_control_as_nominal` warm-starts `u_nom` from the shifted previous optimized control
+  sequence when available; otherwise (and on cold start) reseeds from the diffusion reference.
 
 ### Replay only
 

planning/autoware_mppi_optimizer/config/mppi_optimizer.param.yaml

@@ -2,27 +2,26 @@
   ros__parameters:
     # Vehicle actuation dynamics are loaded from $(vehicle_model)_description/config/simulator_model.param.yaml
 
-    # MPPI controller / FirstOrderDubinsBicycleCostParams
-    # (see first_order_dubins_mppi_interface.cu, first_order_dubins_bicycle_cost.cuh)
-    lambda: 3000.0
+    # FirstOrderDubinsBicycleCostParams (see first_order_dubins_bicycle_cost.cuh)
+    lambda: 100.0
     desired_speed: 2.5
-    speed_coeff: 500.0
+    speed_coeff: 0.0
     track_coeff: 1000.0
-    heading_coeff: 500.0
-    crash_coeff: 100000.0
+    track_terminal_scale: 10.0
+    heading_coeff: 100.0
+    # Spatial (closest-segment) costs restored from pre time-indexed tracking; 0 = off.
+    lateral_distance_coeff: 0.0
+    lateral_yaw_error_coeff: 0.0
+    crash_coeff: 10000.0
     boundary_threshold: 0.8
     boundary_threshold_left: -1.0
     boundary_threshold_right: -1.0
-    lateral_acceleration_coeff: 500.0
-    lateral_jerk_coeff: 3000.0
-    longitudinal_jerk_coeff: 1000.0
-    accel_cmd_coeff: 200.0
-    steer_cmd_coeff: 1000.0
-    steer_rate_coeff: 5000.0
-    goal_pos_coeff: 1000.0
-    goal_speed_coeff: 200.0
-    goal_yaw_coeff: 500.0
-    goal_terminal_scale: 10.0
+    lateral_acceleration_coeff: 0.0
+    lateral_jerk_coeff: 150.0
+    longitudinal_jerk_coeff: 150.0
+    accel_cmd_coeff: 50.0
+    steer_cmd_coeff: 0.0
+    steer_rate_coeff: 0.0
     obstacle_collision_margin: 0.2
     road_border_collision_margin: 0.2
     drivable_area_crossing_coeff: 500.0
@@ -37,3 +36,5 @@
     ignore_drivable_area: true
     force_cold_start_each_step: false
     skip_if_invalid: true  # if the MPPI output is invalid (i.e., collides with obstacles or road borders), the input reference is returned
+    # Warm-start u_nom from shifted previous optimized controls (else reseed from DP each cycle).
+    use_last_control_as_nominal: true

planning/autoware_mppi_optimizer/include/autoware/mppi_optimizer/first_order_dubins_mppi_cost_params.hpp

@@ -27,24 +27,24 @@ struct FirstOrderDubinsMppiCostParams
   float desired_speed{3.0F};
   float speed_coeff{500.0F};
   float track_coeff{1000.0F};
+  float track_terminal_scale{10.0F};
   float heading_coeff{500.0F};
+  float lateral_distance_coeff{0.0F};
+  float lateral_yaw_error_coeff{0.0F};
   float crash_coeff{100000.0F};
   float boundary_threshold{1.5F};
   float boundary_threshold_left{-1.0F};
   float boundary_threshold_right{-1.0F};
   float accel_cmd_coeff{0.0F};
   float steer_cmd_coeff{0.0F};
+
   float steer_rate_coeff{0.0F};
   float lateral_acceleration_coeff{300.0F};
   float lateral_jerk_coeff{300.0F};
   float longitudinal_jerk_coeff{10.0F};
   float obstacle_collision_margin{0.5F};
   float road_border_collision_margin{0.2F};
   float drivable_area_crossing_coeff{10000.0F};
-  float goal_pos_coeff{1000.0F};
-  float goal_speed_coeff{0.0F};
-  float goal_yaw_coeff{500.0F};
-  float goal_terminal_scale{10.0F};
 };
 
 }  // namespace autoware::mppi_optimizer

planning/autoware_mppi_optimizer/include/autoware/mppi_optimizer/first_order_dubins_mppi_interface.hpp

@@ -121,10 +121,14 @@ class FirstOrderDubinsMppiInterface
 
   /**
    * @brief Ablation options to mirror mppi_offline_retune conditions in online sim.
+   * @param use_last_control_as_nominal When true and a previous optimized control sequence
+   *        exists, seed u_nom by shifting that sequence (warm start) instead of reseeding
+   *        from the diffusion reference every cycle.
    */
   void setAblationOptions(
     const bool ignore_obstacles, const bool ignore_drivable_area,
-    const bool force_cold_start_each_step, const bool skip_if_invalid);
+    const bool force_cold_start_each_step, const bool skip_if_invalid,
+    bool use_last_control_as_nominal = false);
 
   /**
    * @brief Copy per-rollout raw costs and normalized importance weights from the last
@@ -134,20 +138,26 @@ class FirstOrderDubinsMppiInterface
   bool copySampleCostDistribution(
     std::vector<float> & raw_costs, std::vector<float> & normalized_weights, int stride = 1) const;
 
+  /**
+   * @brief When true, optimizeTrajectory fills debug.rollouts with top-K weighted samples
+   *        (CPU replay; ~tens of ms). Enable only for offline retune — leave false for online
+   *        planning and debug trajectory logging.
+   */
+  void setRolloutVisualizationEnabled(bool enable);
+
   /**
    * @brief Run one MPPI control step and propagate the vehicle state forward.
    * @param state Current ego state (updated in place).
-   * @param arc_length Current arc length along the reference path (updated in place).
    * @param sim_time Current simulation time [s].
    */
-  FirstOrderDubinsMppiControl computeStep(
-    FirstOrderDubinsMppiState & state, float & arc_length, float sim_time);
+  FirstOrderDubinsMppiControl computeStep(FirstOrderDubinsMppiState & state, float sim_time);
 
   /**
    * @brief Track a diffusion-planner reference (poses + velocities) with one MPPI step.
    *
    * Uses the diffusion trajectory directly as the MPPI reference horizon (x, y, yaw, v),
-   * seeds u_nom from the reference trajectory controls each cycle, and returns the MPPI-predicted
+   * seeds u_nom from the previous optimized controls when use_last_control_as_nominal is set
+   * (otherwise from the reference trajectory), and returns the MPPI-predicted
    * feasible state rollout that best tracks that reference.
    *
    * @param input Reference trajectory from the diffusion planner (map frame).

planning/autoware_mppi_optimizer/include/autoware/mppi_optimizer/first_order_dubins_mppi_runtime_options.hpp

@@ -30,6 +30,8 @@ struct FirstOrderDubinsMppiRuntimeOptions
   bool ignore_drivable_area{false};
   bool force_cold_start_each_step{false};
   bool skip_if_invalid{false};
+  /** Warm-start u_nom from shifted previous optimized controls (else reseed from DP each cycle). */
+  bool use_last_control_as_nominal{false};
 };
 
 }  // namespace autoware::mppi_optimizer

planning/autoware_mppi_optimizer/include/autoware/mppi_optimizer/mppi_debug_trajectory_io.hpp

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__MPPI_OPTIMIZER__MPPI_DEBUG_TRAJECTORY_IO_HPP_
 #define AUTOWARE__MPPI_OPTIMIZER__MPPI_DEBUG_TRAJECTORY_IO_HPP_
 
+#include "autoware/mppi_optimizer/first_order_dubins_mppi_interface.hpp"
 #include "autoware/mppi_optimizer/mppi_debug_trajectory_logger.hpp"
 
 #include <autoware_planning_msgs/msg/trajectory.hpp>
@@ -90,6 +91,25 @@ inline bool writeMppiDebugCostsCsv(
   return true;
 }
 
+inline bool writeMppiDebugRolloutsCsv(
+  const std::string & path, const std::vector<FirstOrderDubinsMppiRollout> & rollouts)
+{
+  std::ofstream out(path);
+  if (!out) {
+    return false;
+  }
+  out << "rollout_index,cost,step,x,y\n";
+  out << std::setprecision(9) << std::fixed;
+  for (size_t r = 0; r < rollouts.size(); ++r) {
+    const auto & rollout = rollouts[r];
+    for (size_t s = 0; s < rollout.points.size(); ++s) {
+      out << r << "," << rollout.cost << "," << s << "," << rollout.points[s].first << ","
+          << rollout.points[s].second << "\n";
+    }
+  }
+  return true;
+}
+
 inline bool loadMppiDebugTrajectoryCsv(
   const std::string & path, autoware_planning_msgs::msg::Trajectory & trajectory)
 {

planning/autoware_mppi_optimizer/include/autoware/mppi_optimizer/mppi_debug_trajectory_logger.hpp

@@ -131,7 +131,10 @@ class MppiDebugTrajectoryLogger
         out << "desired_speed," << cost.desired_speed << "\n";
         out << "speed_coeff," << cost.speed_coeff << "\n";
         out << "track_coeff," << cost.track_coeff << "\n";
+        out << "track_terminal_scale," << cost.track_terminal_scale << "\n";
         out << "heading_coeff," << cost.heading_coeff << "\n";
+        out << "lateral_distance_coeff," << cost.lateral_distance_coeff << "\n";
+        out << "lateral_yaw_error_coeff," << cost.lateral_yaw_error_coeff << "\n";
         out << "crash_coeff," << cost.crash_coeff << "\n";
         out << "boundary_threshold," << cost.boundary_threshold << "\n";
         out << "boundary_threshold_left," << cost.boundary_threshold_left << "\n";
@@ -143,10 +146,6 @@ class MppiDebugTrajectoryLogger
         out << "lateral_jerk_coeff," << cost.lateral_jerk_coeff << "\n";
         out << "longitudinal_jerk_coeff," << cost.longitudinal_jerk_coeff << "\n";
         out << "obstacle_collision_margin," << cost.obstacle_collision_margin << "\n";
-        out << "goal_pos_coeff," << cost.goal_pos_coeff << "\n";
-        out << "goal_speed_coeff," << cost.goal_speed_coeff << "\n";
-        out << "goal_yaw_coeff," << cost.goal_yaw_coeff << "\n";
-        out << "goal_terminal_scale," << cost.goal_terminal_scale << "\n";
       }
     }
     {