feat(crosswalk): ignore predicted path going across the crosswalk (#5849

)

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -63,6 +63,7 @@
       # param for target object filtering
       object_filtering:
         crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk
+        vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle
         target_object:
           unknown: true # [-] whether to look and stop by UNKNOWN objects
           bicycle: true # [-] whether to look and stop by BICYCLE objects

planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp

@@ -50,6 +50,8 @@ enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE };
 struct CollisionPoint
 {
   geometry_msgs::msg::Point collision_point{};
+  std::optional<double> crosswalk_passage_direction{
+    std::nullopt};  // denote obj is passing the crosswalk along the vehicle lane
   double time_to_collision{};
   double time_to_vehicle{};
 };

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -116,6 +116,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   // param for target area & object
   cp.crosswalk_attention_range =
     getOrDeclareParameter<double>(node, ns + ".object_filtering.crosswalk_attention_range");
+  cp.vehicle_object_cross_angle_threshold = getOrDeclareParameter<double>(
+    node, ns + ".object_filtering.vehicle_object_cross_angle_threshold");
   cp.look_unknown =
     getOrDeclareParameter<bool>(node, ns + ".object_filtering.target_object.unknown");
   cp.look_bicycle =

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -25,6 +25,7 @@
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
+#include <lanelet2_core/geometry/LineString.h>
 #include <lanelet2_routing/RoutingGraph.h>
 #include <lanelet2_routing/RoutingGraphContainer.h>
 
@@ -316,6 +317,8 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   const double ego_vel = planner_data_->current_velocity->twist.linear.x;
   const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;
 
+  const std::optional<double> ego_crosswalk_passage_direction =
+    findEgoPassageDirectionAlongPath(sparse_resample_path);
   const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   const auto dist_ego_to_stop =
     calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second;
@@ -349,25 +352,44 @@ 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<std::pair<geometry_msgs::msg::Point, double>> nearest_stop_info;
   std::vector<geometry_msgs::msg::Point> stop_factor_points;
   for (const auto & object : object_info_manager_.getObject()) {
-    const auto & collision_point = object.collision_point;
-    if (collision_point) {
+    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) {
+        const double direction_diff = tier4_autoware_utils::normalizeRadian(
+          collision_point.crosswalk_passage_direction.value() -
+          ego_crosswalk_passage_direction.value());
+        RCLCPP_INFO(
+          rclcpp::get_logger("temp"),
+          "collision_point.crosswalk_passage_direction = %f, ego_crosswalk_passage_direction = %f, "
+          "direction_diff = %f",
+          collision_point.crosswalk_passage_direction.value(),
+          ego_crosswalk_passage_direction.value(), direction_diff);
+        if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) {
+          continue;
+        }
+      }
 
       stop_factor_points.push_back(object.position);
 
       const auto dist_ego2cp =
-        calcSignedArcLength(
-          sparse_resample_path.points, ego_pos, collision_point->collision_point) -
+        calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point) -
         planner_param_.stop_distance_from_object;
       if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) {
         nearest_stop_info =
-          std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front);
+          std::make_pair(collision_point.collision_point, dist_ego2cp - base_link2front);
       }
     }
   }
@@ -580,6 +602,70 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
   return std::make_pair(clamped_near_attention_range, clamped_far_attention_range);
 }
 
+std::optional<double> CrosswalkModule::findEgoPassageDirectionAlongPath(
+  const PathWithLaneId & path) const
+{
+  auto findIntersectPoint = [&](const lanelet::ConstLineString3d line)
+    -> std::optional<std::pair<size_t, geometry_msgs::msg::Point>> {
+    const auto line_start =
+      tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z());
+    const auto line_end =
+      tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z());
+    for (unsigned i = 0; i < path.points.size() - 1; ++i) {
+      const auto & start = path.points.at(i).point.pose.position;
+      const auto & end = path.points.at(i + 1).point.pose.position;
+      if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end);
+          intersect.has_value()) {
+        return std::make_optional(std::make_pair(i, intersect.value()));
+      }
+    }
+    return std::nullopt;
+  };
+  const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound());
+  const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound());
+
+  if (!intersect_pt1 || !intersect_pt2) {
+    return std::nullopt;
+  }
+  const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first;
+  const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second;
+  const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second;
+  return std::atan2(back.y - front.y, back.x - front.x);
+}
+
+std::optional<double> CrosswalkModule::findObjectPassageDirectionAlongVehicleLane(
+  const autoware_auto_perception_msgs::msg::PredictedPath & path) const
+{
+  using tier4_autoware_utils::Segment2d;
+
+  auto findIntersectPoint = [&](const lanelet::ConstLineString3d line)
+    -> std::optional<std::pair<size_t, geometry_msgs::msg::Point>> {
+    const auto line_start =
+      tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z());
+    const auto line_end =
+      tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z());
+    for (unsigned i = 0; i < path.path.size() - 1; ++i) {
+      const auto & start = path.path.at(i).position;
+      const auto & end = path.path.at(i + 1).position;
+      if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end);
+          intersect.has_value()) {
+        return std::make_optional(std::make_pair(i, intersect.value()));
+      }
+    }
+    return std::nullopt;
+  };
+  const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound());
+  const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound());
+
+  if (!intersect_pt1 || !intersect_pt2) {
+    return std::nullopt;
+  }
+  const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first;
+  const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second;
+  const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second;
+  return std::atan2(back.y - front.y, back.x - front.x);
+}
+
 std::optional<CollisionPoint> CrosswalkModule::getCollisionPoint(
   const PathWithLaneId & ego_path, const PredictedObject & object,
   const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area)
