feat(pid_longitudinal_controller): improve FF gain constant calculation

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -1008,7 +1008,8 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
   const double current_vel = control_data.current_motion.vel;
   const double target_vel =
     control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps;
-
+  const double nearest_target_vel =
+    control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps;
   const double vel_after_delay = control_data.state_after_delay.vel;
   const bool is_under_control =
     m_current_operation_mode.is_autoware_control_enabled &&
@@ -1037,7 +1038,9 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
   constexpr double ff_scale_max = 2.0;  // for safety
   constexpr double ff_scale_min = 0.5;  // for safety
   const double ff_scale = std::clamp(
-    std::abs(current_vel) / std::max(std::abs(target_vel), 0.1), ff_scale_min, ff_scale_max);
+    1.0 + (abs(nearest_target_vel - current_vel) / std::max(abs(current_vel), 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;