chore(crosswalk): port the same direction ignore block

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -335,36 +335,18 @@
     dist_default_stop, sparse_resample_path, crosswalk_attention_range, attention_area);
 
   // Check pedestrian for stop
   // NOTE: first stop point and its minimum distance from ego to stop
-  auto isVehicleType = [](const uint8_t label) {
-    return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE;
-  };
   std::optional<double> dist_nearest_cp;
   std::vector<geometry_msgs::msg::Point> stop_factor_points;
-  const std::optional<double> ego_crosswalk_passage_direction =
-    findEgoPassageDirectionAlongPath(sparse_resample_path);
   for (const auto & object : object_info_manager_.getObject()) {
     const auto & collision_point_opt = object.collision_point;
     if (collision_point_opt) {
       const auto & collision_point = collision_point_opt.value();
       const auto & collision_state = object.collision_state;
       if (collision_state != CollisionState::YIELD) {
         continue;
       }
 
-      if (
-        isVehicleType(object.classification) && ego_crosswalk_passage_direction &&
-        collision_point.crosswalk_passage_direction) {
-        double direction_diff = std::abs(std::fmod(
-          collision_point.crosswalk_passage_direction.value() -
-            ego_crosswalk_passage_direction.value(),
-          M_PI_2));
-        direction_diff = std::min(direction_diff, M_PI_2 - direction_diff);
-        if (direction_diff < planner_param_.vehicle_object_cross_angle_threshold) {
-          continue;
-        }
-      }
-
       stop_factor_points.push_back(object.position);
 
       const auto dist_ego2cp =
@@ -1165,10 +1147,12 @@
 
     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_,
-      crosswalk_.polygon2d().basicPolygon(), attention_area);
+      crosswalk_.polygon2d().basicPolygon(), attention_area, ego_crosswalk_passage_direction);
 
     const auto collision_state = object_info_manager_.getCollisionState(obj_uuid);
     if (collision_point) {

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -194,7 +194,8 @@
       const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel,
       const bool is_ego_yielding, const std::optional<CollisionPoint> & collision_point,
       const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon,
-      const bool is_object_away_from_path)
+      const bool is_object_away_from_path,
+      const std::optional<double> & ego_crosswalk_passage_direction)
     {
       const bool is_stopped = vel < planner_param.stop_object_velocity;
 
@@ -224,6 +225,24 @@
 
       // Compare time to collision and vehicle
       if (collision_point) {
+        auto isVehicleType = [](const uint8_t label) {
+          return label == ObjectClassification::MOTORCYCLE ||
+                 label == ObjectClassification::BICYCLE;
+        };
+        if (
+          isVehicleType(classification) && ego_crosswalk_passage_direction &&
+          collision_point->crosswalk_passage_direction) {
+          double direction_diff = std::abs(std::fmod(
+            collision_point->crosswalk_passage_direction.value() -
+              ego_crosswalk_passage_direction.value(),
+            M_PI_2));
+          direction_diff = std::min(direction_diff, M_PI_2 - direction_diff);
+          if (direction_diff < planner_param.vehicle_object_cross_angle_threshold) {
+            collision_state = CollisionState::IGNORE;
+            return;
+          }
+        }
+
         // Check if ego will pass first
         const double ego_pass_first_additional_margin =
           collision_state == CollisionState::EGO_PASS_FIRST
@@ -268,7 +287,8 @@
       const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light,
       const std::optional<CollisionPoint> & collision_point, const uint8_t classification,
       const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon,
-      const Polygon2d & attention_area)
+      const Polygon2d & attention_area,
+      const std::optional<double> & ego_crosswalk_passage_direction)
     {
       // update current uuids
       current_uuids_.push_back(uuid);
@@ -292,7 +312,7 @@
       // update object state
       objects.at(uuid).transitState(
         now, position, vel, is_ego_yielding, collision_point, planner_param, crosswalk_polygon,
-        is_object_away_from_path);
+        is_object_away_from_path, ego_crosswalk_passage_direction);
       objects.at(uuid).collision_point = collision_point;
       objects.at(uuid).position = position;
       objects.at(uuid).classification = classification;