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 && 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; }