feat(crosswalk): add option to disable yield for new stopped object (a…

…utowarefoundation#4465) * feat(crosswalk): disable cancel yield at no traffic signals intersection Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(crosswalk): add option to disable yield for new stopped object Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update config Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tier4 · Aug 9, 2023 · 05a0bbb · 05a0bbb
1 parent 1622b2a
commit 05a0bbb
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 3 deletions.
diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp
@@ -80,6 +80,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   cp.min_object_velocity = node.declare_parameter<double>(ns + ".pass_judge.min_object_velocity");
   cp.disable_stop_for_yield_cancel =
     node.declare_parameter<bool>(ns + ".pass_judge.disable_stop_for_yield_cancel");
+  cp.disable_yield_for_new_stopped_object =
+    node.declare_parameter<bool>(ns + ".pass_judge.disable_yield_for_new_stopped_object");
   cp.timeout_no_intention_to_walk =
     node.declare_parameter<double>(ns + ".pass_judge.timeout_no_intention_to_walk");
   cp.timeout_ego_stop_for_yield =

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -85,6 +85,7 @@ class CrosswalkModule : public SceneModuleInterface
     double stop_object_velocity;
     double min_object_velocity;
     bool disable_stop_for_yield_cancel;
+    bool disable_yield_for_new_stopped_object;
     double timeout_no_intention_to_walk;
     double timeout_ego_stop_for_yield;
     // param for input data
@@ -101,7 +102,7 @@ class CrosswalkModule : public SceneModuleInterface
   {
     // NOTE: FULLY_STOPPED means stopped object which can be ignored.
     enum class State { STOPPED = 0, FULLY_STOPPED, OTHER };
-    State state{State::OTHER};
+    State state;
     std::optional<rclcpp::Time> time_to_start_stopped{std::nullopt};
 
     void updateState(
@@ -146,7 +147,13 @@ class CrosswalkModule : public SceneModuleInterface
 
       // add new object
       if (objects.count(uuid) == 0) {
-        objects.emplace(uuid, ObjectInfo{});
+        if (
+          has_traffic_light && planner_param.disable_stop_for_yield_cancel &&
+          planner_param.disable_yield_for_new_stopped_object) {
+          objects.emplace(uuid, ObjectInfo{ObjectInfo::State::FULLY_STOPPED});
+        } else {
+          objects.emplace(uuid, ObjectInfo{ObjectInfo::State::OTHER});
+        }
       }
 
       // update object state

diff --git a/planning/behavior_velocity_planner/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/config/crosswalk.param.yaml
@@ -41,7 +41,8 @@
         stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph)
         min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph)
         ## param for yielding
-        disable_stop_for_yield_cancel: false
+        disable_stop_for_yield_cancel: false # for the crosswalk where there is a traffic signal
+        disable_yield_for_new_stopped_object: false # for the crosswalk where there is a traffic signal
         timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first.
         timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not