From 31e7eff0dafebaae5f87834c0d4c42b2f9018c95 Mon Sep 17 00:00:00 2001
From: Berkay Karaman <brkay54@gmail.com>
Date: Mon, 18 Sep 2023 15:33:20 +0300
Subject: [PATCH] enable 3d acc

---
 .../pid_longitudinal_controller.hpp           |  1 +
 .../src/pid_longitudinal_controller.cpp       | 22 +++++++++++++++++--
 2 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
index c3a5c8052f01a..0051d68f35282 100644
--- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
+++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
@@ -134,6 +134,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   bool m_enable_slope_compensation;
   bool m_enable_large_tracking_error_emergency;
   bool m_enable_keep_stopped_until_steer_convergence;
+  bool m_enable_convert_ff_3d;  // temp parameter for converting 2d projected acc from planning to 3d
 
   // smooth stop transition
   struct StateTransitionParams
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
index 50cb6eb881796..37af129d73b4d 100644
--- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
+++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -51,6 +51,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
   m_enable_slope_compensation = node.declare_parameter<bool>("enable_slope_compensation");
   m_enable_keep_stopped_until_steer_convergence =
     node.declare_parameter<bool>("enable_keep_stopped_until_steer_convergence");
+  m_enable_convert_ff_3d = node.declare_parameter<bool>("enable_convert_ff_3d");
 
   // parameters for state transition
   {
@@ -994,8 +995,25 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
     1.0 + (abs(current_target_vel_abs - current_vel_abs) / std::max(current_target_vel_abs, 0.1)),
     ff_scale_min, ff_scale_max);
 
-  const double ff_acc =
-    control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale;
+  // Feedforward acceleration:
+  const double tmp_ff_acc =
+    !m_enable_convert_ff_3d ||
+        control_data.target_idx == control_data.interpolated_traj.points.size() - 1 ||
+        control_data.current_motion.vel < 0.01
+      ? control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2
+      : control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 *
+          (std::max(
+             tier4_autoware_utils::calcDistance2d(
+               control_data.interpolated_traj.points.at(control_data.target_idx + 1).pose,
+               control_data.interpolated_traj.points.at(control_data.target_idx).pose),
+             std::numeric_limits<double>::min()) /
+           std::max(
+             tier4_autoware_utils::calcDistance3d(
+               control_data.interpolated_traj.points.at(control_data.target_idx + 1).pose,
+               control_data.interpolated_traj.points.at(control_data.target_idx).pose),
+             std::numeric_limits<double>::min()));
+
+  const double ff_acc = tmp_ff_acc * ff_scale;
 
   const double feedback_acc = ff_acc + pid_acc;