not sure stopped

autowarefoundation · Aug 28, 2023 · 4e1a86a · 4e1a86a
1 parent 7398f36
commit 4e1a86a
Showing 1 changed file with 11 additions and 9 deletions.
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -518,10 +518,12 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm
 
 void PidLongitudinalController::updateControlState(const ControlData & control_data)
 {
-  const double target_vel =
-    control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps;
-  const double target_acc =
-    control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2;
+  //  const double target_vel =
+  //    control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps;
+  //  const double target_acc =
+  //    control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2;
+  const double vel_after_delay = control_data.state_after_delay.vel;
+  const double acc_after_delay = control_data.state_after_delay.acc;
   const double stop_dist = control_data.stop_dist;
 
   // flags for state transition
@@ -536,8 +538,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
   if (
-    std::fabs(target_vel) > p.stopped_state_entry_vel ||
-    std::fabs(target_acc) > p.stopped_state_entry_acc) {
+    std::fabs(vel_after_delay) > p.stopped_state_entry_vel ||
+    std::fabs(acc_after_delay) > p.stopped_state_entry_acc) {
     m_last_running_time = std::make_shared<rclcpp::Time>(node_->now());
   }
   const bool stopped_condition =
@@ -582,7 +584,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
     if (m_enable_smooth_stop) {
       if (stopping_condition) {
-        m_smooth_stop.init(control_data.state_after_delay.vel, control_data.stop_dist);
+        m_smooth_stop.init(vel_after_delay, control_data.stop_dist);
         return changeState(ControlState::STOPPING);
       }
     } else {
@@ -673,8 +675,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
       target_motion.vel, raw_ctrl_cmd.acc);
   } else if (m_control_state == ControlState::STOPPING) {
     raw_ctrl_cmd.acc = m_smooth_stop.calculate(
-      control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc,
-      m_vel_hist, m_delay_compensation_time);
+      control_data.stop_dist, control_data.state_after_delay.vel,
+      control_data.state_after_delay.acc, m_vel_hist, m_delay_compensation_time);
     raw_ctrl_cmd.vel = m_stopped_state_params.vel;
 
     RCLCPP_DEBUG(