feat(pid_longitudinal_controller): take into account the current acce…

…leration to calculate delayed pose (#4616) * feat(pid_longitudinal_controller): take into account the current acceleration to calculate delayed pose Signed-off-by: Berkay Karaman <brkay54@gmail.com> * Update for reverse driving Signed-off-by: Berkay Karaman <brkay54@gmail.com> * update for wrong calculation Signed-off-by: Berkay Karaman <brkay54@gmail.com> * update Signed-off-by: Berkay Karaman <brkay54@gmail.com> --------- Signed-off-by: Berkay Karaman <brkay54@gmail.com>
autowarefoundation · Aug 17, 2023 · e9dfe5c · e9dfe5c
1 parent b5f83d0
commit e9dfe5c
Show file tree

Hide file tree

Showing 4 changed files with 82 additions and 21 deletions.
diff --git a/...itudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/...itudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp
@@ -72,11 +72,12 @@ double getPitchByTraj(
 double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to);
 
 /**
- * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for
- * delayed time
+ * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and
+ * acceleration for delayed time
  */
 Pose calcPoseAfterTimeDelay(
-  const Pose & current_pose, const double delay_time, const double current_vel);
+  const Pose & current_pose, const double delay_time, const double current_vel,
+  const double current_acc);
 
 /**
  * @brief apply linear interpolation to orientation

diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
@@ -129,11 +129,23 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint
 }
 
 Pose calcPoseAfterTimeDelay(
-  const Pose & current_pose, const double delay_time, const double current_vel)
+  const Pose & current_pose, const double delay_time, const double current_vel,
+  const double current_acc)
 {
+  if (delay_time <= 0.0) {
+    return current_pose;
+  }
+
+  // check time to stop
+  const double time_to_stop = -current_vel / current_acc;
+
+  const double delay_time_calculation =
+    time_to_stop > 0.0 && time_to_stop < delay_time ? time_to_stop : delay_time;
   // simple linear prediction
   const double yaw = tf2::getYaw(current_pose.orientation);
-  const double running_distance = delay_time * current_vel;
+  const double running_distance = delay_time_calculation * current_vel + 0.5 * current_acc *
+                                                                           delay_time_calculation *
+                                                                           delay_time_calculation;
   const double dx = running_distance * std::cos(yaw);
   const double dy = running_distance * std::sin(yaw);
 

diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -626,7 +626,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
   Motion target_motion{};
   if (m_control_state == ControlState::DRIVE) {
     const auto target_pose = longitudinal_utils::calcPoseAfterTimeDelay(
-      current_pose, m_delay_compensation_time, current_vel);
+      current_pose, m_delay_compensation_time, current_vel, current_acc);
     const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose);
     target_motion = Motion{
       target_interpolated_point.longitudinal_velocity_mps,

diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
@@ -197,72 +197,120 @@ TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay)
   quaternion_tf.setRPY(0.0, 0.0, 0.0);
   current_pose.orientation = tf2::toMsg(quaternion_tf);
 
-  // With a delay and/or a velocity of 0.0 there is no change of position
+  // With a delay acceleration and/or a velocity of 0.0 there is no change of position
   double delay_time = 0.0;
   double current_vel = 0.0;
+  double current_acc = 0.0;
   Pose delayed_pose =
-    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 
   delay_time = 1.0;
   current_vel = 0.0;
-  delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
+  current_acc = 0.0;
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 
   delay_time = 0.0;
   current_vel = 1.0;
-  delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
+  current_acc = 0.0;
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 
   // With both delay and velocity: change of position
   delay_time = 1.0;
   current_vel = 1.0;
-  delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
+  current_acc = 0.0;
+
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err);
   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 
+  // With all, acceleration, delay and velocity: change of position
+  delay_time = 1.0;
+  current_vel = 1.0;
+  current_acc = 1.0;
+
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
+  EXPECT_NEAR(
+    delayed_pose.position.x,
+    current_pose.position.x + current_vel * delay_time +
+      0.5 * current_acc * delay_time * delay_time,
+    abs_err);
+  EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
+  EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
+
   // Vary the yaw
   quaternion_tf.setRPY(0.0, 0.0, M_PI);
   current_pose.orientation = tf2::toMsg(quaternion_tf);
-  delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
-  EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x - current_vel * delay_time, abs_err);
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
+  EXPECT_NEAR(
+    delayed_pose.position.x,
+    current_pose.position.x - current_vel * delay_time -
+      0.5 * current_acc * delay_time * delay_time,
+    abs_err);
   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 
   quaternion_tf.setRPY(0.0, 0.0, M_PI_2);
   current_pose.orientation = tf2::toMsg(quaternion_tf);
-  delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
-  EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y + current_vel * delay_time, abs_err);
+  EXPECT_NEAR(
+    delayed_pose.position.y,
+    current_pose.position.y + current_vel * delay_time +
+      0.5 * current_acc * delay_time * delay_time,
+    abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 
   quaternion_tf.setRPY(0.0, 0.0, -M_PI_2);
   current_pose.orientation = tf2::toMsg(quaternion_tf);
-  delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
-  EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y - current_vel * delay_time, abs_err);
+  EXPECT_NEAR(
+    delayed_pose.position.y,
+    current_pose.position.y - current_vel * delay_time -
+      0.5 * current_acc * delay_time * delay_time,
+    abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 
   // Vary the pitch : no effect /!\ NOTE: bug with roll of +-PI/2 which rotates the yaw by PI
   quaternion_tf.setRPY(0.0, M_PI_4, 0.0);
   current_pose.orientation = tf2::toMsg(quaternion_tf);
-  delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
-  EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err);
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
+  EXPECT_NEAR(
+    delayed_pose.position.x,
+    current_pose.position.x + current_vel * delay_time +
+      0.5 * current_acc * delay_time * delay_time,
+    abs_err);
   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 
   // Vary the roll : no effect
   quaternion_tf.setRPY(M_PI_2, 0.0, 0.0);
   current_pose.orientation = tf2::toMsg(quaternion_tf);
-  delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel);
-  EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err);
+  delayed_pose =
+    longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc);
+  EXPECT_NEAR(
+    delayed_pose.position.x,
+    current_pose.position.x + current_vel * delay_time +
+      0.5 * current_acc * delay_time * delay_time,
+    abs_err);
   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
 }