From 23d651f72819dca8de63fabd2c1718a13070a9f6 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Mon, 11 Mar 2024 11:22:20 +0900 Subject: [PATCH] feat: keep stop policy v0.11.2 (#1125) * feat(avoidance): keep stopping until all shift lines are registered Signed-off-by: satoshi-ota * feat(map_loader): display curbstone as marker array (#4958) display curbstone as marker array Signed-off-by: Shohei Sakai Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> * revert refactoring due to cherry-pick * feat(ndt_scan_matcher): use glog (#5465) (#1031) * feat(ndt_scan_matcher): use glog * style(pre-commit): autofix * update * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat(out_of_lane): improve reuse of previous decision (#1017) * Do not directly reuse a prev stop point but project it on the new path Signed-off-by: Maxime CLEMENT * Improve reuse of the previously inserted stop point Signed-off-by: Maxime CLEMENT * Fix precision of inserted stop point Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Maxime CLEMENT * fix(system_monitor): output command line (#5430) (#1057) * fix(system_monitor): output command line * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari Co-authored-by: takeshi-iwanari Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * chore: pull tracking object merger from awf/main Signed-off-by: yoshiri * feat(intersection): check path margin for overshoot vehicles on red light (#5394) (#1056) Co-authored-by: Mamoru Sobue * fix(ndt_scan_matcher): delete diagnostics thread (#5532) Signed-off-by: yamato-ando Co-authored-by: yamato-ando * fix(ndt_scan_matcher): delete unmerged feature * fix(ndt_scan_matcher): fixed a lock scope in update_ndt (#5951) Fixed the lock scope in update_ndt Signed-off-by: Shintaro SAKODA * fix(static_drivable_area_expansion): fix bug in expansion logic for hatched road marking (#5842) (#1075) fix(utils): fix drivable area expansion logic for zebra zone Signed-off-by: satoshi-ota * fix(bvp): traffic light state debug (#1083) Signed-off-by: Mehmet Dogru * fix(traffic_light): stop if the traffic light signal timed out (#5819) (#1124) * fix(traffic_light): stop if the traffic light signal timed out * fix(traffic_light): fix README format --------- Signed-off-by: Fumiya Watanabe Signed-off-by: Tomohito Ando Co-authored-by: Fumiya Watanabe * refactor(mpc_lateral_controller): add debug info of qp solver (#5459) (#1098) * add debug info of qp solver * no info for EigenLeastSquareLLT * return 0 in base class --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kyoichi Sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Signed-off-by: satoshi-ota Signed-off-by: Shohei Sakai Signed-off-by: kminoda Signed-off-by: Maxime CLEMENT Signed-off-by: takeshi.iwanari Signed-off-by: yoshiri Signed-off-by: yamato-ando Signed-off-by: Shintaro SAKODA Signed-off-by: Mehmet Dogru Signed-off-by: Fumiya Watanabe Signed-off-by: Tomohito Ando Signed-off-by: kyoichi-sugahara Co-authored-by: satoshi-ota Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Tomohito ANDO Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: takeshi-iwanari Co-authored-by: yoshiri Co-authored-by: Mamoru Sobue Co-authored-by: Yamato Ando Co-authored-by: Shinnosuke Hirakawa Co-authored-by: SakodaShintaro Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: Kyoichi Sugahara --- .../config/avoidance/avoidance.param.yaml | 4 ++++ .../utils/avoidance/avoidance_module_data.hpp | 3 +++ .../scene_module/avoidance/avoidance_module.cpp | 16 ++++++++++++++-- .../src/scene_module/avoidance/manager.cpp | 1 + 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 7c8fa1ac18348..9ba7fb717b4bb 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -192,6 +192,10 @@ stop_buffer: 1.0 # [m] policy: + # policy for rtc request. select "shift_line" or "maneuver". + # "shift_line": request approval for each shift line. + # "maneuver": request approval for avoidance maneuver (avoid + return). + approval: "shift_line" # policy for vehicle slow down behavior. select "best_effort" or "reliable". # "best_effort": slow down deceleration & jerk are limited by constraints. # but there is a possibility that the vehicle can't stop in front of the vehicle. 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 d14a99eb80e73..c41829c8d491b 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 @@ -280,6 +280,9 @@ struct AvoidanceParameters // policy bool use_shorten_margin_immediately{false}; + // policy + std::string policy_approval{"shift_line"}; + // policy std::string policy_deceleration{"best_effort"}; 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 2143e42afd7b6..20dfb994ea114 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 @@ -85,6 +85,11 @@ bool isBestEffort(const std::string & policy) { return policy == "best_effort"; } + +bool perManeuver(const std::string & policy) +{ + return policy == "maneuver"; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -2316,9 +2321,16 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat break; } - if (!is_ignore_shift(candidate)) { - return get_subsequent_shift(i); + if (is_ignore_shift(candidate)) { + continue; } + + if (perManeuver(parameters_->policy_approval)) { + return candidates; + } + + const auto new_shift_lines = get_subsequent_shift(i); + return new_shift_lines; } DEBUG_PRINT("No new shift point exists."); 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 871932e1e2232..461341ff488e6 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -218,6 +218,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // policy { std::string ns = "avoidance.policy."; + p.policy_approval = getOrDeclareParameter(*node, ns + "approval"); p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); p.use_shorten_margin_immediately =