feat: keep stop policy v0.11.2 (#1125)

* feat(avoidance): keep stopping until all shift lines are registered Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(map_loader): display curbstone as marker array (autowarefoundation#4958) display curbstone as marker array Signed-off-by: Shohei Sakai <saka1s.jp@gmail.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> * revert refactoring due to cherry-pick * feat(ndt_scan_matcher): use glog (autowarefoundation#5465) (#1031) * feat(ndt_scan_matcher): use glog * style(pre-commit): autofix * update * style(pre-commit): autofix --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> 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 <maxime.clement@tier4.jp> * Improve reuse of the previously inserted stop point Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * Fix precision of inserted stop point Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> --------- Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> * fix(system_monitor): output command line (autowarefoundation#5430) (#1057) * fix(system_monitor): output command line * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: takeshi-iwanari <takeshi.iwanari@tier4.jp> 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 <yoshiyoshidetteiu@gmail.com> * feat(intersection): check path margin for overshoot vehicles on red light (autowarefoundation#5394) (#1056) Co-authored-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fix(ndt_scan_matcher): delete diagnostics thread (autowarefoundation#5532) Signed-off-by: yamato-ando <Yamato ANDO> Co-authored-by: yamato-ando <Yamato ANDO> * fix(ndt_scan_matcher): delete unmerged feature * fix(ndt_scan_matcher): fixed a lock scope in update_ndt (autowarefoundation#5951) Fixed the lock scope in update_ndt Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * fix(static_drivable_area_expansion): fix bug in expansion logic for hatched road marking (autowarefoundation#5842) (#1075) fix(utils): fix drivable area expansion logic for zebra zone Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(bvp): traffic light state debug (#1083) Signed-off-by: Mehmet Dogru <mdogru@leodrive.ai> * fix(traffic_light): stop if the traffic light signal timed out (autowarefoundation#5819) (#1124) * fix(traffic_light): stop if the traffic light signal timed out * fix(traffic_light): fix README format --------- Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> * refactor(mpc_lateral_controller): add debug info of qp solver (autowarefoundation#5459) (#1098) * add debug info of qp solver * no info for EigenLeastSquareLLT * return 0 in base class --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: Shohei Sakai <saka1s.jp@gmail.com> Signed-off-by: kminoda <koji.minoda@tier4.jp> Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp> Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> Signed-off-by: yamato-ando <Yamato ANDO> Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> Signed-off-by: Mehmet Dogru <mdogru@leodrive.ai> Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> Co-authored-by: satoshi-ota <satoshi.ota928@gmail.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Tomohito ANDO <tomohito.ando@tier4.jp> 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 <takeshi.iwanari@tier4.jp> Co-authored-by: yoshiri <yoshiyoshidetteiu@gmail.com> Co-authored-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Co-authored-by: Yamato Ando <yamato.ando@gmail.com> Co-authored-by: Shinnosuke Hirakawa <shinnosuke.hirakawa@tier4.jp> Co-authored-by: SakodaShintaro <shintaro.sakoda@tier4.jp> 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 <rej55.g@gmail.com> Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
tier4 · Mar 11, 2024 · 23d651f · 23d651f
1 parent eafcf88
commit 23d651f
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 2 deletions.
diff --git 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/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_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
@@ -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
@@ -218,6 +218,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
   // policy
   {
     std::string ns = "avoidance.policy.";
+    p.policy_approval = getOrDeclareParameter<std::string>(*node, ns + "approval");
     p.policy_deceleration = getOrDeclareParameter<std::string>(*node, ns + "deceleration");
     p.policy_lateral_margin = getOrDeclareParameter<std::string>(*node, ns + "lateral_margin");
     p.use_shorten_margin_immediately =