fix(pid_longitudinal_control): not check steering convergence when mo…

…ving (#999) * fix(pid_longitudinal_control): not check steering convergence when moving Signed-off-by: Takamasa Horibe <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Takamasa Horibe <[email protected]> Co-authored-by: Tomoya Kimura <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Nov 1, 2023 · 7aa0994 · 7aa0994
1 parent 4354856
commit 7aa0994
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 7 deletions.
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -489,8 +489,13 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist;
   const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist;
 
-  const bool keep_stopped_condition =
-    m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged;
+  // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
+  static constexpr double vel_epsilon = 1e-3;
+
+  // Let vehicle start after the steering is converged for control accuracy
+  const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
+                                      m_enable_keep_stopped_until_steer_convergence &&
+                                      !lateral_sync_data_.is_steer_converged;
 
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
   if (
@@ -503,8 +508,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
       ? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
       : false;
 
-  static constexpr double vel_epsilon =
-    1e-3;  // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
   const double current_vel_cmd =
     std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
   const bool emergency_condition = m_enable_overshoot_emergency &&

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -611,7 +611,6 @@ void replaceObjectYawWithLaneletsYaw(
   pose_with_cov.pose.orientation = tf2::toMsg(filtered_quaternion);
 }
 
-
 MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options)
 : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0)
 {
@@ -860,8 +859,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
       // Generate Predicted Path
       std::vector<PredictedPath> predicted_paths;
       for (const auto & ref_path : ref_paths) {
-        PredictedPath predicted_path =
-          path_generator_->generatePathForOnLaneVehicle(yaw_fixed_transformed_object, ref_path.path);
+        PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
+          yaw_fixed_transformed_object, ref_path.path);
         if (predicted_path.path.empty()) {
           continue;
         }