change inside to overlaps to check point inside lanelet

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/route_handler/src/route_handler.cpp

@@ -27,6 +27,7 @@
 #include <autoware_planning_msgs/msg/lanelet_primitive.hpp>
 
 #include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/Polygon.h>
 #include <lanelet2_core/primitives/LaneletSequence.h>
 #include <lanelet2_routing/Route.h>
 #include <lanelet2_routing/RoutingGraph.h>
@@ -716,8 +717,9 @@ lanelet::ConstLanelets RouteHandler::getRoadLaneletsAtPose(const Pose & pose) co
   const lanelet::BasicPoint2d p{pose.position.x, pose.position.y};
   const auto lanelets_at_pose = lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p));
   for (const auto & lanelet_at_pose : lanelets_at_pose) {
-    // confirm that the pose is inside the lanelet since "search" does an approximation with boxes
-    const auto is_pose_inside_lanelet = lanelet::geometry::inside(lanelet_at_pose, p);
+    // confirm that the pose is on the lanelet since "search" does an approximation with boxes
+    const auto is_pose_inside_lanelet =
+      lanelet::geometry::overlaps(p, lanelet_at_pose.polygon2d().basicPolygon());
     if (is_pose_inside_lanelet && isRoadLanelet(lanelet_at_pose))
       road_lanelets_at_pose.push_back(lanelet_at_pose);
   }
@@ -768,8 +770,9 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletsAtPose(const Pose & pose
   const lanelet::BasicPoint2d p{pose.position.x, pose.position.y};
   const auto candidates_at_pose = lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p));
   for (const auto & candidate : candidates_at_pose) {
-    // confirm that the pose is inside the lanelet since "search" does an approximation with boxes
-    const auto is_pose_inside_lanelet = lanelet::geometry::inside(candidate, p);
+    // confirm that the pose is on the lanelet since "search" does an approximation with boxes
+    const auto is_pose_inside_lanelet =
+      lanelet::geometry::overlaps(p, candidate.polygon2d().basicPolygon());
     if (is_pose_inside_lanelet && isShoulderLanelet(candidate))
       lanelets_at_pose.push_back(candidate);
   }