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 ee371f8592833..20fd564049133 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 @@ -1039,6 +1039,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | Name | Unit | Type | Description | Default value | | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `time_limit` | [ms] | double | Time limit for lane change candidate path generation | 50.0 | | `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | | `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 | 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 df9491576a4ee..15eb23daecf95 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 @@ -1,6 +1,7 @@ /**: ros__parameters: lane_change: + time_limit: 50.0 # [ms] backward_lane_length: 200.0 backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index f0adcb1d69b42..60386f535fc64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -162,6 +162,7 @@ struct Parameters FrenetPlannerParameters frenet{}; // lane change parameters + double time_limit{50.0}; double backward_lane_length{200.0}; double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; 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 07b05ab0ea131..f26cf79e8b120 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 @@ -190,6 +190,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lane change parameters + p.time_limit = getOrDeclareParameter(*node, parameter("time_limit")); p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); @@ -313,6 +314,17 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortime_limit; + updateParam(parameters, ns + "time_limit", time_limit); + if (time_limit >= 10.0) { + p->time_limit = time_limit; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'time_limit' is not updated because the value (%.3f ms) is not valid, " + "keep current value (%.3f ms)", + time_limit, p->time_limit); + } updateParam(parameters, ns + "backward_lane_length", p->backward_lane_length); updateParam( parameters, ns + "backward_length_buffer_for_end_of_lane", @@ -349,25 +361,27 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.lane_changing_decel_factor); updateParam( parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature); - int longitudinal_acc_sampling_num = 0; + int longitudinal_acc_sampling_num = p->trajectory.lon_acc_sampling_num; updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; } else { - RCLCPP_WARN_ONCE( - node_->get_logger(), - "Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not positive", + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not " + "positive", longitudinal_acc_sampling_num); } - int lateral_acc_sampling_num = 0; + int lateral_acc_sampling_num = p->trajectory.lat_acc_sampling_num; updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; } else { - RCLCPP_WARN_ONCE( - node_->get_logger(), - "Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not positive", + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not " + "positive", lateral_acc_sampling_num); } @@ -409,8 +423,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.lat_acc_map = lat_acc_map; } else { - RCLCPP_WARN_ONCE( - node_->get_logger(), + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, "Mismatched size for lateral acceleration. Expected size: %lu, but velocity: %lu, " "min_values: %lu, max_values: %lu", std::max(2ul, velocity.size()), velocity.size(), min_values.size(), max_values.size()); @@ -515,28 +529,32 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorcancel.enable_on_prepare_phase; updateParam(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase); if (!enable_on_prepare_phase) { - RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change cancel function is disabled."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change cancel function is disabled."); p->cancel.enable_on_prepare_phase = enable_on_prepare_phase; } - bool enable_on_lane_changing_phase = true; + bool enable_on_lane_changing_phase = p->cancel.enable_on_lane_changing_phase; updateParam( parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase); if (!enable_on_lane_changing_phase) { - RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change abort function is disabled."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change abort function is disabled."); p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase; } - int deceleration_sampling_num = 0; + int deceleration_sampling_num = p->cancel.deceleration_sampling_num; updateParam(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num); if (deceleration_sampling_num > 0) { p->cancel.deceleration_sampling_num = deceleration_sampling_num; } else { - RCLCPP_WARN_ONCE( - node_->get_logger(), + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, "Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not " "positive", deceleration_sampling_num); 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 7748795851865..45fcd6fa99086 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 @@ -1154,18 +1154,22 @@ bool NormalLaneChange::get_path_using_frenet( const std::vector> & sorted_lane_ids, LaneChangePaths & candidate_paths) const { - stop_watch_.tic("frenet_candidates"); + stop_watch_.tic(__func__); constexpr auto found_safe_path = true; const auto frenet_candidates = utils::lane_change::generate_frenet_candidates( common_data_ptr_, prev_module_output_.path, prepare_metrics); RCLCPP_DEBUG( logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(), - stop_watch_.toc("frenet_candidates")); + stop_watch_.toc(__func__)); candidate_paths.reserve(frenet_candidates.size()); lane_change_debug_.frenet_states.clear(); lane_change_debug_.frenet_states.reserve(frenet_candidates.size()); for (const auto & frenet_candidate : frenet_candidates) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + break; + } + lane_change_debug_.frenet_states.emplace_back( frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter, frenet_candidate.max_lane_changing_length); @@ -1186,7 +1190,7 @@ bool NormalLaneChange::get_path_using_frenet( if (check_candidate_path_safety(*candidate_path_opt, target_objects)) { RCLCPP_DEBUG( logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]", - frenet_candidates.size(), stop_watch_.toc("frenet_candidates")); + frenet_candidates.size(), stop_watch_.toc("__func__")); utils::lane_change::append_target_ref_to_candidate( *candidate_path_opt, common_data_ptr_->lc_param_ptr->frenet.th_curvature_smoothing); candidate_paths.push_back(*candidate_path_opt); @@ -1204,7 +1208,7 @@ bool NormalLaneChange::get_path_using_frenet( RCLCPP_DEBUG( logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(), - stop_watch_.toc("frenet_candidates")); + stop_watch_.toc(__func__)); return !found_safe_path; } @@ -1214,6 +1218,7 @@ bool NormalLaneChange::get_path_using_path_shifter( const std::vector> & sorted_lane_ids, LaneChangePaths & candidate_paths) const { + stop_watch_.tic(__func__); const auto & target_lanes = get_target_lanes(); candidate_paths.reserve( prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); @@ -1279,6 +1284,11 @@ bool NormalLaneChange::get_path_using_path_shifter( prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); for (const auto & lc_metric : lane_changing_metrics) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + RCLCPP_DEBUG(logger_, "Time limit reached and no safe path was found."); + return false; + } + debug_metrics.lc_metrics.emplace_back(lc_metric, -1); const auto debug_print_lat = [&](const std::string & s) {