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

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -4,8 +4,8 @@
       common:
         show_processing_time: false # [-] whether to show processing time
         # param for input data
-        enable_rtc: true # 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
         traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal
+        enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc.
 
       # param for stop position
       stop_position:
@@ -38,7 +38,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
 

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -77,6 +77,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 =

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -832,6 +832,10 @@
 {
   const auto & objects_ptr = planner_data_->predicted_objects;
 
+  const auto traffic_lights_reg_elems =
+    crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>();
+  const bool has_traffic_light = !traffic_lights_reg_elems.empty();
+
   // Check if ego is yielding
   const bool is_ego_yielding = [&]() {
     const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold;
@@ -846,7 +850,8 @@
     const auto obj_uuid = toHexString(object.object_id);
     const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear;
     object_info_manager_.update(
-      obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, planner_param_);
+      obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, has_traffic_light,
+      planner_param_);
   }
   object_info_manager_.finalize();
 }

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -82,6 +82,7 @@
     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
@@ -98,12 +99,12 @@
   {
     // 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(
       const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding,
-      const PlannerParam & planner_param)
+      const bool has_traffic_light, const PlannerParam & planner_param)
     {
       const bool is_stopped = obj_vel < planner_param.stop_object_velocity;
 
@@ -117,7 +118,9 @@
         }
         const bool intent_to_cross =
           (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk;
-        if ((is_ego_yielding || planner_param.disable_stop_for_yield_cancel) && !intent_to_cross) {
+        if (
+          (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) &&
+          !intent_to_cross) {
           state = State::FULLY_STOPPED;
         } else {
           // NOTE: Object may start moving
@@ -134,18 +137,24 @@
     void init() { current_uuids_.clear(); }
     void update(
       const std::string & uuid, const double obj_vel, const rclcpp::Time & now,
-      const bool is_ego_yielding, const PlannerParam & planner_param)
+      const bool is_ego_yielding, const bool has_traffic_light, const PlannerParam & planner_param)
     {
       // update current uuids
       current_uuids_.push_back(uuid);
 
       // 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
-      objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, planner_param);
+      objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, has_traffic_light, planner_param);
     }
     void finalize()
     {