From 827959e5f1baf05c0ec7d1e7d172bea9fed877e1 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 1 Nov 2023 18:29:58 +0900 Subject: [PATCH 1/2] fix(pid_longitudinal_control): not check steering convergence when moving Signed-off-by: Takamasa Horibe --- .../src/pid_longitudinal_controller.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 23c4bcc857cd4..d7d487f1ec1c1 100644 --- 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 && From 6ce1cfdfbeaa385596355294e23b3ba1f2aa4766 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 1 Nov 2023 10:17:59 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../map_based_prediction/src/map_based_prediction_node.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index c013b75e27e81..10a90188faf04 100644 --- 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 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; }