feat(lanelet2_extension)!: move package to autoware_common

[kenji-miyake]

Description

See autowarefoundation/autoware_common#84 and autowarefoundation/autoware_common#85.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[kenji-miyake]

#873
#1318
#1461

❯ git log --oneline c45e59756e0ef451f70c89e4f17bbdda9b6613f1..origin/main map/lanelet2_extension/
141b7a913 fix(lanelet2_extension): make all polygons clockwise in ll2 visualization (#1461)
4c76f992e fix(lanelet2_extension): fix getCurrentLanelets() (#1318)
56885ed56 feat(behavior_path_planner): update pull_over module (#873)

diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp
index 6737da3e4..e44f8f80b 100644
--- a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp
+++ b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp
@@ -81,7 +81,8 @@ double getLaneletAngle(
 bool isInLanelet(
   const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
   const double radius = 0.0);
-
+geometry_msgs::msg::Pose getClosestCenterPose(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point);
 }  // namespace utils
 }  // namespace lanelet
 
diff --git a/map/lanelet2_extension/lib/query.cpp b/map/lanelet2_extension/lib/query.cpp
index a24c1b958..a110c106b 100644
--- a/map/lanelet2_extension/lib/query.cpp
+++ b/map/lanelet2_extension/lib/query.cpp
@@ -773,7 +773,7 @@ bool query::getCurrentLanelets(
 
   lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y);
   for (const auto & llt : lanelets) {
-    if (!lanelet::geometry::inside(llt, search_point)) {
+    if (lanelet::geometry::inside(llt, search_point)) {
       current_lanelets_ptr->push_back(llt);
     }
   }
diff --git a/map/lanelet2_extension/lib/utilities.cpp b/map/lanelet2_extension/lib/utilities.cpp
index 6a274488b..137119042 100644
--- a/map/lanelet2_extension/lib/utilities.cpp
+++ b/map/lanelet2_extension/lib/utilities.cpp
@@ -25,6 +25,12 @@
 #include <lanelet2_traffic_rules/TrafficRules.h>
 #include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
 #include <algorithm>
 #include <limits>
 #include <map>
@@ -685,5 +691,29 @@ bool isInLanelet(
   return false;
 }
 
+geometry_msgs::msg::Pose getClosestCenterPose(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point)
+{
+  lanelet::BasicPoint2d llt_search_point(search_point.x, search_point.y);
+  lanelet::ConstLineString3d segment = getClosestSegment(llt_search_point, lanelet.centerline());
+
+  const Eigen::Vector2d direction(
+    (segment.back().basicPoint2d() - segment.front().basicPoint2d()).normalized());
+  const Eigen::Vector2d xf(segment.front().basicPoint2d());
+  const Eigen::Vector2d x(search_point.x, search_point.y);
+  const Eigen::Vector2d p = xf + (x - xf).dot(direction) * direction;
+
+  geometry_msgs::msg::Pose closest_pose;
+  closest_pose.position.x = p.x();
+  closest_pose.position.y = p.y();
+  closest_pose.position.z = search_point.z;
+
+  const float lane_yaw = getLaneletAngle(lanelet, search_point);
+  tf2::Quaternion q;
+  q.setRPY(0, 0, lane_yaw);
+  closest_pose.orientation = tf2::toMsg(q);
+
+  return closest_pose;
+}
 }  // namespace utils
 }  // namespace lanelet
diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp
index da7fe490a..cb05a72c7 100644
--- a/map/lanelet2_extension/lib/visualization.cpp
+++ b/map/lanelet2_extension/lib/visualization.cpp
@@ -1175,14 +1175,14 @@ void visualization::pushLineStringMarker(
     p.z = (*(i + 1)).z();
     marker->points.push_back(p);
     marker->colors.push_back(c);
-    p.x = (*(i + 1)).x() + x_offset;
-    p.y = (*(i + 1)).y() - y_offset;
-    p.z = (*(i + 1)).z();
-    marker->points.push_back(p);
     p.x = (*(i + 1)).x() - x_offset;
     p.y = (*(i + 1)).y() + y_offset;
     p.z = (*(i + 1)).z();
     marker->points.push_back(p);
+    p.x = (*(i + 1)).x() + x_offset;
+    p.y = (*(i + 1)).y() - y_offset;
+    p.z = (*(i + 1)).z();
+    marker->points.push_back(p);
     p.x = (*i).x() - x_offset;
     p.y = (*i).y() + y_offset;
     p.z = (*i).z();

[taikitanaka3]

@kenji-miyake
Can I add this bug fix before moving to autoware_common?
#1506