enable 3d acc

brkay54 · Sep 18, 2023 · 31e7eff · 31e7eff
1 parent ff59f87
commit 31e7eff
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 2 deletions.
diff --git a/...ngitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/...ngitudinal_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
@@ -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;