update

Signed-off-by: Berkay Karaman <[email protected]>

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -431,7 +431,7 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
 
   // Insert the interpolated point
   control_data.interpolated_traj.points.insert(
-    control_data.interpolated_traj.points.begin() + uint(nearest_idx), nearest_point);
+    control_data.interpolated_traj.points.begin() + nearest_idx, nearest_point);
   control_data.nearest_idx = nearest_idx;
   control_data.target_idx = control_data.nearest_idx;
 
@@ -456,7 +456,7 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
   control_data.state_after_delay =
     predictedStateAfterDelay(control_data.current_motion, m_delay_compensation_time);
 
-  // calculate the target motion
+  // calculate the target motion for delay compensation
   constexpr double min_running_dist = 0.01;
   if (control_data.state_after_delay.running_distance > min_running_dist) {
     const auto target_pose = longitudinal_utils::findTrajectoryPoseAfterDistance(
@@ -466,10 +466,14 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
       calcInterpolatedTrajPointAndSegment(control_data.interpolated_traj, target_pose);
     control_data.target_idx = target_interpolated_point.second + 1;
     control_data.interpolated_traj.points.insert(
-      control_data.interpolated_traj.points.begin() + uint(control_data.target_idx),
+      control_data.interpolated_traj.points.begin() + control_data.target_idx,
       target_interpolated_point.first);
   }
 
+  // Remove overlapped points after inserting the interpolated points
+  control_data.interpolated_traj.points =
+    motion_utils::removeOverlapPoints(control_data.interpolated_traj.points);
+
   // send debug values
   m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, control_data.state_after_delay.vel);
   m_debug_values.setValues(
@@ -710,6 +714,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
     raw_ctrl_cmd.vel =
       control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps;
     raw_ctrl_cmd.acc = applyVelocityFeedback(control_data);
+    raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx);
 
     RCLCPP_DEBUG(
       logger_,
@@ -739,8 +744,6 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
     raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt);
   }
 
-  raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx);
-
   // store acceleration without slope compensation
   m_prev_raw_ctrl_cmd = raw_ctrl_cmd;
 
@@ -844,11 +847,9 @@ double PidLongitudinalController::calcFilteredAcc(
   // store ctrl cmd without slope filter
   storeAccelCmd(acc_max_filtered);
 
-  // if current state is STOPPED, do not apply slope compensation to avoid sudden acceleration
+  // apply slope compensation
   const double acc_slope_filtered =
-    m_control_state == ControlState::STOPPED
-      ? acc_max_filtered
-      : applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift);
+    applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift);
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered);
 
   // This jerk filter must be applied after slope compensation
@@ -962,20 +963,20 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS
                                                                    : delay_compensation_time;
     // simple linear prediction
     pred_vel = current_vel + current_acc * delay_time_calculation;
-    running_distance = abs(
+    running_distance = std::abs(
       delay_time_calculation * current_vel +
       0.5 * current_acc * delay_time_calculation * delay_time_calculation);
     // avoid to change sign of current_vel and pred_vel
     return StateAfterDelay{pred_vel, pred_acc, running_distance};
   }
 
   for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) {
-    if ((clock_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) {
+    if ((clock_->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < delay_compensation_time) {
       // add velocity to accel * dt
       const double time_to_next_acc =
         (i == m_ctrl_cmd_vec.size() - 1)
           ? std::min(
-              (clock_->now() - m_ctrl_cmd_vec.back().stamp).seconds(), m_delay_compensation_time)
+              (clock_->now() - m_ctrl_cmd_vec.back().stamp).seconds(), delay_compensation_time)
           : std::min(
               (rclcpp::Time(m_ctrl_cmd_vec.at(i + 1).stamp) -
                rclcpp::Time(m_ctrl_cmd_vec.at(i).stamp))
@@ -984,8 +985,8 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS
       const double acc = m_ctrl_cmd_vec.at(i).acceleration;
       // because acc_cmd is positive when vehicle is running backward
       pred_acc = std::copysignf(1.0, static_cast<float>(pred_vel)) * acc;
-      running_distance +=
-        abs(abs(pred_vel) * time_to_next_acc + 0.5 * acc * time_to_next_acc * time_to_next_acc);
+      running_distance += std::abs(
+        std::abs(pred_vel) * time_to_next_acc + 0.5 * acc * time_to_next_acc * time_to_next_acc);
       pred_vel += pred_vel < 0.0 ? (-acc * time_to_next_acc) : (acc * time_to_next_acc);
       if (pred_vel / current_vel < 0.0) {
         // sign of velocity is changed
@@ -1016,7 +1017,7 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
   const double diff_vel = (target_vel - vel_after_delay) * vel_sign;
 
   const bool enable_integration =
-    (abs(vel_after_delay) > m_current_vel_threshold_pid_integrate) && is_under_control;
+    (std::abs(vel_after_delay) > m_current_vel_threshold_pid_integrate) && is_under_control;
 
   const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel);