chore(crosswalk): port the same direction ignore block (#9983)

Signed-off-by: yuki-takagi-66 <[email protected]>
autowarefoundation · Jan 21, 2025 · e86f92b · e86f92b
1 parent ce3358e
commit e86f92b
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 22 deletions.
diff --git a/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -337,13 +337,8 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
 
   // 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) {
@@ -353,19 +348,6 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
         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 =
@@ -1167,10 +1149,12 @@ void CrosswalkModule::updateObjectState(
 
     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) {

diff --git a/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -194,7 +194,8 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC
       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 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC
 
       // 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 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC
       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 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC
       // 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;