diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index db2436f796dfe..22de79f4dc7d0 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -11,10 +11,10 @@ # avoidance module common setting enable_bound_clipping: false - enable_update_path_when_object_is_gone: false enable_force_avoidance_for_stopped_vehicle: false enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false + enable_cancel_maneuver: false disable_path_update: false # drivable area setting diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 9be99f705f560..4e4d2258fc276 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -601,13 +601,13 @@ $$ ### Avoidance cancelling maneuver -If `enable_update_path_when_object_is_gone` parameter is true, Avoidance Module takes different actions according to the situations as follows: +If `enable_cancel_maneuver` parameter is true, Avoidance Module takes different actions according to the situations as follows: - If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled. - If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled. - If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled. -If `enable_update_path_when_object_is_gone` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. +If `enable_cancel_maneuver` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. ## How to keep the consistency of the optimize-base path generation logic @@ -621,15 +621,15 @@ The avoidance specific parameter configuration file can be located at `src/autow namespace: `avoidance.` -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | -| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | -| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | -| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | -| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | +| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | +| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | +| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | +| enable_cancel_maneuver | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | +| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | +| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | | Name | Unit | Type | Description | Default value | | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index b6cc840a560f0..3095ec2c03251 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -195,19 +195,6 @@ class AvoidanceModule : public SceneModuleInterface */ AvoidanceState updateEgoState(const AvoidancePlanningData & data) const; - /** - * @brief check whether the ego is shifted based on shift line. - * @return result. - */ - bool isAvoidanceManeuverRunning(); - - /** - * @brief check whether the ego is in avoidance maneuver based on shift line and target object - * existence. - * @return result. - */ - bool isAvoidancePlanRunning() const; - // ego behavior update /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 2b1a9cf1b82d4..0263eee5fe727 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -92,8 +92,8 @@ struct AvoidanceParameters // to use this, enable_avoidance_over_same_direction need to be set to true. bool use_opposite_lane{true}; - // enable update path when if detected objects on planner data is gone. - bool enable_update_path_when_object_is_gone{false}; + // if this param is true, it reverts avoidance path when the path is no longer needed. + bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle bool enable_force_avoidance_for_stopped_vehicle{false}; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 601badee6a9f9..db80bc72f21ad 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -135,64 +135,53 @@ bool AvoidanceModule::isExecutionReady() const bool AvoidanceModule::canTransitSuccessState() { const auto & data = avoidance_data_; - const auto is_plan_running = isAvoidancePlanRunning(); - const bool has_avoidance_target = !data.target_objects.empty(); + // Change input lane. -> EXIT. if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Exit."); return true; } - const auto idx = planner_data_->findEgoIndex(data.reference_path.points); - if (idx == data.reference_path.points.size() - 1) { - arrived_path_end_ = true; - } - - constexpr double THRESHOLD = 1.0; - if ( - calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD && - arrived_path_end_) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "reach path end point. exit avoidance module."); - return true; - } + helper_.setPreviousDrivingLanes(data.current_lanelets); - DEBUG_PRINT( - "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); + // Reach input path end point. -> EXIT. + { + const auto idx = planner_data_->findEgoIndex(data.reference_path.points); + if (idx == data.reference_path.points.size() - 1) { + arrived_path_end_ = true; + } - if (!is_plan_running && !has_avoidance_target) { - return true; + constexpr double THRESHOLD = 1.0; + const auto is_further_than_threshold = + calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD; + if (is_further_than_threshold && arrived_path_end_) { + RCLCPP_WARN(getLogger(), "Reach path end point. Exit."); + return true; + } } - if ( - !has_avoidance_target && parameters_->enable_update_path_when_object_is_gone && - !isAvoidanceManeuverRunning()) { - // if dynamic objects are removed on path, change current state to reset path - return true; + const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_shift_point = !path_shifter_.getShiftLines().empty(); + const bool has_base_offset = + std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + + // Nothing to do. -> EXIT. + if (!has_avoidance_target) { + if (!has_shift_point && !has_base_offset) { + RCLCPP_INFO(getLogger(), "No objects. No approved shift lines. Exit."); + return true; + } } - helper_.setPreviousDrivingLanes(data.current_lanelets); - - return false; -} - -bool AvoidanceModule::isAvoidancePlanRunning() const -{ - constexpr double AVOIDING_SHIFT_THR = 0.1; - const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > AVOIDING_SHIFT_THR; - const bool has_shift_point = (path_shifter_.getShiftLinesSize() > 0); - return has_base_offset || has_shift_point; -} -bool AvoidanceModule::isAvoidanceManeuverRunning() -{ - const auto path_idx = avoidance_data_.ego_closest_path_index; - - for (const auto & al : registered_raw_shift_lines_) { - if (path_idx > al.start_idx || is_avoidance_maneuver_starts) { - is_avoidance_maneuver_starts = true; + // Be able to canceling avoidance path. -> EXIT. + if (!has_avoidance_target) { + if (!helper_.isShifted() && parameters_->enable_cancel_maneuver) { + RCLCPP_INFO(getLogger(), "No objects. Cancel avoidance path. Exit."); return true; } } - return false; + + return false; // Keep current state. } AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 13b3344dc7c39..a0ae98276c05e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -55,8 +55,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.detection_area_left_expand_dist = get_parameter(node, ns + "detection_area_left_expand_dist"); p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); + p.enable_cancel_maneuver = get_parameter(node, ns + "enable_cancel_maneuver"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 7c12579bf34f5..0f770efb542c7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -197,8 +197,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( get_parameter(node, ns + "detection_area_right_expand_dist"); p.detection_area_left_expand_dist = get_parameter(node, ns + "detection_area_left_expand_dist"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); }