fix(crosswalk): don't stop in front of the crosswalk if vehicle stuck…

… in intersection (#5722)

Signed-off-by: satoshi-ota <[email protected]>

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -27,6 +27,7 @@
 
       # param for stuck vehicles
       stuck_vehicle:
+        enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection
         stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck
         max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked
         stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk

planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp

@@ -82,7 +82,7 @@ struct DebugData
   std::vector<std::vector<geometry_msgs::msg::Point>> obj_polygons;
 };
 
-std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
+std::vector<std::pair<int64_t, lanelet::ConstLanelet>> getCrosswalksOnPath(
   const geometry_msgs::msg::Pose & current_pose,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const lanelet::LaneletMapPtr lanelet_map,

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -63,6 +63,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   cp.no_relax_velocity = getOrDeclareParameter<double>(node, ns + ".slow_down.no_relax_velocity");
 
   // param for stuck vehicle
+  cp.enable_stuck_check_in_intersection =
+    getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.enable_stuck_check_in_intersection");
   cp.stuck_vehicle_velocity =
     getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_velocity");
   cp.max_stuck_vehicle_lateral_offset =
@@ -129,8 +131,9 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
   const auto rh = planner_data_->route_handler_;
 
   const auto launch = [this, &path](
-                        const auto lane_id, const std::optional<int64_t> & reg_elem_id) {
-    if (isModuleRegistered(lane_id)) {
+                        const auto road_lanelet_id, const auto crosswalk_lanelet_id,
+                        const std::optional<int64_t> & reg_elem_id) {
+    if (isModuleRegistered(crosswalk_lanelet_id)) {
       return;
     }
 
@@ -141,25 +144,27 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
     // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case
     //       where both regulatory element and non-regulatory element crosswalks exist.
     registerModule(std::make_shared<CrosswalkModule>(
-      node_, lane_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_));
-    generateUUID(lane_id);
+      node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger,
+      clock_));
+    generateUUID(crosswalk_lanelet_id);
     updateRTCStatus(
-      getUUID(lane_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
+      getUUID(crosswalk_lanelet_id), true, std::numeric_limits<double>::lowest(),
+      path.header.stamp);
   };
 
   const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
     path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
   for (const auto & crosswalk : crosswalk_leg_elem_map) {
     // NOTE: The former id is a lane id, and the latter one is a regulatory element's id.
-    launch(crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id());
+    launch(crosswalk.second.id(), crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id());
   }
 
   const auto crosswalk_lanelets = getCrosswalksOnPath(
     planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
 
   for (const auto & crosswalk : crosswalk_lanelets) {
-    launch(crosswalk.id(), std::nullopt);
+    launch(crosswalk.first, crosswalk.second.id(), std::nullopt);
   }
 }
 

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -176,9 +176,10 @@ tier4_debug_msgs::msg::StringStamped createStringStampedMessage(
 }  // namespace
 
 CrosswalkModule::CrosswalkModule(
-  rclcpp::Node & node, const int64_t module_id, const std::optional<int64_t> & reg_elem_id,
-  const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param,
-  const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock)
+  rclcpp::Node & node, const int64_t lane_id, const int64_t module_id,
+  const std::optional<int64_t> & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const PlannerParam & planner_param, const rclcpp::Logger & logger,
+  const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   module_id_(module_id),
   planner_param_(planner_param),
@@ -200,6 +201,8 @@ CrosswalkModule::CrosswalkModule(
     crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id);
   }
 
+  road_ = lanelet_map_ptr->laneletLayer.get(lane_id);
+
   collision_info_pub_ =
     node.create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
 }
@@ -803,6 +806,16 @@ std::optional<StopFactor> CrosswalkModule::checkStopForStuckVehicles(
     return {};
   }
 
+  // skip stuck vehicle checking for crosswalk which is in intersection.
+  if (!p.enable_stuck_check_in_intersection) {
+    std::string turn_direction = road_.attributeOr("turn_direction", "else");
+    if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
+      if (!road_.regulatoryElementsAs<const lanelet::TrafficLight>().empty()) {
+        return {};
+      }
+    }
+  }
+
   for (const auto & object : objects) {
     if (!isVehicle(object)) {
       continue;

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -121,6 +121,7 @@ class CrosswalkModule : public SceneModuleInterface
     double max_slow_down_accel;
     double no_relax_velocity;
     // param for stuck vehicle
+    bool enable_stuck_check_in_intersection{false};
     double stuck_vehicle_velocity;
     double max_stuck_vehicle_lateral_offset;
     double stuck_vehicle_attention_range;
@@ -299,9 +300,10 @@ class CrosswalkModule : public SceneModuleInterface
   };
 
   CrosswalkModule(
-    rclcpp::Node & node, const int64_t module_id, const std::optional<int64_t> & reg_elem_id,
-    const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param,
-    const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock);
+    rclcpp::Node & node, const int64_t lane_id, const int64_t module_id,
+    const std::optional<int64_t> & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+    const PlannerParam & planner_param, const rclcpp::Logger & logger,
+    const rclcpp::Clock::SharedPtr clock);
 
   bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
 
@@ -406,6 +408,8 @@ class CrosswalkModule : public SceneModuleInterface
 
   lanelet::ConstLanelet crosswalk_;
 
+  lanelet::ConstLanelet road_;
+
   lanelet::ConstLineStrings3d stop_lines_;
 
   // Parameter

planning/behavior_velocity_crosswalk_module/src/util.cpp

@@ -53,19 +53,19 @@ using tier4_autoware_utils::createPoint;
 using tier4_autoware_utils::Line2d;
 using tier4_autoware_utils::Point2d;
 
-std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
+std::vector<std::pair<int64_t, lanelet::ConstLanelet>> getCrosswalksOnPath(
   const geometry_msgs::msg::Pose & current_pose,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const lanelet::LaneletMapPtr lanelet_map,
   const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
 {
-  std::vector<lanelet::ConstLanelet> crosswalks;
+  std::vector<std::pair<lanelet::Id, lanelet::ConstLanelet>> crosswalks;
 
   // Add current lane id
   const auto nearest_lane_id =
     behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose);
 
-  std::vector<int64_t> unique_lane_ids;
+  std::vector<lanelet::Id> unique_lane_ids;
   if (nearest_lane_id) {
     // Add subsequent lane_ids from nearest lane_id
     unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath(
@@ -81,24 +81,24 @@ std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
     constexpr int PEDESTRIAN_GRAPH_ID = 1;
     const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID);
     for (const auto & crosswalk : conflicting_crosswalks) {
-      crosswalks.push_back(crosswalk);
+      crosswalks.emplace_back(lane_id, crosswalk);
     }
   }
 
   return crosswalks;
 }
 
-std::set<int64_t> getCrosswalkIdSetOnPath(
+std::set<lanelet::Id> getCrosswalkIdSetOnPath(
   const geometry_msgs::msg::Pose & current_pose,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const lanelet::LaneletMapPtr lanelet_map,
   const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
 {
-  std::set<int64_t> crosswalk_id_set;
+  std::set<lanelet::Id> crosswalk_id_set;
 
   for (const auto & crosswalk :
        getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) {
-    crosswalk_id_set.insert(crosswalk.id());
+    crosswalk_id_set.insert(crosswalk.second.id());
   }
 
   return crosswalk_id_set;

planning/behavior_velocity_walkway_module/src/manager.cpp

@@ -75,7 +75,7 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
     planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
 
   for (const auto & crosswalk : crosswalk_lanelets) {
-    launch(crosswalk, false);
+    launch(crosswalk.second, false);
   }
 }