fix(avoidance): fix bugs in parked vehicle filtering logic (autowaref…

…oundation#7072) * fix(avoidance): check prev/next lane Signed-off-by: satoshi-ota <[email protected]> * fix(avoidance): fix parked vehicle filtering Signed-off-by: satoshi-ota <[email protected]> * fix(avoidance): check intersection location Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
Interplai · Jun 3, 2024 · 8510a7f · 8510a7f
1 parent f6156df
commit 8510a7f
Showing 1 changed file with 80 additions and 15 deletions.
diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
@@ -318,25 +318,55 @@ bool isWithinCrosswalk(
 bool isWithinIntersection(
   const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
 {
-  const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else");
-  if (id == "else") {
+  const std::string area_id = object.overhang_lanelet.attributeOr("intersection_area", "else");
+  if (area_id == "else") {
+    return false;
+  }
+
+  const std::string location = object.overhang_lanelet.attributeOr("location", "else");
+  if (location == "private") {
     return false;
   }
 
   const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);
 
-  const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str()));
+  const auto polygon =
+    route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str()));
 
   return boost::geometry::within(
     object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
 }
 
-bool isOnEgoLane(const ObjectData & object)
+bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
 {
   const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
-  return boost::geometry::within(
-    lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
-    object.overhang_lanelet.polygon2d().basicPolygon());
+  if (boost::geometry::within(
+        lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
+        object.overhang_lanelet.polygon2d().basicPolygon())) {
+    return true;
+  }
+
+  // push previous lanelet
+  lanelet::ConstLanelets prev_lanelet;
+  if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) {
+    if (boost::geometry::within(
+          lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
+          prev_lanelet.front().polygon2d().basicPolygon())) {
+      return true;
+    }
+  }
+
+  // push next lanelet
+  lanelet::ConstLanelet next_lanelet;
+  if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) {
+    if (boost::geometry::within(
+          lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
+          next_lanelet.polygon2d().basicPolygon())) {
+      return true;
+    }
+  }
+
+  return false;
 }
 
 bool isParallelToEgoLane(const ObjectData & object, const double threshold)
@@ -604,14 +634,49 @@ bool isNeverAvoidanceTarget(
 
   if (object.is_on_ego_lane) {
     const auto right_lane =
-      planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false);
+      planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, true);
+    if (right_lane.has_value() && isOnRight(object)) {
+      const lanelet::Attribute & right_lane_sub_type =
+        right_lane.value().attribute(lanelet::AttributeName::Subtype);
+      if (right_lane_sub_type != "road_shoulder") {
+        object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
+        RCLCPP_DEBUG(
+          rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
+        return true;
+      }
+
+      const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);
+      const auto is_disjoint_right_lane =
+        boost::geometry::disjoint(object_polygon, right_lane.value().polygon2d().basicPolygon());
+      if (is_disjoint_right_lane) {
+        object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
+        RCLCPP_DEBUG(
+          rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
+        return true;
+      }
+    }
+
     const auto left_lane =
-      planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false);
-    if (right_lane.has_value() && left_lane.has_value()) {
-      object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
-      RCLCPP_DEBUG(
-        rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
-      return true;
+      planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true);
+    if (left_lane.has_value() && !isOnRight(object)) {
+      const lanelet::Attribute & left_lane_sub_type =
+        left_lane.value().attribute(lanelet::AttributeName::Subtype);
+      if (left_lane_sub_type != "road_shoulder") {
+        object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
+        RCLCPP_DEBUG(
+          rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
+        return true;
+      }
+
+      const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object);
+      const auto is_disjoint_left_lane =
+        boost::geometry::disjoint(object_polygon, left_lane.value().polygon2d().basicPolygon());
+      if (is_disjoint_left_lane) {
+        object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
+        RCLCPP_DEBUG(
+          rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
+        return true;
+      }
     }
   }
 
@@ -754,7 +819,7 @@ bool isSatisfiedWithVehicleCondition(
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   object.behavior = getObjectBehavior(object, parameters);
-  object.is_on_ego_lane = isOnEgoLane(object);
+  object.is_on_ego_lane = isOnEgoLane(object, planner_data->route_handler);
 
   if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) {
     return false;