From 0dda531039c82354df9773dac1aa4ea613aa114b Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 26 Sep 2023 15:18:58 +0900 Subject: [PATCH] feat(lane_change): enable lane change in crosswalk/intersection if ego vehicle gets stuck (#5105) * feat(lane_change): enalbe lane change in crosswalk/intersection if ego vehicle stucks Signed-off-by: Fumiya Watanabe * docs(lane_change): fix document Signed-off-by: Fumiya Watanabe * docs(lane_change): fix words in document Signed-off-by: Fumiya Watanabe * refactor(lane_change): refactor initializing Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../config/lane_change/lane_change.param.yaml | 5 ++ ...ehavior_path_planner_lane_change_design.md | 10 ++++ .../scene_module/lane_change/normal.hpp | 5 ++ .../lane_change/lane_change_module_data.hpp | 4 ++ .../src/scene_module/lane_change/manager.cpp | 6 ++ .../src/scene_module/lane_change/normal.cpp | 57 +++++++++++-------- 6 files changed, 62 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index d7179cd384008..583bdeaaf8dc5 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -60,6 +60,11 @@ crosswalk: false intersection: false + # ego vehicle stuck detection + stuck_detection: + velocity: 0.1 # [m/s] + stop_time: 3.0 # [s] + # collision check enable_prepare_segment_collision_check: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 24b9672c1260e..45779a75b3637 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -190,6 +190,9 @@ The following figure illustrates when the lane is blocked in multiple lane chang If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`. +If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswalk/intersection. +If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck. +If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping. ### Aborting lane change @@ -307,6 +310,13 @@ The following parameters are configurable in `lane_change.param.yaml`. | `regulation.crosswalk` | [-] | boolean | Regulate lane change on crosswalks | false | | `regulation.intersection` | [-] | boolean | Regulate lane change on intersections | false | +### Ego vehicle stuck detection + +| Name | Unit | Type | Description | Default value | +| :-------------------------- | ----- | ------ | --------------------------------------------------- | ------------- | +| `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | +| `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | + ### Collision checks during lane change The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 747c75babb573..f1e4483c8bf51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -155,7 +155,12 @@ class NormalLaneChange : public LaneChangeBase void setStopPose(const Pose & stop_pose); + void updateStopTime(); + + double getStopTime() const { return stop_time_; } + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); + double stop_time_{0.0}; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 398b60e426d00..d3555eae83539 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -70,6 +70,10 @@ struct LaneChangeParameters bool regulate_on_crosswalk{false}; bool regulate_on_intersection{false}; + // ego vehicle stuck detection + double stop_velocity_threshold{0.1}; + double stop_time_threshold{3.0}; + // true by default for all objects utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; 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 ac62460713f29..8d90a0a13179c 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 @@ -80,6 +80,12 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.regulate_on_intersection = getOrDeclareParameter(*node, parameter("regulation.intersection")); + // ego vehicle stuck detection + p.stop_velocity_threshold = + getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); + p.stop_time_threshold = + getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index dea6073654ae2..e392e67428b08 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -39,10 +39,12 @@ NormalLaneChange::NormalLaneChange( : LaneChangeBase(parameters, type, direction) { stop_watch_.tic(getModuleTypeStr()); + stop_watch_.tic("stop_time"); } void NormalLaneChange::updateLaneChangeStatus() { + updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -822,29 +824,6 @@ bool NormalLaneChange::hasEnoughLength( return false; } - if (lane_change_parameters_->regulate_on_crosswalk) { - const double dist_to_crosswalk_from_lane_change_start_pose = - utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - - path.info.length.prepare; - // Check lane changing section includes crosswalk - if ( - dist_to_crosswalk_from_lane_change_start_pose > 0.0 && - dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { - return false; - } - } - - if (lane_change_parameters_->regulate_on_intersection) { - const double dist_to_intersection_from_lane_change_start_pose = - utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; - // Check lane changing section includes intersection - if ( - dist_to_intersection_from_lane_change_start_pose > 0.0 && - dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { - return false; - } - } - return true; } @@ -1096,14 +1075,22 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_crosswalk && !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { RCLCPP_DEBUG(logger_, "Including crosswalk!!"); - continue; + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + continue; + } + RCLCPP_WARN_STREAM( + logger_, "Stop time is over threshold. Allow lane change in crosswalk."); } if ( lane_change_parameters_->regulate_on_intersection && !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { RCLCPP_DEBUG(logger_, "Including intersection!!"); - continue; + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + continue; + } + RCLCPP_WARN_STREAM( + logger_, "Stop time is over threshold. Allow lane change in intersection."); } if (utils::lane_change::passParkedObject( @@ -1463,4 +1450,24 @@ void NormalLaneChange::setStopPose(const Pose & stop_pose) lane_change_stop_pose_ = stop_pose; } +void NormalLaneChange::updateStopTime() +{ + const auto current_vel = getEgoVelocity(); + + if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { + stop_time_ = 0.0; + } else { + const double duration = stop_watch_.toc("stop_time"); + // clip stop time + if (stop_time_ + duration * 0.001 > lane_change_parameters_->stop_time_threshold) { + constexpr double eps = 0.1; + stop_time_ = lane_change_parameters_->stop_time_threshold + eps; + } else { + stop_time_ += duration * 0.001; + } + } + + stop_watch_.tic("stop_time"); +} + } // namespace behavior_path_planner