diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/README.md index 2416ebf2d5599..84deb0b1f83cf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/README.md @@ -12,15 +12,16 @@ This module is activated when there is a detection area on the target lane. ### Module Parameters -| Parameter | Type | Description | -| ---------------------------------- | ------ | -------------------------------------------------------------------------------------------------- | -| `use_dead_line` | bool | [-] weather to use dead line or not | -| `use_pass_judge_line` | bool | [-] weather to use pass judge line or not | -| `state_clear_time` | double | [s] when the vehicle is stopping for certain time without incoming obstacle, move to STOPPED state | -| `stop_margin` | double | [m] a margin that the vehicle tries to stop before stop_line | -| `dead_line_margin` | double | [m] ignore threshold that vehicle behind is collide with ego vehicle or not | -| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section) | -| `distance_to_judge_over_stop_line` | double | [m] parameter for judging that the stop line has been crossed | +| Parameter | Type | Description | +| ----------------------------------- | ------ | -------------------------------------------------------------------------------------------------- | +| `use_dead_line` | bool | [-] weather to use dead line or not | +| `use_pass_judge_line` | bool | [-] weather to use pass judge line or not | +| `state_clear_time` | double | [s] when the vehicle is stopping for certain time without incoming obstacle, move to STOPPED state | +| `stop_margin` | double | [m] a margin that the vehicle tries to stop before stop_line | +| `dead_line_margin` | double | [m] ignore threshold that vehicle behind is collide with ego vehicle or not | +| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section) | +| `distance_to_judge_over_stop_line` | double | [m] parameter for judging that the stop line has been crossed | +| `suppress_pass_judge_when_stopping` | bool | [m] parameter for suppressing pass judge when stopping | ### Inner-workings / Algorithm @@ -47,7 +48,9 @@ endif :get clear stop state duration; if (clear stop state duration is more than state_clear_time?) then (yes) - :set current state GO; + if (suppress_pass_judge_when_stopping is false or ego is moving) then (yes) + :set current state GO; + endif :reset clear stop state duration; stop else (no) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml index 62b5f2458336f..b49b43794685c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -9,3 +9,4 @@ hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + suppress_pass_judge_when_stopping: false diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 166bc9cfaee4d..3f39696ff6bcc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -48,6 +48,8 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".hold_stop_margin_distance"); planner_param_.distance_to_judge_over_stop_line = getOrDeclareParameter(node, ns + ".distance_to_judge_over_stop_line"); + planner_param_.suppress_pass_judge_when_stopping = + getOrDeclareParameter(node, ns + ".suppress_pass_judge_when_stopping"); } void DetectionAreaModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index ad3ba986733e1..afbff7425542a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -112,8 +112,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * setSafe(detection_area::can_clear_stop_state( last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time)); if (isActivated()) { - state_ = State::GO; last_obstacle_found_time_ = {}; + if (!planner_param_.suppress_pass_judge_when_stopping || !is_stopped) { + state_ = State::GO; + } return true; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index d756ea39a7f92..46bb46bcdeb61 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -61,6 +61,7 @@ class DetectionAreaModule : public SceneModuleInterface double state_clear_time; double hold_stop_margin_distance; double distance_to_judge_over_stop_line; + bool suppress_pass_judge_when_stopping; }; DetectionAreaModule(