fix: is_in_parking_lot check method (#992)

Signed-off-by: NorahXiong <norah.xiong@autocore.ai>

map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp

@@ -160,6 +160,11 @@ lanelet::ConstLanelets getLinkedLanelets(
 bool getLinkedParkingLot(
   const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots,
   lanelet::ConstPolygon3d * linked_parking_lot);
+// get linked parking lot from current pose of ego car
+bool getLinkedParkingLot(
+  const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots,
+  lanelet::ConstPolygon3d * linked_parking_lot);
+// get linked parking lot from parking space
 bool getLinkedParkingLot(
   const lanelet::ConstLineString3d & parking_space,
   const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot);

map/lanelet2_extension/lib/query.cpp

@@ -450,6 +450,22 @@ bool query::getLinkedParkingLot(
   return false;
 }
 
+// get overlapping parking lot
+bool query::getLinkedParkingLot(
+  const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots,
+  lanelet::ConstPolygon3d * linked_parking_lot)
+{
+  for (const auto & parking_lot : all_parking_lots) {
+    const double distance =
+      boost::geometry::distance(current_position, to2D(parking_lot).basicPolygon());
+    if (distance < std::numeric_limits<double>::epsilon()) {
+      *linked_parking_lot = parking_lot;
+      return true;
+    }
+  }
+  return false;
+}
+
 // get overlapping parking lot
 bool query::getLinkedParkingLot(
   const lanelet::ConstLineString3d & parking_space,

planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp

@@ -93,21 +93,13 @@ geometry_msgs::msg::PoseStamped::ConstSharedPtr getCurrentPose(
 // copied from scenario selector
 std::shared_ptr<lanelet::ConstPolygon3d> findNearestParkinglot(
   const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
-  const lanelet::BasicPoint2d & search_point)
+  const lanelet::BasicPoint2d & current_position)
 {
-  std::vector<std::pair<double, lanelet::Lanelet>> nearest_lanelets =
-    lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 1);
-
-  if (nearest_lanelets.empty()) {
-    return {};
-  }
-
-  const auto nearest_lanelet = nearest_lanelets.front().second;
   const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr);
 
   const auto linked_parking_lot = std::make_shared<lanelet::ConstPolygon3d>();
   const auto result = lanelet::utils::query::getLinkedParkingLot(
-    nearest_lanelet, all_parking_lots, linked_parking_lot.get());
+    current_position, all_parking_lots, linked_parking_lot.get());
 
   if (result) {
     return linked_parking_lot;

planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp

@@ -39,21 +39,13 @@ void onData(const T & data, T * buffer)
 
 std::shared_ptr<lanelet::ConstPolygon3d> findNearestParkinglot(
   const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
-  const lanelet::BasicPoint2d & search_point)
+  const lanelet::BasicPoint2d & current_position)
 {
-  std::vector<std::pair<double, lanelet::Lanelet>> nearest_lanelets =
-    lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 1);
-
-  if (nearest_lanelets.empty()) {
-    return {};
-  }
-
-  const auto nearest_lanelet = nearest_lanelets.front().second;
   const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr);
 
   const auto linked_parking_lot = std::make_shared<lanelet::ConstPolygon3d>();
   const auto result = lanelet::utils::query::getLinkedParkingLot(
-    nearest_lanelet, all_parking_lots, linked_parking_lot.get());
+    current_position, all_parking_lots, linked_parking_lot.get());
 
   if (result) {
     return linked_parking_lot;