diff --git a/.github/CODEOWNERS b/.github/_CODEOWNERS similarity index 100% rename from .github/CODEOWNERS rename to .github/_CODEOWNERS diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 499327e3b8483..86d8d1c0c1413 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -886,7 +886,8 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | | `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | -| `trajectory.prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 | | `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | | `trajectory.minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | | `trajectory.lon_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 3 | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 572caedb58306..bf892b3058a16 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -11,7 +11,8 @@ # trajectory generation trajectory: - prepare_duration: 4.0 + max_prepare_duration: 4.0 + min_prepare_duration: 2.0 lateral_jerk: 0.5 min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 550925c4a10fc..7202a1c6a9108 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -54,7 +54,7 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; - void update_transient_data() final; + void update_transient_data(const bool is_approved) final; void update_filtered_objects() final; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index e5c7fcb6d621e..741b5afb50e08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -68,7 +68,7 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; - virtual void update_transient_data() = 0; + virtual void update_transient_data(const bool is_approved) = 0; virtual void update_filtered_objects() = 0; @@ -267,6 +267,15 @@ class LaneChangeBase return turn_signal; } + void set_signal_activation_time(const bool reset = false) const + { + if (reset) { + signal_activation_time_ = std::nullopt; + } else if (!signal_activation_time_) { + signal_activation_time_ = clock_.now(); + } + } + LaneChangeStatus status_{}; PathShifter path_shifter_{}; @@ -292,6 +301,7 @@ class LaneChangeBase mutable StopWatch stop_watch_; mutable lane_change::Debug lane_change_debug_; + mutable std::optional signal_activation_time_{std::nullopt}; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index b2aa21e4dc085..29047c90590b3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -75,7 +75,7 @@ double calc_dist_to_last_fit_width( * of the maximum lane change preparation duration and the maximum velocity of the ego vehicle. * * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change + * - `lc_param_ptr->maximum_prepare_duration`: The maximum duration allowed for lane change * preparation. * - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle. * @@ -131,6 +131,24 @@ std::vector calc_lon_acceleration_samples( const CommonDataPtr & common_data_ptr, const double max_path_velocity, const double prepare_duration); +/** + * @brief calculates the actual prepare duration that will be used for path generation + * + * @details computes the required prepare duration to be used for candidate path + * generation based on the elapsed time of turn signal activation, the minimum + * and maximum parameterized values for the prepare duration, + * and the minimum lane changing velocity. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which includes + * lane change parameters and bpp common parameters. + * @param current_velocity current ego vehicle velocity. + * @param active_signal_duration elapsed time since turn signal activation. + * @return The calculated prepare duration value in seconds (s) + */ +double calc_actual_prepare_duration( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double active_signal_duration); + std::vector calc_prepare_phase_metrics( const CommonDataPtr & common_data_ptr, const double current_velocity, const double max_path_velocity, const double min_length_threshold = 0.0, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 32cf000ecb1a7..c121ab8512cce 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -222,6 +222,8 @@ struct TransientData size_t current_path_seg_idx; // index of nearest segment to ego along current path double current_path_velocity; // velocity of the current path at the ego position along the path + double lane_change_prepare_duration{0.0}; + bool is_ego_near_current_terminal_start{false}; bool is_ego_stuck{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp index 31bd5d94f09c8..a76742307e061 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp @@ -115,7 +115,8 @@ struct SafetyParameters struct TrajectoryParameters { - double prepare_duration{4.0}; + double max_prepare_duration{4.0}; + double min_prepare_duration{1.0}; double lateral_jerk{0.5}; double min_longitudinal_acc{-1.0}; double max_longitudinal_acc{1.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 77bf32512c0dc..f80aad721a07c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -80,7 +80,7 @@ void LaneChangeInterface::updateData() module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->update_filtered_objects(); - module_type_->update_transient_data(); + module_type_->update_transient_data(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -155,10 +155,10 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - *prev_approved_path_ = getPreviousModuleOutput().path; - BehaviorModuleOutput out = getPreviousModuleOutput(); + *prev_approved_path_ = out.path; + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); @@ -168,7 +168,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + path_reference_ = std::make_shared(out.reference_path); stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index baba4311e2175..1bb41069140e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -45,8 +45,10 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // trajectory generation { - p.trajectory.prepare_duration = - getOrDeclareParameter(*node, parameter("trajectory.prepare_duration")); + p.trajectory.max_prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.max_prepare_duration")); + p.trajectory.min_prepare_duration = + getOrDeclareParameter(*node, parameter("trajectory.min_prepare_duration")); p.trajectory.lateral_jerk = getOrDeclareParameter(*node, parameter("trajectory.lateral_jerk")); p.trajectory.min_longitudinal_acc = @@ -67,8 +69,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("trajectory.lat_acc_sampling_num")); const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); - p.trajectory.min_lane_changing_velocity = - std::min(p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.prepare_duration); + p.trajectory.min_lane_changing_velocity = std::min( + p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.max_prepare_duration); // validation of trajectory parameters if (p.trajectory.lon_acc_sampling_num < 1 || p.trajectory.lat_acc_sampling_num < 1) { @@ -311,7 +313,10 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "prepare_duration", p->trajectory.prepare_duration); + updateParam( + parameters, ns + "max_prepare_duration", p->trajectory.max_prepare_duration); + updateParam( + parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration); updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); updateParam( parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 8769547550852..99e7cc73e59ff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -131,7 +131,7 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } -void NormalLaneChange::update_transient_data() +void NormalLaneChange::update_transient_data(const bool is_approved) { if ( !common_data_ptr_ || !common_data_ptr_->is_data_available() || @@ -150,6 +150,13 @@ void NormalLaneChange::update_transient_data() prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps; transient_data.current_path_seg_idx = nearest_seg_idx; + const auto active_signal_duration = + signal_activation_time_ ? (clock_.now() - signal_activation_time_.value()).seconds() : 0.0; + transient_data.lane_change_prepare_duration = + is_approved ? status_.lane_change_path.info.duration.prepare + : calculation::calc_actual_prepare_duration( + common_data_ptr_, common_data_ptr_->get_ego_speed(), active_signal_duration); + std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); @@ -328,6 +335,8 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const return get_terminal_turn_signal_info(); } + set_signal_activation_time(); + return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end); } @@ -356,9 +365,14 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx; - return getTurnSignalDecider().overwrite_turn_signal( + const auto turn_signal_info = getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); + + set_signal_activation_time( + turn_signal_info.turn_signal.command != terminal_turn_signal_info.turn_signal.command); + + return turn_signal_info; } LaneChangePath NormalLaneChange::getLaneChangePath() const @@ -429,8 +443,6 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.path = utils::combinePath(output.path, *found_extended_path); } output.reference_path = getReferencePath(); - output.turn_signal_info = - get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); if (isStopState()) { const auto current_velocity = getEgoVelocity(); @@ -446,12 +458,17 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() extendOutputDrivableArea(output); + const auto turn_signal_info = + get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, + turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); + set_signal_activation_time( + output.turn_signal_info.turn_signal.command != turn_signal_info.turn_signal.command); + return output; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 68b2debecc4d2..3ba11d62a2be7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -104,7 +104,7 @@ double calc_dist_to_last_fit_width( double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { - const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.prepare_duration; + const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration; const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel; return max_prepare_duration * ego_max_speed; @@ -197,7 +197,7 @@ std::vector calc_max_lane_change_lengths( const auto & params = common_data_ptr->lc_param_ptr->trajectory; const auto lat_jerk = params.lateral_jerk; - const auto t_prepare = params.prepare_duration; + const auto t_prepare = params.max_prepare_duration; const auto current_velocity = common_data_ptr->get_ego_speed(); const auto path_velocity = common_data_ptr->transient_data.current_path_velocity; @@ -400,18 +400,41 @@ double calc_lane_changing_acceleration( prepare_longitudinal_acc); } +double calc_actual_prepare_duration( + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double active_signal_duration) +{ + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto min_lc_velocity = params.min_lane_changing_velocity; + + // need to ensure min prep duration is sufficient to reach minimum lane changing velocity + const auto min_prepare_duration = std::invoke([&]() -> double { + if (current_velocity >= min_lc_velocity) { + return params.min_prepare_duration; + } + const auto max_acc = + std::min(common_data_ptr->bpp_param_ptr->max_acc, params.max_longitudinal_acc); + if (max_acc < eps) { + return params.max_prepare_duration; + } + return (min_lc_velocity - current_velocity) / max_acc; + }); + + return std::max(params.max_prepare_duration - active_signal_duration, min_prepare_duration); +} + std::vector calc_prepare_durations(const CommonDataPtr & common_data_ptr) { const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto threshold = common_data_ptr->bpp_param_ptr->base_link2front + lc_param_ptr->min_length_for_turn_signal_activation; - const auto max_prepare_duration = lc_param_ptr->trajectory.prepare_duration; // TODO(Azu) this check seems to cause scenario failures. if (common_data_ptr->transient_data.dist_to_terminal_start >= threshold) { - return {max_prepare_duration}; + return {common_data_ptr->transient_data.lane_change_prepare_duration}; } + const auto max_prepare_duration = lc_param_ptr->trajectory.max_prepare_duration; std::vector prepare_durations; constexpr double step = 0.5; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 510fef2c07d7c..ea36e4dc6960a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -223,7 +223,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenInvalid) constexpr auto is_approved = true; normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); - normal_lane_change_->update_transient_data(); + normal_lane_change_->update_transient_data(!is_approved); normal_lane_change_->updateLaneChangeStatus(); const auto & lc_status = normal_lane_change_->getLaneChangeStatus(); @@ -261,7 +261,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenValid) set_previous_approved_path(); normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); - normal_lane_change_->update_transient_data(); + normal_lane_change_->update_transient_data(!is_approved); const auto lane_change_required = normal_lane_change_->isLaneChangeRequired(); ASSERT_TRUE(lane_change_required);