feat(pid_longitudinal_controller): fix some bugs and improve delay compensation #4712

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

Author: brkay54

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

@@ -29,9 +29,7 @@
 #include <cmath>
 #include <limits>
 
-namespace autoware::motion::control::pid_longitudinal_controller
-{
-namespace longitudinal_utils
+namespace autoware::motion::control::pid_longitudinal_controller::longitudinal_utils
 {
 
 using autoware_auto_planning_msgs::msg::Trajectory;
@@ -75,9 +73,8 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint
  * @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 double current_acc);
+std::pair<double, double> calcDistAndVelAfterTimeDelay(
+  const double delay_time, const double current_vel, const double current_acc);
 
 /**
  * @brief apply linear interpolation to orientation
@@ -93,7 +90,7 @@ Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, c
  * @param [in] point Interpolated point is nearest to this point.
  */
 template <class T>
-TrajectoryPoint lerpTrajectoryPoint(
+std::pair<TrajectoryPoint, size_t> lerpTrajectoryPoint(
   const T & points, const Pose & pose, const double max_dist, const double max_yaw)
 {
   TrajectoryPoint interpolated_point;
@@ -112,6 +109,8 @@ TrajectoryPoint lerpTrajectoryPoint(
       points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
     interpolated_point.pose.position.y = interpolation::lerp(
       points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
+    interpolated_point.pose.position.z = interpolation::lerp(
+      points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio);
     interpolated_point.pose.orientation = lerpOrientation(
       points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
     interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
@@ -125,7 +124,7 @@ TrajectoryPoint lerpTrajectoryPoint(
       points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio);
   }
 
-  return interpolated_point;
+  return std::make_pair(interpolated_point, seg_idx);
 }
 
 /**
@@ -149,7 +148,16 @@ double applyDiffLimitFilter(
 double applyDiffLimitFilter(
   const double input_val, const double prev_val, const double dt, const double max_val,
   const double min_val);
-}  // namespace longitudinal_utils
-}  // namespace autoware::motion::control::pid_longitudinal_controller
+
+/**
+ * @brief calculate the projected pose after distance from the current index
+ * @param [in] src_idx current index
+ * @param [in] distance distance to project
+ * @param [in] trajectory reference trajectory
+ */
+geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
+  const size_t src_idx, const double distance,
+  const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
+}  // namespace autoware::motion::control::pid_longitudinal_controller::longitudinal_utils
 
 #endif  // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -64,13 +64,25 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
     double vel{0.0};
     double acc{0.0};
   };
-
+  struct StateAfterDelay
+  {
+    StateAfterDelay(const double velocity, const double acceleration, const double distance)
+    : vel(velocity), acc(acceleration), running_distance(distance)
+    {
+    }
+    double vel{0.0};
+    double acc{0.0};
+    double running_distance{0.0};
+  };
   enum class Shift { Forward = 0, Reverse };
 
   struct ControlData
   {
     bool is_far_from_trajectory{false};
+    autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{};
     size_t nearest_idx{0};  // nearest_idx = 0 when nearest_idx is not found with findNearestIdx
+    size_t target_idx{0};
+    StateAfterDelay state_after_delay{0.0, 0.0, 0.0};
     Motion current_motion{};
     Shift shift{Shift::Forward};  // shift is used only to calculate the sign of pitch compensation
     double stop_dist{0.0};  // signed distance that is positive when car is before the stopline
@@ -268,11 +280,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 
   /**
    * @brief calculate control command based on the current control state
-   * @param [in] current_pose current ego pose
    * @param [in] control_data control data
    */
-  Motion calcCtrlCmd(
-    const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data);
+  Motion calcCtrlCmd(const ControlData & control_data);
 
   /**
    * @brief publish control command
@@ -303,7 +313,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
    * @brief calculate direction (forward or backward) that vehicle moves
    * @param [in] nearest_idx nearest index on trajectory to vehicle
    */
-  enum Shift getCurrentShift(const size_t nearest_idx) const;
+  enum Shift getCurrentShift(const ControlData & control_data) const;
 
   /**
    * @brief filter acceleration command with limitation of acceleration and jerk, and slope
@@ -342,7 +352,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
    * @param [in] point vehicle position
    * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position
    */
-  autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue(
+  std::pair<autoware_auto_planning_msgs::msg::TrajectoryPoint, size_t>
+  calcInterpolatedTrajPointAndSegment(
     const autoware_auto_planning_msgs::msg::Trajectory & traj,
     const geometry_msgs::msg::Pose & pose) const;
 
@@ -351,18 +362,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
    * @param [in] current_motion current velocity and acceleration of the vehicle
    * @param [in] delay_compensation_time predicted time delay
    */
-  double predictedVelocityInTargetPoint(
+  StateAfterDelay predictedStateAfterDelay(
     const Motion current_motion, const double delay_compensation_time) const;
 
   /**
    * @brief calculate velocity feedback with feed forward and pid controller
-   * @param [in] target_motion reference velocity and acceleration. This acceleration will be used
-   * as feed forward.
-   * @param [in] dt time step to use
-   * @param [in] current_vel current velocity of the vehicle
+   * @param [in] control_data data for control calculation
    */
-  double applyVelocityFeedback(
-    const Motion target_motion, const double dt, const double current_vel);
+  double applyVelocityFeedback(const ControlData & control_data);
 
   /**
    * @brief update variables for debugging about pitch
@@ -375,12 +382,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   /**
    * @brief update variables for velocity and acceleration
    * @param [in] ctrl_cmd latest calculated control command
-   * @param [in] current_pose current pose of the vehicle
    * @param [in] control_data data for control calculation
    */
-  void updateDebugVelAcc(
-    const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose,
-    const ControlData & control_data);
+  void updateDebugVelAcc(const Motion & ctrl_cmd, const ControlData & control_data);
 };
 }  // namespace autoware::motion::control::pid_longitudinal_controller
 

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

@@ -17,8 +17,6 @@
 #include "tf2/LinearMath/Matrix3x3.h"
 #include "tf2/LinearMath/Quaternion.h"
 
-#include <experimental/optional>  // NOLINT
-
 #ifdef ROS_DISTRO_GALACTIC
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 #else
@@ -28,9 +26,7 @@
 #include <algorithm>
 #include <limits>
 
-namespace autoware::motion::control::pid_longitudinal_controller
-{
-namespace longitudinal_utils
+namespace autoware::motion::control::pid_longitudinal_controller::longitudinal_utils
 {
 
 bool isValidTrajectory(const Trajectory & traj)
@@ -48,11 +44,7 @@ bool isValidTrajectory(const Trajectory & traj)
   }
 
   // when trajectory is empty
-  if (traj.points.empty()) {
-    return false;
-  }
-
-  return true;
+  return !traj.points.empty();
 }
 
 double calcStopDistance(
@@ -75,7 +67,9 @@ double calcStopDistance(
 
 double getPitchByPose(const Quaternion & quaternion_msg)
 {
-  double roll, pitch, yaw;
+  double roll{0.0};
+  double pitch{0.0};
+  double yaw{0.0};
   tf2::Quaternion quaternion;
   tf2::fromMsg(quaternion_msg, quaternion);
   tf2::Matrix3x3{quaternion}.getRPY(roll, pitch, yaw);
@@ -128,12 +122,11 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint
   return pitch;
 }
 
-Pose calcPoseAfterTimeDelay(
-  const Pose & current_pose, const double delay_time, const double current_vel,
-  const double current_acc)
+std::pair<double, double> calcDistAndVelAfterTimeDelay(
+  const double delay_time, const double current_vel, const double current_acc)
 {
   if (delay_time <= 0.0) {
-    return current_pose;
+    return std::make_pair(0.0, 0.0);
   }
 
   // check time to stop
@@ -142,17 +135,11 @@ Pose calcPoseAfterTimeDelay(
   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 vel_after_delay = current_vel + current_acc * delay_time_calculation;
   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);
-
-  auto pred_pose = current_pose;
-  pred_pose.position.x += dx;
-  pred_pose.position.y += dy;
-  return pred_pose;
+  return std::make_pair(running_distance, vel_after_delay);
 }
 
 double lerp(const double v_from, const double v_to, const double ratio)
@@ -162,7 +149,8 @@ double lerp(const double v_from, const double v_to, const double ratio)
 
 Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
 {
-  tf2::Quaternion q_from, q_to;
+  tf2::Quaternion q_from;
+  tf2::Quaternion q_to;
   tf2::fromMsg(o_from, q_from);
   tf2::fromMsg(o_to, q_to);
 
@@ -187,5 +175,35 @@ double applyDiffLimitFilter(
   const double min_val = -max_val;
   return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val);
 }
-}  // namespace longitudinal_utils
-}  // namespace autoware::motion::control::pid_longitudinal_controller
+
+geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
+  const size_t src_idx, const double distance,
+  const autoware_auto_planning_msgs::msg::Trajectory & trajectory)
+{
+  double remain_dist = distance;
+  geometry_msgs::msg::Pose p = trajectory.points.back().pose;
+  for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) {
+    const double dist = tier4_autoware_utils::calcDistance3d(
+      trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose);
+    if (remain_dist < dist) {
+      if (remain_dist <= 0.0) {
+        return trajectory.points.at(i).pose;
+      }
+      double ratio = remain_dist / dist;
+      p = trajectory.points.at(i).pose;
+      p.position.x = trajectory.points.at(i).pose.position.x +
+                     ratio * (trajectory.points.at(i + 1).pose.position.x -
+                              trajectory.points.at(i).pose.position.x);
+      p.position.y = trajectory.points.at(i).pose.position.y +
+                     ratio * (trajectory.points.at(i + 1).pose.position.y -
+                              trajectory.points.at(i).pose.position.y);
+      p.position.z = trajectory.points.at(i).pose.position.z +
+                     ratio * (trajectory.points.at(i + 1).pose.position.z -
+                              trajectory.points.at(i).pose.position.z);
+      break;
+    }
+    remain_dist -= dist;
+  }
+  return p;
+}
+}  // namespace autoware::motion::control::pid_longitudinal_controller::longitudinal_utils