@@ -625,6 +711,8 @@ std::optional<CollisionPoint> CrosswalkModule::getCollisionPoint(
       // Calculate multi-step object polygon, and reset start_idx
       const size_t start_idx_with_margin = std::max(static_cast<int>(start_idx) - 1, 0);
       const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1);
+      const auto object_crosswalk_passage_direction =
+        findObjectPassageDirectionAlongVehicleLane(obj_path);
       const auto obj_multi_step_polygon = createMultiStepPolygon(
         obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin);
       is_start_idx_initialized = false;
@@ -660,7 +748,8 @@ std::optional<CollisionPoint> CrosswalkModule::getCollisionPoint(
       if (dist_ego2cp < minimum_stop_dist) {
         minimum_stop_dist = dist_ego2cp;
         nearest_collision_point = createCollisionPoint(
-          intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel);
+          intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel,
+          object_crosswalk_passage_direction);
         debug_data_.obj_polygons.push_back(
           toGeometryPointVector(obj_multi_step_polygon, ego_pos.z));
       }
@@ -679,17 +768,18 @@ std::optional<CollisionPoint> CrosswalkModule::getCollisionPoint(
 CollisionPoint CrosswalkModule::createCollisionPoint(
   const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp,
   const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel,
-  const geometry_msgs::msg::Vector3 & obj_vel) const
+  const geometry_msgs::msg::Vector3 & obj_vel,
+  const std::optional<double> object_crosswalk_passage_direction) const
 {
   constexpr double min_ego_velocity = 1.38;  // [m/s]
-
-  const auto & base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
+  const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
 
   const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
   const auto velocity = std::max(planner_param_.min_object_velocity, estimated_velocity);
 
   CollisionPoint collision_point{};
   collision_point.collision_point = nearest_collision_point;
+  collision_point.crosswalk_passage_direction = object_crosswalk_passage_direction;
   collision_point.time_to_collision =
     std::max(0.0, dist_ego2cp - planner_param_.stop_distance_from_object - base_link2front) /
     std::max(ego_vel.x, min_ego_velocity);
@@ -931,7 +1021,8 @@ void CrosswalkModule::updateObjectState(
       getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area);
     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, planner_param_, crosswalk_.polygon2d().basicPolygon());
+      has_traffic_light, collision_point, object.classification.front().label, planner_param_,
+      crosswalk_.polygon2d().basicPolygon());
 
     if (collision_point) {
       const auto collision_state = object_info_manager_.getCollisionState(obj_uuid);

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -150,6 +150,7 @@ class CrosswalkModule : public SceneModuleInterface
     double traffic_light_state_timeout;
     // param for target area & object
     double crosswalk_attention_range;
+    double vehicle_object_cross_angle_threshold;
     bool look_unknown;
     bool look_bicycle;
     bool look_motorcycle;
@@ -160,6 +161,7 @@ class CrosswalkModule : public SceneModuleInterface
   {
     CollisionState collision_state{};
     std::optional<rclcpp::Time> time_to_start_stopped{std::nullopt};
+    uint8_t classification{ObjectClassification::UNKNOWN};
 
     geometry_msgs::msg::Point position{};
     std::optional<CollisionPoint> collision_point{};
@@ -242,8 +244,8 @@ class CrosswalkModule : public SceneModuleInterface
     void update(
       const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel,
       const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light,
-      const std::optional<CollisionPoint> & collision_point, const PlannerParam & planner_param,
-      const lanelet::BasicPolygon2d & crosswalk_polygon)
+      const std::optional<CollisionPoint> & collision_point, const uint8_t classification,
+      const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon)
     {
       // update current uuids
       current_uuids_.push_back(uuid);
@@ -265,6 +267,7 @@ class CrosswalkModule : public SceneModuleInterface
         crosswalk_polygon);
       objects.at(uuid).collision_point = collision_point;
       objects.at(uuid).position = position;
+      objects.at(uuid).classification = classification;
     }
     void finalize()
     {
@@ -330,6 +333,11 @@ class CrosswalkModule : public SceneModuleInterface
     const std::vector<geometry_msgs::msg::Point> & path_intersects,
     const std::optional<geometry_msgs::msg::Pose> & stop_pose) const;
 
+  std::optional<double> findEgoPassageDirectionAlongPath(
+    const PathWithLaneId & sparse_resample_path) const;
+  std::optional<double> findObjectPassageDirectionAlongVehicleLane(
+    const autoware_auto_perception_msgs::msg::PredictedPath & path) const;
+
   std::optional<CollisionPoint> getCollisionPoint(
     const PathWithLaneId & ego_path, const PredictedObject & object,
     const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area);
@@ -366,7 +374,8 @@ class CrosswalkModule : public SceneModuleInterface
   CollisionPoint createCollisionPoint(
     const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp,
     const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel,
-    const geometry_msgs::msg::Vector3 & obj_vel) const;
+    const geometry_msgs::msg::Vector3 & obj_vel,
+    const std::optional<double> object_crosswalk_passage_direction) const;
 
   float calcTargetVelocity(
     const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const;