diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 928e53b1ccff5..11a2a92024f4f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -384,7 +384,6 @@ In addition, the safety check has a time hysteresis, and if the path is judged " | Name | Unit | Type | Description | Default value | | :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | -| enable_safety_check | [-] | bool | flag whether to use safety check | true | | method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | | keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | | check_all_predicted_path | - | bool | Flag to check all predicted paths | true | diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp index b3a63dcc12132..cdd2a474afda6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -57,22 +57,18 @@ PathDecisionState PathDecisionStateController::get_next_state( auto next_state = current_state_; // update safety - if (!parameters.safety_check_params.enable_safety_check) { - next_state.is_stable_safe = true; - } else { - if (is_current_safe) { - if (!next_state.safe_start_time) { - next_state.safe_start_time = now; - next_state.is_stable_safe = false; - } else { - next_state.is_stable_safe = - ((now - next_state.safe_start_time.value()).seconds() > - parameters.safety_check_params.keep_unsafe_time); - } - } else { - next_state.safe_start_time = std::nullopt; + if (is_current_safe) { + if (!next_state.safe_start_time) { + next_state.safe_start_time = now; next_state.is_stable_safe = false; + } else { + next_state.is_stable_safe = + ((now - next_state.safe_start_time.value()).seconds() > + parameters.safety_check_params.keep_unsafe_time); } + } else { + next_state.safe_start_time = std::nullopt; + next_state.is_stable_safe = false; } // Once this function returns true, it will continue to return true thereafter @@ -87,10 +83,9 @@ PathDecisionState PathDecisionStateController::get_next_state( } const auto & pull_over_path = pull_over_path_opt.value(); - const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved - if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { + if (!next_state.is_stable_safe && !is_activated) { RCLCPP_DEBUG( logger_, "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -132,7 +127,7 @@ PathDecisionState PathDecisionStateController::get_next_state( return next_state; } - if (enable_safety_check && !next_state.is_stable_safe) { + if (!next_state.is_stable_safe) { RCLCPP_DEBUG( logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 15924059d97e9..ef22194e90a74 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -79,11 +79,6 @@ GoalPlannerModule::GoalPlannerModule( is_lane_parking_cb_running_{false}, is_freespace_parking_cb_running_{false} { - // TODO(soblin): remove safety_check_params.enable_safety_check because it does not make sense - if (!parameters_.safety_check_params.enable_safety_check) { - throw std::domain_error("goal_planner never works without safety check"); - } - occupancy_grid_map_ = std::make_shared(); // planner when goal modification is not allowed diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 9dbed01ed71e9..4b89ddab1f272 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -368,6 +368,10 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + // NOTE(soblin): remove safety_check_params.enable_safety_check because it does not make sense + if (!p.safety_check_params.enable_safety_check) { + throw std::domain_error("goal_planner never works without safety check"); + } p.safety_check_params.keep_unsafe_time = node->declare_parameter(safety_check_ns + "keep_unsafe_time"); p.safety_check_params.method = node->declare_parameter(safety_check_ns + "method"); @@ -786,9 +790,6 @@ void GoalPlannerModuleManager::updateModuleParams( // SafetyCheckParams const std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; { - updateParam( - parameters, safety_check_ns + "enable_safety_check", - p->safety_check_params.enable_safety_check); updateParam( parameters, safety_check_ns + "keep_unsafe_time", p->safety_check_params.keep_unsafe_time); updateParam(parameters, safety_check_ns + "method", p->safety_check_params.method);