feat(lanelet2_extension): consider lane-ego angle difference in lane …

…following module (autowarefoundation#148)

* feat(lanelet2_extension): consider lane-ego angle difference in lane following module

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

* remove debug prints

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

* include limits

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

* style(pre-commit): autofix

* fix: use bg:distance

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

* refactor: replace with getLaneletAngle

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

---------

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp

@@ -31,6 +31,7 @@
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_routing/RoutingGraph.h>
 
+#include <limits>
 #include <memory>
 #include <string>
 #include <vector>
@@ -238,6 +239,12 @@ bool getClosestLanelet(
   const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
   ConstLanelet * closest_lanelet_ptr);
 
+bool getClosestLaneletWithConstrains(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelet * closest_lanelet_ptr,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
+
 bool getCurrentLanelets(
   const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
   ConstLanelets * current_lanelets_ptr);

tmp/lanelet2_extension/lib/query.cpp

@@ -794,6 +794,74 @@ bool query::getClosestLanelet(
   return found;
 }
 
+bool query::getClosestLaneletWithConstrains(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelet * closest_lanelet_ptr, const double dist_threshold, const double yaw_threshold)
+{
+  bool found = false;
+
+  if (closest_lanelet_ptr == nullptr) {
+    std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet"
+              << std::endl;
+    return found;
+  }
+
+  if (lanelets.empty()) {
+    return found;
+  }
+
+  lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y);
+
+  // find by distance
+  std::vector<std::pair<lanelet::ConstLanelet, double>> candidate_lanelets;
+  {
+    for (const auto & llt : lanelets) {
+      double distance = boost::geometry::distance(llt.polygon2d().basicPolygon(), search_point);
+
+      if (distance <= dist_threshold) {
+        candidate_lanelets.emplace_back(llt, distance);
+      }
+    }
+
+    if (!candidate_lanelets.empty()) {
+      // sort by distance
+      std::sort(
+        candidate_lanelets.begin(), candidate_lanelets.end(),
+        [](
+          const std::pair<lanelet::ConstLanelet, double> & x,
+          std::pair<lanelet::ConstLanelet, double> & y) { return x.second < y.second; });
+    } else {
+      std::cerr << "there are no lanelets close enough!" << std::endl;
+      return found;
+    }
+  }
+
+  // find closest lanelet within yaw_threshold
+  {
+    double min_angle = std::numeric_limits<double>::max();
+    double min_distance = std::numeric_limits<double>::max();
+    double pose_yaw = tf2::getYaw(search_pose.orientation);
+    for (const auto & llt_pair : candidate_lanelets) {
+      const auto & distance = llt_pair.second;
+
+      double lanelet_angle = getLaneletAngle(llt_pair.first, search_pose.position);
+      double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
+
+      if (angle_diff > std::abs(yaw_threshold)) continue;
+      if (min_distance < distance) break;
+
+      if (angle_diff < min_angle) {
+        min_angle = angle_diff;
+        min_distance = distance;
+        *closest_lanelet_ptr = llt_pair.first;
+        found = true;
+      }
+    }
+  }
+
+  return found;
+}
+
 bool query::getCurrentLanelets(
   const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
   ConstLanelets * current_lanelets_ptr)