From 626b1df74902f10f44be0168559d7eb10643bceb Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Mon, 26 Aug 2024 16:16:51 +0900
Subject: [PATCH] po

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../pid_longitudinal_controller.hpp           |   6 +-
 .../src/pid_longitudinal_controller.cpp       |  12 +-
 .../plot_juggler_trajectory_follower.xml      | 249 ++++++++-------
 ...plot_juggler_trajectory_follower_accfb.xml | 298 ------------------
 4 files changed, 140 insertions(+), 425 deletions(-)
 delete mode 100644 control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower_accfb.xml

diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
index 95fa9904f4d2a..1d4192d51d98d 100644
--- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
+++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
@@ -169,7 +169,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 
   // drive
   PIDController m_pid_vel;
-  std::unique_ptr<LowpassFilter1d> m_lpf_vel_error{nullptr};
+  std::shared_ptr<LowpassFilter1d> m_lpf_vel_error{nullptr};
   bool m_enable_integration_at_low_speed;
   double m_current_vel_threshold_pid_integrate;
   double m_time_threshold_before_pid_integrate;
@@ -198,7 +198,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 
   // acc feedback
   double m_acc_feedback_gain;
-  std::unique_ptr<LowpassFilter1d> m_lpf_acc_error{nullptr};
+  std::shared_ptr<LowpassFilter1d> m_lpf_acc_error{nullptr};
 
   // acceleration limit
   double m_max_acc;
@@ -213,7 +213,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE };
   SlopeSource m_slope_source{SlopeSource::RAW_PITCH};
   double m_adaptive_trajectory_velocity_th;
-  std::unique_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
+  std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
   double m_max_pitch_rad;
   double m_min_pitch_rad;
 
diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
index 4f8ed3b7560c6..96e2796a91d22 100644
--- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -104,7 +104,7 @@ PidLongitudinalController::PidLongitudinalController(
 
     // set lowpass filter for vel error and pitch
     const double lpf_vel_error_gain{node.declare_parameter<double>("lpf_vel_error_gain")};
-    m_lpf_vel_error = std::make_unique<LowpassFilter1d>(0.0, lpf_vel_error_gain);
+    m_lpf_vel_error = std::make_shared<LowpassFilter1d>(0.0, lpf_vel_error_gain);
 
     m_enable_integration_at_low_speed =
       node.declare_parameter<bool>("enable_integration_at_low_speed");
@@ -167,7 +167,7 @@ PidLongitudinalController::PidLongitudinalController(
   // parameters for acc feedback
   {
     const double lpf_acc_error_gain{node.declare_parameter<double>("lpf_acc_error_gain")};
-    m_lpf_acc_error = std::make_unique<LowpassFilter1d>(0.0, lpf_acc_error_gain);
+    m_lpf_acc_error = std::make_shared<LowpassFilter1d>(0.0, lpf_acc_error_gain);
     m_acc_feedback_gain = node.declare_parameter<double>("acc_feedback_gain");
   }
 
@@ -184,7 +184,7 @@ PidLongitudinalController::PidLongitudinalController(
   m_adaptive_trajectory_velocity_th =
     node.declare_parameter<double>("adaptive_trajectory_velocity_th");  // [m/s^2]
   const double lpf_pitch_gain{node.declare_parameter<double>("lpf_pitch_gain")};
-  m_lpf_pitch = std::make_unique<LowpassFilter1d>(0.0, lpf_pitch_gain);
+  m_lpf_pitch = std::make_shared<LowpassFilter1d>(0.0, lpf_pitch_gain);
   m_max_pitch_rad = node.declare_parameter<double>("max_pitch_rad");  // [rad]
   m_min_pitch_rad = node.declare_parameter<double>("min_pitch_rad");  // [rad]
 
@@ -872,9 +872,9 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
     m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_FB_APPLIED, acc_cmd);
     RCLCPP_DEBUG(
       logger_,
-      "[acc feedback]: raw_ctrl_cmd.acc: %1.3f, control_data.current_motion.acc: %1.3f, "
-      "m_lpf_acc_error.getValue(): %1.3f, acc_cmd: %1.3f",
-      raw_ctrl_cmd.acc, control_data.current_motion.acc, 0.0, acc_cmd);
+      "[acc feedback]: raw_ctrl_cmd.acc: %1.3f, control_data.current_motion.acc: %1.3f, acc_cmd: "
+      "%1.3f",
+      raw_ctrl_cmd.acc, control_data.current_motion.acc, acc_cmd);
 
     ctrl_cmd_as_pedal_pos.acc =
       applySlopeCompensation(acc_cmd, control_data.slope_angle, control_data.shift);
diff --git a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml
index ab127c2d47b97..e458edaf6c471 100644
--- a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml
+++ b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml
@@ -1,130 +1,130 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root>
- <tabbed_widget name="Main Window" parent="main_window">
-  <Tab containers="1" tab_name="Lateral Controller">
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="Lateral Controller" containers="1">
    <Container>
-    <DockSplitter orientation="-" count="2" sizes="0.502237;0.497763">
-     <DockSplitter orientation="|" count="3" sizes="0.333333;0.333333;0.333333">
+    <DockSplitter sizes="0.50214;0.49786" count="2" orientation="-">
+     <DockSplitter sizes="0.333487;0.333025;0.333487" count="3" orientation="|">
       <DockArea name="Lateral Controller Info">
-       <plot mode="TimeSeries" style="Lines">
-        <range bottom="-1.152264" top="1.022526" right="118.796638" left="76.172500"/>
+       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+        <range right="698.352475" top="0.186116" bottom="-0.195346" left="612.164064"/>
         <limitY/>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[5]" color="#1f77b4">
          <transform alias="lateral error" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[8]" color="#d62728">
          <transform alias="yaw error" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[0]" color="#1ac938">
          <transform alias="steer cmd (final)" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[2]" color="#ff7f0e">
          <transform alias="steer cmd (FF-filtered)" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[3]" color="#f14cc1">
          <transform alias="steer cmd (FF-raw)" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[4]" color="#9467bd">
          <transform alias="steer measured" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[1]" color="#17becf">
          <transform alias="steer cmd (mpc-raw)" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
        </plot>
       </DockArea>
       <DockArea name="Velocity">
-       <plot mode="TimeSeries" style="Lines">
-        <range bottom="-0.069444" top="2.847222" right="118.796638" left="76.172500"/>
+       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+        <range right="698.352475" top="4.394836" bottom="-0.304341" left="612.164064"/>
         <limitY/>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[9]" color="#1f77b4">
          <transform alias="trajectory velocity" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[10]" color="#d62728">
          <transform alias="current velocity" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
        </plot>
       </DockArea>
       <DockArea name="path curvature">
-       <plot mode="TimeSeries" style="Lines">
-        <range bottom="-0.460804" top="0.984813" right="118.796638" left="76.172500"/>
+       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+        <range right="698.352475" top="0.038695" bottom="-0.007898" left="612.164064"/>
         <limitY/>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[14]" color="#d62728">
          <transform alias="smoothed" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[15]" color="#1f77b4">
          <transform alias="raw" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
        </plot>
       </DockArea>
      </DockSplitter>
-     <DockSplitter orientation="|" count="3" sizes="0.33211;0.33578;0.33211">
+     <DockSplitter sizes="0.332102;0.335797;0.332102" count="3" orientation="|">
       <DockArea name="angular-velocity converted from">
-       <plot mode="TimeSeries" style="Lines">
-        <range bottom="-0.045019" top="1.491571" right="118.796638" left="76.172500"/>
+       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+        <range right="698.352475" top="0.044071" bottom="-0.025022" left="612.164064"/>
         <limitY/>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[11]" color="#1ac938">
          <transform alias="steering cmd" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[12]" color="#ff7f0e">
          <transform alias="steering measured " name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[13]" color="#d62728">
          <transform alias="path curvature (smoothed)" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[17]" color="#1f77b4">
          <transform alias="steering predicted" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
        </plot>
       </DockArea>
       <DockArea name="yaw">
-       <plot mode="TimeSeries" style="Lines">
-        <range bottom="-3.295688" top="3.281428" right="118.796638" left="76.172500"/>
+       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+        <range right="698.352475" top="2.871953" bottom="2.734902" left="612.164064"/>
         <limitY/>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[7]" color="#1ac938">
          <transform alias="desired yaw" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
         <curve name="/control/trajectory_follower/lateral/diagnostic/data[6]" color="#ff7f0e">
          <transform alias="current yaw" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
+          <options value_offset="0" time_offset="0" value_scale="1.0"/>
          </transform>
         </curve>
        </plot>
       </DockArea>
       <DockArea name="control mode (1: Auto, 2: Manual, 3: Disengaged, 4: Not Ready)">
-       <plot mode="TimeSeries" style="Lines">
-        <range bottom="0.900000" top="1.100000" right="118.791728" left="76.166273"/>
+       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+        <range right="698.352475" top="1.100000" bottom="0.900000" left="612.164064"/>
         <limitY/>
         <curve name="/vehicle/status/control_mode/mode" color="#1f77b4"/>
        </plot>
@@ -133,100 +133,114 @@
     </DockSplitter>
    </Container>
   </Tab>
-  <Tab containers="1" tab_name="Longitudinal Controller">
+  <Tab tab_name="Longitudinal Controller" containers="1">
    <Container>
-    <DockSplitter orientation="-" count="2" sizes="0.502237;0.497763">
-     <DockSplitter orientation="|" count="2" sizes="0.498778;0.501222">
-      <DockArea name="Longitudinal Controller Info">
-       <plot mode="TimeSeries" style="Lines">
-        <range bottom="-46.710209" top="91.855136" right="109.929097" left="76.129089"/>
-        <limitY/>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[3]" color="#1ac938">
-         <transform alias="FF" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[14]" color="#ff7f0e">
-         <transform alias="FF+FB" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[19]" color="#f14cc1">
-         <transform alias="FB-P" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[20]" color="#9467bd">
-         <transform alias="FB-I" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[21]" color="#17becf">
-         <transform alias="FB-D" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[15]" color="#bcbd22">
-         <transform alias="acc filtered" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[16]" color="#1f77b4">
-         <transform alias="jerk filtered" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[17]" color="#d62728">
-         <transform alias="slope added" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[18]" color="#1ac938">
-         <transform alias="final" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[25]" color="#ff7f0e">
-         <transform alias="calculated" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-       </plot>
-      </DockArea>
-      <DockArea name="State">
-       <plot mode="TimeSeries" style="Lines">
-        <range bottom="-0.050000" top="2.050000" right="109.929097" left="76.129089"/>
-        <limitY/>
-        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[13]" color="#d62728">
-         <transform alias="0:Drive 1:Stopping 2:Stopped 3:Emergency" name="Scale/Offset">
-          <options value_scale="1.0" time_offset="0" value_offset="0"/>
-         </transform>
-        </curve>
-       </plot>
-      </DockArea>
-     </DockSplitter>
+    <DockSplitter sizes="0.3015;0.29045;0.278611;0.12944" count="4" orientation="-">
      <DockArea name="Velocity">
-      <plot mode="TimeSeries" style="Lines">
-       <range bottom="-0.070625" top="2.895625" right="109.929097" left="76.129089"/>
+      <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+       <range right="698.352475" top="4.401672" bottom="-0.413450" left="612.164064"/>
        <limitY/>
        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[2]" color="#17becf">
         <transform alias="target" name="Scale/Offset">
-         <options value_scale="1.0" time_offset="0" value_offset="0"/>
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
         </transform>
        </curve>
        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[1]" color="#9467bd">
         <transform alias="current" name="Scale/Offset">
-         <options value_scale="1.0" time_offset="0" value_offset="0"/>
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
         </transform>
        </curve>
        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[4]" color="#bcbd22">
         <transform alias="closest" name="Scale/Offset">
-         <options value_scale="1.0" time_offset="0" value_offset="0"/>
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
         </transform>
        </curve>
        <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[24]" color="#1f77b4">
         <transform alias="predicted" name="Scale/Offset">
-         <options value_scale="1.0" time_offset="0" value_offset="0"/>
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+     <DockArea name="Longitudinal Controller Info">
+      <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+       <range right="698.352475" top="1.211409" bottom="-3.512473" left="612.164064"/>
+       <limitY/>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[3]" color="#1ac938">
+        <transform alias="FF" name="Scale/Offset">
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[14]" color="#ff7f0e">
+        <transform alias="FF+FB" name="Scale/Offset">
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[19]" color="#f14cc1">
+        <transform alias="FB-P" name="Scale/Offset">
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[16]" color="#1f77b4">
+        <transform alias="jerk filtered" name="Scale/Offset">
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[17]" color="#d62728">
+        <transform alias="slope added" name="Scale/Offset">
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[18]" color="#004eff">
+        <transform alias="final" name="Scale/Offset">
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[25]" color="#ff7f0e">
+        <transform alias="calculated" name="Scale/Offset">
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+       <range right="698.352475" top="1.302476" bottom="-3.514695" left="612.164064"/>
+       <limitY/>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[25]" color="#d62728">
+        <transform alias="measured acc (1/0.1s+1)" name="Scale/Offset">
+         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[31]" color="#ff7f0e">
+        <transform alias="error acc " name="Scale/Offset">
+         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[32]" color="#f14cc1">
+        <transform alias="error acc filtered" name="Scale/Offset">
+         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[33]" color="#9467bd">
+        <transform alias="acc cmd output" name="Scale/Offset">
+         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
+        </transform>
+       </curve>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[16]" color="#17becf">
+        <transform alias="desired acc" name="Scale/Offset">
+         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
+        </transform>
+       </curve>
+      </plot>
+     </DockArea>
+     <DockArea name="State">
+      <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
+       <range right="698.352475" top="2.050000" bottom="-0.050000" left="612.164064"/>
+       <limitY/>
+       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[13]" color="#d62728">
+        <transform alias="0:Drive 1:Stopping 2:Stopped 3:Emergency" name="Scale/Offset">
+         <options value_offset="0" time_offset="0" value_scale="1.0"/>
         </transform>
        </curve>
       </plot>
@@ -234,17 +248,17 @@
     </DockSplitter>
    </Container>
   </Tab>
-  <currentTabIndex index="0"/>
+  <currentTabIndex index="1"/>
  </tabbed_widget>
  <use_relative_time_offset enabled="1"/>
  <!-- - - - - - - - - - - - - - - -->
  <!-- - - - - - - - - - - - - - - -->
  <Plugins>
   <plugin ID="DataLoad CSV">
-   <parameters time_axis="" delimiter="0"/>
+   <parameters delimiter="0" time_axis=""/>
   </plugin>
   <plugin ID="DataLoad MCAP"/>
-  <plugin ID="DataLoad ROS2 bags"> <!-- cspell:disable-line -->
+  <plugin ID="DataLoad ROS2 bags">
    <use_header_stamp value="false"/>
    <discard_large_arrays value="true"/>
    <max_array_size value="100"/>
@@ -254,13 +268,13 @@
   </plugin>
   <plugin ID="DataLoad ULog"/>
   <plugin ID="MQTT Subscriber (Mosquitto)"/>
-  <plugin ID="ROS2 Topic Subscriber"> <!-- cspell:disable-line -->
+  <plugin ID="ROS2 Topic Subscriber">
    <use_header_stamp value="false"/>
    <discard_large_arrays value="false"/>
    <max_array_size value="100"/>
    <boolean_strings_to_number value="false"/>
    <remove_suffix_from_strings value="false"/>
-   <selected_topics value="/control/trajectory_follower/longitudinal/diagnostic;/control/trajectory_follower/lateral/diagnostic;/vehicle/status/control_mode"/>
+   <selected_topics value="/control/trajectory_follower/lateral/diagnostic;/control/trajectory_follower/longitudinal/diagnostic;/localization/acceleration;/vehicle/status/control_mode"/>
   </plugin>
   <plugin ID="UDP Server"/>
   <plugin ID="WebSocket Server"/>
@@ -272,11 +286,10 @@
    <scripts/>
   </plugin>
   <plugin ID="CSV Exporter"/>
-  <plugin ID="ROS2 Topic Re-Publisher"/> <!-- cspell:disable-line -->
+  <plugin ID="ROS2 Topic Re-Publisher"/>
  </Plugins>
  <!-- - - - - - - - - - - - - - - -->
  <previouslyLoaded_Datafiles/>
- <previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/> <!-- cspell:disable-line -->
  <!-- - - - - - - - - - - - - - - -->
  <customMathEquations/>
  <snippets/>
diff --git a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower_accfb.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower_accfb.xml
deleted file mode 100644
index d5ab47a3c16fa..0000000000000
--- a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower_accfb.xml
+++ /dev/null
@@ -1,298 +0,0 @@
-<?xml version='1.0' encoding='UTF-8'?>
-<root>
- <tabbed_widget parent="main_window" name="Main Window">
-  <Tab tab_name="Lateral Controller" containers="1">
-   <Container>
-    <DockSplitter sizes="0.50214;0.49786" count="2" orientation="-">
-     <DockSplitter sizes="0.333487;0.333025;0.333487" count="3" orientation="|">
-      <DockArea name="Lateral Controller Info">
-       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-        <range right="698.352475" top="0.186116" bottom="-0.195346" left="612.164064"/>
-        <limitY/>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[5]" color="#1f77b4">
-         <transform alias="lateral error" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[8]" color="#d62728">
-         <transform alias="yaw error" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[0]" color="#1ac938">
-         <transform alias="steer cmd (final)" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[2]" color="#ff7f0e">
-         <transform alias="steer cmd (FF-filtered)" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[3]" color="#f14cc1">
-         <transform alias="steer cmd (FF-raw)" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[4]" color="#9467bd">
-         <transform alias="steer measured" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[1]" color="#17becf">
-         <transform alias="steer cmd (mpc-raw)" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-       </plot>
-      </DockArea>
-      <DockArea name="Velocity">
-       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-        <range right="698.352475" top="4.394836" bottom="-0.304341" left="612.164064"/>
-        <limitY/>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[9]" color="#1f77b4">
-         <transform alias="trajectory velocity" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[10]" color="#d62728">
-         <transform alias="current velocity" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-       </plot>
-      </DockArea>
-      <DockArea name="path curvature">
-       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-        <range right="698.352475" top="0.038695" bottom="-0.007898" left="612.164064"/>
-        <limitY/>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[14]" color="#d62728">
-         <transform alias="smoothed" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[15]" color="#1f77b4">
-         <transform alias="raw" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-       </plot>
-      </DockArea>
-     </DockSplitter>
-     <DockSplitter sizes="0.332102;0.335797;0.332102" count="3" orientation="|">
-      <DockArea name="angular-velocity converted from">
-       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-        <range right="698.352475" top="0.044071" bottom="-0.025022" left="612.164064"/>
-        <limitY/>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[11]" color="#1ac938">
-         <transform alias="steering cmd" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[12]" color="#ff7f0e">
-         <transform alias="steering measured " name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[13]" color="#d62728">
-         <transform alias="path curvature (smoothed)" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[17]" color="#1f77b4">
-         <transform alias="steering predicted" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-       </plot>
-      </DockArea>
-      <DockArea name="yaw">
-       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-        <range right="698.352475" top="2.871953" bottom="2.734902" left="612.164064"/>
-        <limitY/>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[7]" color="#1ac938">
-         <transform alias="desired yaw" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-        <curve name="/control/trajectory_follower/lateral/diagnostic/data[6]" color="#ff7f0e">
-         <transform alias="current yaw" name="Scale/Offset">
-          <options value_offset="0" time_offset="0" value_scale="1.0"/>
-         </transform>
-        </curve>
-       </plot>
-      </DockArea>
-      <DockArea name="control mode (1: Auto, 2: Manual, 3: Disengaged, 4: Not Ready)">
-       <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-        <range right="698.352475" top="1.100000" bottom="0.900000" left="612.164064"/>
-        <limitY/>
-        <curve name="/vehicle/status/control_mode/mode" color="#1f77b4"/>
-       </plot>
-      </DockArea>
-     </DockSplitter>
-    </DockSplitter>
-   </Container>
-  </Tab>
-  <Tab tab_name="Longitudinal Controller" containers="1">
-   <Container>
-    <DockSplitter sizes="0.3015;0.29045;0.278611;0.12944" count="4" orientation="-">
-     <DockArea name="Velocity">
-      <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-       <range right="698.352475" top="4.401672" bottom="-0.413450" left="612.164064"/>
-       <limitY/>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[2]" color="#17becf">
-        <transform alias="target" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[1]" color="#9467bd">
-        <transform alias="current" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[4]" color="#bcbd22">
-        <transform alias="closest" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[24]" color="#1f77b4">
-        <transform alias="predicted" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-      </plot>
-     </DockArea>
-     <DockArea name="Longitudinal Controller Info">
-      <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-       <range right="698.352475" top="1.211409" bottom="-3.512473" left="612.164064"/>
-       <limitY/>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[3]" color="#1ac938">
-        <transform alias="FF" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[14]" color="#ff7f0e">
-        <transform alias="FF+FB" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[19]" color="#f14cc1">
-        <transform alias="FB-P" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[16]" color="#1f77b4">
-        <transform alias="jerk filtered" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[17]" color="#d62728">
-        <transform alias="slope added" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[18]" color="#004eff">
-        <transform alias="final" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[25]" color="#ff7f0e">
-        <transform alias="calculated" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-      </plot>
-     </DockArea>
-     <DockArea name="...">
-      <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-       <range right="698.352475" top="1.302476" bottom="-3.514695" left="612.164064"/>
-       <limitY/>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[25]" color="#d62728">
-        <transform alias="measured acc (1/0.1s+1)" name="Scale/Offset">
-         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[31]" color="#ff7f0e">
-        <transform alias="error acc " name="Scale/Offset">
-         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[32]" color="#f14cc1">
-        <transform alias="error acc filtered" name="Scale/Offset">
-         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[33]" color="#9467bd">
-        <transform alias="acc cmd output" name="Scale/Offset">
-         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
-        </transform>
-       </curve>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[16]" color="#17becf">
-        <transform alias="desired acc" name="Scale/Offset">
-         <options value_offset="0.0" time_offset="0.0" value_scale="1.0"/>
-        </transform>
-       </curve>
-      </plot>
-     </DockArea>
-     <DockArea name="State">
-      <plot flip_x="false" mode="TimeSeries" style="Lines" flip_y="false">
-       <range right="698.352475" top="2.050000" bottom="-0.050000" left="612.164064"/>
-       <limitY/>
-       <curve name="/control/trajectory_follower/longitudinal/diagnostic/data[13]" color="#d62728">
-        <transform alias="0:Drive 1:Stopping 2:Stopped 3:Emergency" name="Scale/Offset">
-         <options value_offset="0" time_offset="0" value_scale="1.0"/>
-        </transform>
-       </curve>
-      </plot>
-     </DockArea>
-    </DockSplitter>
-   </Container>
-  </Tab>
-  <currentTabIndex index="1"/>
- </tabbed_widget>
- <use_relative_time_offset enabled="1"/>
- <!-- - - - - - - - - - - - - - - -->
- <!-- - - - - - - - - - - - - - - -->
- <Plugins>
-  <plugin ID="DataLoad CSV">
-   <parameters delimiter="0" time_axis=""/>
-  </plugin>
-  <plugin ID="DataLoad MCAP"/>
-  <plugin ID="DataLoad ROS2 bags">
-   <use_header_stamp value="false"/>
-   <discard_large_arrays value="true"/>
-   <max_array_size value="100"/>
-   <boolean_strings_to_number value="true"/>
-   <remove_suffix_from_strings value="true"/>
-   <selected_topics value=""/>
-  </plugin>
-  <plugin ID="DataLoad ULog"/>
-  <plugin ID="MQTT Subscriber (Mosquitto)"/>
-  <plugin ID="ROS2 Topic Subscriber">
-   <use_header_stamp value="false"/>
-   <discard_large_arrays value="false"/>
-   <max_array_size value="100"/>
-   <boolean_strings_to_number value="false"/>
-   <remove_suffix_from_strings value="false"/>
-   <selected_topics value="/control/trajectory_follower/lateral/diagnostic;/control/trajectory_follower/longitudinal/diagnostic;/localization/acceleration;/vehicle/status/control_mode"/>
-  </plugin>
-  <plugin ID="UDP Server"/>
-  <plugin ID="WebSocket Server"/>
-  <plugin ID="ZMQ Subscriber"/>
-  <plugin ID="Fast Fourier Transform"/>
-  <plugin ID="Quaternion to RPY"/>
-  <plugin ID="Reactive Script Editor">
-   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indices, not Lua indices&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
-   <scripts/>
-  </plugin>
-  <plugin ID="CSV Exporter"/>
-  <plugin ID="ROS2 Topic Re-Publisher"/>
- </Plugins>
- <!-- - - - - - - - - - - - - - - -->
- <previouslyLoaded_Datafiles/>
- <!-- - - - - - - - - - - - - - - -->
- <customMathEquations/>
- <snippets/>
- <!-- - - - - - - - - - - - - - - -->
-</root>
-