fix

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

Author: PanConChicharron

planning/autoware_trajectory_optimizer/CMakeLists.txt

@@ -3,6 +3,7 @@ project(autoware_trajectory_optimizer)
 
 find_package(autoware_cmake REQUIRED)
 find_package(acados REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 autoware_package()
 
@@ -39,7 +40,11 @@ ament_auto_add_library(autoware_trajectory_optimizer_plugins SHARED
   src/trajectory_optimizer_plugins/plugin_utils/trajectory_spline_smoother_utils.cpp
   src/trajectory_optimizer_plugins/plugin_utils/trajectory_velocity_optimizer_utils.cpp
 )
-target_link_libraries(autoware_trajectory_optimizer_plugins acados_interface_temporal)
+target_link_libraries(
+  autoware_trajectory_optimizer_plugins
+  acados_interface_temporal
+  tf2_geometry_msgs::tf2_geometry_msgs
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -70,6 +75,7 @@ install(FILES
 
 install(PROGRAMS
   scripts/temporal_mpt_debug_visualizer.py
+  scripts/temporal_mpt_python_reference.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

planning/autoware_trajectory_optimizer/config/plugins/trajectory_temporal_mpt_optimizer.param.yaml

@@ -1,15 +1,13 @@
 /**:
   ros__parameters:
     trajectory_temporal_mpt_optimizer:
-      # Acados temporal solver (SQP). If res_stat/res_eq stay ~O(1), check reference vs odometry
-      # alignment and steering reference (see TrajectoryTemporalMPTOptimizer).
+      # SQP. Horizon yref uses closest path index to ego + stage k (see TrajectoryTemporalMPTOptimizer).
       max_iter: 100
       tol: 0.001
-      # Bicycle geometry: should match vehicle_info (affects dynamics and inferred path curvature → δ_ref).
+      # Bicycle geometry: should match vehicle_info (dynamics only).
       lf: 1.0
       lr: 1.0
-      tau: 0.1
       min_points_for_optimization: 10
       output_points: 30
-      enable_debug_info: false
+      enable_debug_info: true
       publish_debug_topics: true

planning/autoware_trajectory_optimizer/config/trajectory_optimizer.param.yaml

@@ -1,14 +1,18 @@
 /**:
   ros__parameters:
-    # Plugin execution order - plugins will run in this order
+    # Plugin execution order — this list is the ONLY control for which plugins load and run.
+    # To disable temporal MPC entirely, omit TrajectoryTemporalMPTOptimizer from this list.
+    #
+    # use_temporal_mpt_optimizer only skips optimize_trajectory() when that plugin IS loaded; it
+    # does not unload the plugin or stop acados init. For a full off, remove from plugin_names.
     plugin_names:
-      # - "autoware::trajectory_optimizer::plugin::TrajectoryPointFixer"
-      # - "autoware::trajectory_optimizer::plugin::TrajectoryKinematicFeasibilityEnforcer"
-      # - "autoware::trajectory_optimizer::plugin::TrajectoryQPSmoother"
-      # - "autoware::trajectory_optimizer::plugin::TrajectoryKinematicFeasibilityEnforcer"
-      # - "autoware::trajectory_optimizer::plugin::TrajectoryVelocityOptimizer"
-      # - "autoware::trajectory_optimizer::plugin::TrajectorySplineSmoother"
-      - "autoware::trajectory_optimizer::plugin::TrajectoryTemporalMPTOptimizer"
+      - "autoware::trajectory_optimizer::plugin::TrajectoryPointFixer"
+      - "autoware::trajectory_optimizer::plugin::TrajectoryKinematicFeasibilityEnforcer"
+      - "autoware::trajectory_optimizer::plugin::TrajectoryQPSmoother"
+      - "autoware::trajectory_optimizer::plugin::TrajectoryKinematicFeasibilityEnforcer"
+      - "autoware::trajectory_optimizer::plugin::TrajectoryVelocityOptimizer"
+      - "autoware::trajectory_optimizer::plugin::TrajectorySplineSmoother"
+      # - "autoware::trajectory_optimizer::plugin::TrajectoryTemporalMPTOptimizer"
 
     # Plugin activation flags - control runtime enable/disable
     use_akima_spline_interpolation: true

planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_temporal_mpt_optimizer.hpp

@@ -39,7 +39,6 @@ struct TemporalMPTParams
   double tol{1.0e-3};
   double lf{1.0};
   double lr{1.0};
-  double tau{0.1};
   size_t min_points_for_optimization{10};
   size_t output_points{30};
   bool enable_debug_info{false};

planning/autoware_trajectory_optimizer/scripts/temporal_mpt_debug_visualizer.py

@@ -11,8 +11,11 @@
 Debug topics (under ``--topic-prefix``):
 
   input/reference_trajectory, input/initial_state, output/trajectory, output/solve_status
-  output/control_acceleration_mps2, output/control_delta_cmd_rad (Float64MultiArray, u = [a, delta_cmd];
+  output/control_acceleration_mps2, output/control_delta_cmd_rad (Float64MultiArray, u = [a, delta];
   also published when solve fails, from the solver's last iterate)
+
+With ``--compare-python``, also subscribes to the Python reference node under
+``<prefix>/python/output/...`` (same message types) for side-by-side comparison with C++.
 """
 
 # Copyright 2026 TIER IV, Inc.
@@ -97,10 +100,17 @@ class DebugFrame:
     control_delta_cmd: List[float] = field(default_factory=list)
     initial_xy_yaw: Optional[Tuple[float, float, float]] = None
     solve_status: Optional[int] = None
+    # Optional: temporal_mpt_python_reference.py replay (same inputs as C++ debug I/O)
+    python_output_xy: Optional[Tuple[List[float], List[float]]] = None
+    python_output_vel: List[float] = field(default_factory=list)
+    python_output_delta: List[float] = field(default_factory=list)
+    python_control_accel: List[float] = field(default_factory=list)
+    python_control_delta_cmd: List[float] = field(default_factory=list)
+    python_solve_status: Optional[int] = None
 
 
 class TemporalMptDebugVisualizer(Node):
-    def __init__(self, *, topic_prefix: str, update_hz: float) -> None:
+    def __init__(self, *, topic_prefix: str, update_hz: float, compare_python: bool) -> None:
         super().__init__("temporal_mpt_debug_visualizer")
 
         update_hz = max(update_hz, 1.0)
@@ -146,6 +156,34 @@ def __init__(self, *, topic_prefix: str, update_hz: float) -> None:
             TEMPORAL_MPT_DEBUG_QOS,
         )
 
+        self._compare_python = compare_python
+        if compare_python:
+            py = f"{topic_prefix}/python"
+            self.create_subscription(
+                Trajectory,
+                f"{py}/output/trajectory",
+                self.on_python_output_trajectory,
+                TEMPORAL_MPT_DEBUG_QOS,
+            )
+            self.create_subscription(
+                Int32,
+                f"{py}/output/solve_status",
+                self.on_python_solve_status,
+                TEMPORAL_MPT_DEBUG_QOS,
+            )
+            self.create_subscription(
+                Float64MultiArray,
+                f"{py}/output/control_acceleration_mps2",
+                self.on_python_control_accel,
+                TEMPORAL_MPT_DEBUG_QOS,
+            )
+            self.create_subscription(
+                Float64MultiArray,
+                f"{py}/output/control_delta_cmd_rad",
+                self.on_python_control_delta_cmd,
+                TEMPORAL_MPT_DEBUG_QOS,
+            )
+
         self._fig = plt.figure(figsize=(14, 8))
         gs = gridspec.GridSpec(
             3, 2, figure=self._fig, width_ratios=[1.15, 1.0], wspace=0.28, hspace=0.35
@@ -162,6 +200,8 @@ def __init__(self, *, topic_prefix: str, update_hz: float) -> None:
 
         self.get_logger().info("Temporal MPT debug visualizer started.")
         self.get_logger().info(f"Listening under prefix: {topic_prefix}")
+        if compare_python:
+            self.get_logger().info(f"Python comparison enabled: {topic_prefix}/python/output/...")
         self.get_logger().info(
             "Subscriptions use BEST_EFFORT QoS (must match temporal MPT debug publishers)."
         )
@@ -228,6 +268,25 @@ def on_control_delta_cmd(self, msg: Float64MultiArray) -> None:
         with self._lock:
             self._frame.control_delta_cmd = list(msg.data)
 
+    def on_python_output_trajectory(self, msg: Trajectory) -> None:
+        with self._lock:
+            self._frame.python_output_xy = trajectory_to_xy(msg.points)
+            self._frame.python_output_vel, self._frame.python_output_delta = (
+                trajectory_velocity_steering(msg.points)
+            )
+
+    def on_python_solve_status(self, msg: Int32) -> None:
+        with self._lock:
+            self._frame.python_solve_status = msg.data
+
+    def on_python_control_accel(self, msg: Float64MultiArray) -> None:
+        with self._lock:
+            self._frame.python_control_accel = list(msg.data)
+
+    def on_python_control_delta_cmd(self, msg: Float64MultiArray) -> None:
+        with self._lock:
+            self._frame.python_control_delta_cmd = list(msg.data)
+
     def on_timer(self) -> None:
         with self._lock:
             frame = DebugFrame(
@@ -241,6 +300,12 @@ def on_timer(self) -> None:
                 control_delta_cmd=list(self._frame.control_delta_cmd),
                 initial_xy_yaw=self._frame.initial_xy_yaw,
                 solve_status=self._frame.solve_status,
+                python_output_xy=self._frame.python_output_xy,
+                python_output_vel=list(self._frame.python_output_vel),
+                python_output_delta=list(self._frame.python_output_delta),
+                python_control_accel=list(self._frame.python_control_accel),
+                python_control_delta_cmd=list(self._frame.python_control_delta_cmd),
+                python_solve_status=self._frame.python_solve_status,
             )
 
         # --- Left: XY path ---
@@ -261,7 +326,20 @@ def on_timer(self) -> None:
             )
         if frame.output_xy is not None and len(frame.output_xy[0]) > 0:
             self._ax_xy.plot(
-                frame.output_xy[0], frame.output_xy[1], "r-", linewidth=2, label="output optimized"
+                frame.output_xy[0],
+                frame.output_xy[1],
+                "r-",
+                linewidth=2,
+                label="output (C++)",
+            )
+        if frame.python_output_xy is not None and len(frame.python_output_xy[0]) > 0:
+            self._ax_xy.plot(
+                frame.python_output_xy[0],
+                frame.python_output_xy[1],
+                color="darkorange",
+                linestyle="--",
+                linewidth=2,
+                label="output (Python)",
             )
 
         if frame.initial_xy_yaw is not None:
@@ -278,23 +356,31 @@ def on_timer(self) -> None:
                 ec="k",
             )
 
-        if frame.solve_status is not None:
-            status_text = f"solve_status: {frame.solve_status}"
-            status_color = "green" if frame.solve_status == 0 else "red"
+        if frame.solve_status is not None or frame.python_solve_status is not None:
+            parts = []
+            if frame.solve_status is not None:
+                parts.append(f"C++ status: {frame.solve_status}")
+            if frame.python_solve_status is not None:
+                parts.append(f"Py status: {frame.python_solve_status}")
+            status_text = " | ".join(parts)
+            ok_cpp = frame.solve_status is None or frame.solve_status == 0
+            ok_py = frame.python_solve_status is None or frame.python_solve_status == 0
+            status_color = "green" if (ok_cpp and ok_py) else "red"
             self._ax_xy.text(
                 0.02,
                 0.98,
                 status_text,
                 transform=self._ax_xy.transAxes,
                 verticalalignment="top",
                 color=status_color,
-                fontsize=11,
+                fontsize=10,
                 bbox={"facecolor": "white", "alpha": 0.8, "edgecolor": status_color},
             )
 
         if (
             (frame.input_xy and len(frame.input_xy[0]) > 0)
             or (frame.output_xy and len(frame.output_xy[0]) > 0)
+            or (frame.python_output_xy and len(frame.python_output_xy[0]) > 0)
             or frame.initial_xy_yaw is not None
         ):
             self._ax_xy.relim()
@@ -317,8 +403,18 @@ def on_timer(self) -> None:
                 idx, frame.input_vel[:n_compare], "b.--", markersize=5, label="input ref v"
             )
             self._ax_v.plot(
-                idx, frame.output_vel[:n_compare], "r-", linewidth=2, label="output state v"
+                idx, frame.output_vel[:n_compare], "r-", linewidth=2, label="output v (C++)"
             )
+            if frame.python_output_vel:
+                n_py = min(n_compare, len(frame.python_output_vel))
+                self._ax_v.plot(
+                    list(range(n_py)),
+                    frame.python_output_vel[:n_py],
+                    color="darkorange",
+                    linestyle="--",
+                    linewidth=2,
+                    label="output v (Python)",
+                )
             self._ax_v.legend(loc="best")
         elif frame.input_vel:
             self._ax_v.plot(
@@ -351,8 +447,18 @@ def on_timer(self) -> None:
                 idx, frame.input_delta[:n_compare], "b.--", markersize=5, label="input ref δ"
             )
             self._ax_delta.plot(
-                idx, frame.output_delta[:n_compare], "r-", linewidth=2, label="output state δ"
+                idx, frame.output_delta[:n_compare], "r-", linewidth=2, label="output δ (C++)"
             )
+            if frame.python_output_delta:
+                n_py = min(n_compare, len(frame.python_output_delta))
+                self._ax_delta.plot(
+                    list(range(n_py)),
+                    frame.python_output_delta[:n_py],
+                    color="darkorange",
+                    linestyle="--",
+                    linewidth=2,
+                    label="output δ (Python)",
+                )
             self._ax_delta.legend(loc="best")
         elif frame.input_delta:
             self._ax_delta.plot(
@@ -389,9 +495,20 @@ def on_timer(self) -> None:
             self._ax_u.set_ylabel("a [m/s²]", color="tab:green")
             if na >= n_stages:
                 self._ax_u.plot(
-                    stages, frame.control_accel[:n_stages], "g-", linewidth=1.5, label="a (u₀)"
+                    stages, frame.control_accel[:n_stages], "g-", linewidth=1.5, label="a C++"
                 )
             self._ax_u.tick_params(axis="y", labelcolor="tab:green")
+            npa = len(frame.python_control_accel)
+            if npa > 0:
+                n_ap = min(n_stages, npa)
+                self._ax_u.plot(
+                    stages[:n_ap],
+                    frame.python_control_accel[:n_ap],
+                    color="darkgoldenrod",
+                    linestyle="--",
+                    linewidth=1.5,
+                    label="a Python",
+                )
 
             self._ax_u_twin.set_ylabel("δ_cmd [rad]", color="tab:purple")
             if nd >= n_stages:
@@ -401,9 +518,20 @@ def on_timer(self) -> None:
                     color="tab:purple",
                     linestyle="-",
                     linewidth=1.5,
-                    label="δ_cmd (u₁)",
+                    label="δ_cmd C++",
                 )
             self._ax_u_twin.tick_params(axis="y", labelcolor="tab:purple")
+            npd = len(frame.python_control_delta_cmd)
+            if npd > 0:
+                n_dp = min(n_stages, npd)
+                self._ax_u_twin.plot(
+                    stages[:n_dp],
+                    frame.python_control_delta_cmd[:n_dp],
+                    color="darkorange",
+                    linestyle="--",
+                    linewidth=1.5,
+                    label="δ_cmd Python",
+                )
 
             lines = self._ax_u.get_lines() + self._ax_u_twin.get_lines()
             labels = [ln.get_label() for ln in lines]
@@ -449,6 +577,12 @@ def parse_args(argv: list[str]) -> argparse.Namespace:
         default=10.0,
         help="Matplotlib refresh rate",
     )
+    parser.add_argument(
+        "--compare-python",
+        action=argparse.BooleanOptionalAction,
+        default=True,
+        help="Also subscribe to <prefix>/python/output/... from temporal_mpt_python_reference.py",
+    )
     return parser.parse_args(argv)
 
 
@@ -460,6 +594,7 @@ def main() -> None:
     node = TemporalMptDebugVisualizer(
         topic_prefix=cli.topic_prefix.rstrip("/"),
         update_hz=cli.update_hz,
+        compare_python=cli.compare_python,
     )
     try:
         while rclpy.ok():