feat(crosswalk_module): consider objects on crosswalk when pedestrian traffic light is red (#10332)

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

Author: mehmetdogru

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -46,6 +46,8 @@
         ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering
         ego_min_assumed_speed: 2.0 # [m/s] assumed speed to calculate the time to collision point
 
+        consider_obj_on_crosswalk_on_red_light: true # consider and do not ignore objects if they are on the crosswalk when the crosswalk pedestrian traffic light is red
+
         no_stop_decision: # parameters to determine stop cancel. {-$overrun_threshold_length + f($min_acc, $min_jerk)} is compared against distance to stop pose.
           min_acc: -1.5 # min acceleration [m/ss]
           min_jerk: -1.5 # min jerk [m/sss]

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp

@@ -106,6 +106,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     get_or_declare_parameter<double>(node, ns + ".pass_judge.ego_pass_later_additional_margin");
   cp.ego_min_assumed_speed =
     get_or_declare_parameter<double>(node, ns + ".pass_judge.ego_min_assumed_speed");
+  cp.consider_obj_on_crosswalk_on_red_light =
+    get_or_declare_parameter<bool>(node, ns + ".pass_judge.consider_obj_on_crosswalk_on_red_light");
   cp.min_acc_for_no_stop_decision =
     get_or_declare_parameter<double>(node, ns + ".pass_judge.no_stop_decision.min_acc");
   cp.min_jerk_for_no_stop_decision =

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1112,6 +1112,7 @@ void CrosswalkModule::updateObjectState(
   const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,
   const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area)
 {
+  const auto & p = planner_param_;
   const auto & objects_ptr = planner_data_->predicted_objects;
 
   const auto traffic_lights_reg_elems =
@@ -1120,37 +1121,48 @@ void CrosswalkModule::updateObjectState(
 
   // 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;
+    const auto has_reached_stop_point = dist_ego_to_stop < p.stop_position_threshold;
 
-    return planner_data_->isVehicleStopped(planner_param_.timeout_ego_stop_for_yield) &&
-           has_reached_stop_point;
+    return planner_data_->isVehicleStopped(p.timeout_ego_stop_for_yield) && has_reached_stop_point;
   }();
 
-  const auto ignore_crosswalk = isRedSignalForPedestrians();
-  debug_data_.ignore_crosswalk = ignore_crosswalk;
+  const auto is_red_signal_for_pedestrians = isRedSignalForPedestrians();
+  auto ignore_crosswalk = is_red_signal_for_pedestrians;
 
   // Update object state
   object_info_manager_.init();
   for (const auto & object : objects_ptr->objects) {
-    const auto obj_uuid = to_hex_string(object.object_id);
-    const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position;
-    const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear;
-
-    // calculate collision point and state
     if (!isCrosswalkUserType(object)) {
       continue;
     }
-    if (ignore_crosswalk) {
+
+    auto ignore_obj = is_red_signal_for_pedestrians;
+    if (p.consider_obj_on_crosswalk_on_red_light && is_red_signal_for_pedestrians) {
+      const auto is_object_on_crosswalk =
+        bg::intersects(autoware_utils::to_polygon2d(object), crosswalk_.polygon2d().basicPolygon());
+
+      if (is_object_on_crosswalk) {
+        ignore_obj = false;
+        ignore_crosswalk = false;
+      }
+    }
+
+    if (ignore_obj) {
       continue;
     }
 
+    const auto obj_uuid = to_hex_string(object.object_id);
+    const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position;
+    const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear;
+
+    // calculate collision point and state
     const auto collision_point =
       getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area);
     const std::optional<double> ego_crosswalk_passage_direction =
       findEgoPassageDirectionAlongPath(sparse_resample_path);
     object_info_manager_.update(
       obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding,
-      has_traffic_light, collision_point, object.classification.front().label, planner_param_,
+      has_traffic_light, collision_point, object.classification.front().label, p,
       crosswalk_.polygon2d().basicPolygon(), attention_area, ego_crosswalk_passage_direction);
 
     const auto collision_state = object_info_manager_.getCollisionState(obj_uuid);
@@ -1178,6 +1190,7 @@ void CrosswalkModule::updateObjectState(
       getLabelColor(collision_state));
   }
 
+  debug_data_.ignore_crosswalk = ignore_crosswalk;
   object_info_manager_.finalize();
 }
 

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -149,6 +149,7 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC
     std::vector<double> ego_pass_later_margin_y;
     double ego_pass_later_additional_margin;
     double ego_min_assumed_speed;
+    bool consider_obj_on_crosswalk_on_red_light;
     double min_acc_for_no_stop_decision;
     double min_jerk_for_no_stop_decision;
     double overrun_threshold_length_for_no_stop_decision;