fix(landmark_based_localizer): fix to for moving the definition code of landmarks (#5803)

* Fixed to use lanelet extension landmark

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed to build

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

* Fixed link

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

---------

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Author: SakodaShintaro

localization/landmark_based_localizer/README.md

@@ -43,68 +43,7 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc
 
 ## Map specifications
 
-For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret.
-
-The four vertices of a landmark are defined counterclockwise.
-
-The order of the four vertices is defined as follows. In the coordinate system of a landmark,
-
-- the x-axis is parallel to the vector from the first vertex to the second vertex
-- the y-axis is parallel to the vector from the second vertex to the third vertex
-
-![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg)
-
-### Example of `lanelet2_map.osm`
-
-The values provided below are placeholders.
-Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`.
-
-For example, when using the AR tag, it would look like this.
-
-```xml
-...
-
-  <node id="1" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.2356"/>
-    <tag k="local_y" v="87.4506"/>
-    <tag k="ele" v="2.1725"/>
-  </node>
-  <node id="2" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.639"/>
-    <tag k="local_y" v="87.5886"/>
-    <tag k="ele" v="2.5947"/>
-  </node>
-  <node id="3" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="22.2331"/>
-    <tag k="local_y" v="87.4713"/>
-    <tag k="ele" v="3.0208"/>
-  </node>
-  <node id="4" lat="35.8xxxxx" lon="139.6xxxxx">
-    <tag k="mgrs_code" v="99XXX000000"/>
-    <tag k="local_x" v="21.8298"/>
-    <tag k="local_y" v="87.3332"/>
-    <tag k="ele" v="2.5985"/>
-  </node>
-
-...
-
-  <way id="5">
-    <nd ref="1"/>
-    <nd ref="2"/>
-    <nd ref="3"/>
-    <nd ref="4"/>
-    <tag k="type" v="pose_marker"/>
-    <tag k="subtype" v="apriltag_16h5"/>
-    <tag k="area" v="yes"/>
-    <tag k="marker_id" v="0"/>
-  </way>
-
-...
-
-```
+See <https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#localization-landmarks>
 
 ## About `consider_orientation`
 

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp

@@ -142,7 +142,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options)
 
 void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg)
 {
-  landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger());
+  landmark_manager_.parse_landmarks(msg, "apriltag_16h5");
   const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg();
   mapped_tag_pose_pub_->publish(marker_msg);
 }
@@ -173,7 +173,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
   const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose;
 
   // detect
-  const std::vector<landmark_manager::Landmark> landmarks = detect_landmarks(msg);
+  const std::vector<Landmark> landmarks = detect_landmarks(msg);
   if (landmarks.empty()) {
     return;
   }
@@ -183,7 +183,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
     PoseArray pose_array_msg;
     pose_array_msg.header.stamp = sensor_stamp;
     pose_array_msg.header.frame_id = "map";
-    for (const landmark_manager::Landmark & landmark : landmarks) {
+    for (const Landmark & landmark : landmarks) {
       const Pose detected_marker_on_map =
         tier4_autoware_utils::transformPose(landmark.pose, self_pose);
       pose_array_msg.poses.push_back(detected_marker_on_map);
@@ -293,7 +293,7 @@ std::vector<landmark_manager::Landmark> ArTagBasedLocalizer::detect_landmarks(
     cv_ptr->image.copyTo(in_image);
   } catch (cv_bridge::Exception & e) {
     RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
-    return std::vector<landmark_manager::Landmark>{};
+    return std::vector<Landmark>{};
   }
 
   // get transform from base_link to camera
@@ -303,15 +303,15 @@ std::vector<landmark_manager::Landmark> ArTagBasedLocalizer::detect_landmarks(
       tf_buffer_->lookupTransform("base_link", msg->header.frame_id, sensor_stamp);
   } catch (tf2::TransformException & ex) {
     RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what());
-    return std::vector<landmark_manager::Landmark>{};
+    return std::vector<Landmark>{};
   }
 
   // detect
   std::vector<aruco::Marker> markers;
   detector_.detect(in_image, markers, cam_param_, marker_size_, false);
 
   // parse
-  std::vector<landmark_manager::Landmark> landmarks;
+  std::vector<Landmark> landmarks;
 
   for (aruco::Marker & marker : markers) {
     // convert marker pose to tf
@@ -327,7 +327,7 @@ std::vector<landmark_manager::Landmark> ArTagBasedLocalizer::detect_landmarks(
     const double distance = std::hypot(pose.position.x, pose.position.y, pose.position.z);
     if (distance <= distance_threshold_) {
       tf2::doTransform(pose, pose, transform_sensor_to_base_link);
-      landmarks.push_back(landmark_manager::Landmark{std::to_string(marker.id), pose});
+      landmarks.push_back(Landmark{std::to_string(marker.id), pose});
     }
 
     // for debug, drawing the detected markers

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp

@@ -80,6 +80,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   using TransformStamped = geometry_msgs::msg::TransformStamped;
   using MarkerArray = visualization_msgs::msg::MarkerArray;
   using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
+  using Landmark = landmark_manager::Landmark;
 
 public:
   explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
@@ -89,7 +90,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   void image_callback(const Image::ConstSharedPtr & msg);
   void cam_info_callback(const CameraInfo::ConstSharedPtr & msg);
   void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg);
-  std::vector<landmark_manager::Landmark> detect_landmarks(const Image::ConstSharedPtr & msg);
+  std::vector<Landmark> detect_landmarks(const Image::ConstSharedPtr & msg);
 
   // Parameters
   float marker_size_{};

localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp

@@ -15,6 +15,8 @@
 #ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
 #define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
 
+#include "lanelet2_extension/localization/landmark.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
@@ -40,13 +42,13 @@ class LandmarkManager
 public:
   void parse_landmarks(
     const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
-    const std::string & target_subtype, const rclcpp::Logger & logger);
+    const std::string & target_subtype);
 
   [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const;
 
   [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose(
-    const std::vector<landmark_manager::Landmark> & detected_landmarks,
-    const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const;
+    const std::vector<Landmark> & detected_landmarks, const geometry_msgs::msg::Pose & self_pose,
+    const bool consider_orientation) const;
 
 private:
   // To allow multiple landmarks with the same id to be registered on a vector_map,

localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp

@@ -29,32 +29,17 @@ namespace landmark_manager
 
 void LandmarkManager::parse_landmarks(
   const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
-  const std::string & target_subtype, const rclcpp::Logger & logger)
+  const std::string & target_subtype)
 {
-  RCLCPP_INFO_STREAM(logger, "msg->format_version: " << msg->format_version);
-  RCLCPP_INFO_STREAM(logger, "msg->map_format: " << msg->map_format);
-  RCLCPP_INFO_STREAM(logger, "msg->map_version: " << msg->map_version);
-  RCLCPP_INFO_STREAM(logger, "msg->data.size(): " << msg->data.size());
-  lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared<lanelet::LaneletMap>()};
-  lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr);
-
-  for (const auto & poly : lanelet_map_ptr->polygonLayer) {
-    const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")};
-    if (type != "pose_marker") {
-      continue;
-    }
-    const std::string subtype{poly.attributeOr(lanelet::AttributeName::Subtype, "none")};
-    if (subtype != target_subtype) {
-      continue;
-    }
-
+  std::vector<lanelet::Polygon3d> landmarks =
+    lanelet::localization::parseLandmarkPolygons(msg, target_subtype);
+  for (const lanelet::Polygon3d & poly : landmarks) {
     // Get landmark_id
     const std::string landmark_id = poly.attributeOr("marker_id", "none");
 
     // Get 4 vertices
     const auto & vertices = poly.basicPolygon();
     if (vertices.size() != 4) {
-      RCLCPP_WARN_STREAM(logger, "vertices.size() (" << vertices.size() << ") != 4");
       continue;
     }
 
@@ -65,12 +50,8 @@ void LandmarkManager::parse_landmarks(
     const auto & v2 = vertices[2];
     const auto & v3 = vertices[3];
     const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0;
-    RCLCPP_INFO_STREAM(logger, "volume = " << volume);
     const double volume_threshold = 1e-5;
     if (volume > volume_threshold) {
-      RCLCPP_WARN_STREAM(
-        logger,
-        "volume (" << volume << ") > threshold (" << volume_threshold << "), This is not plane.");
       continue;
     }
 
@@ -99,13 +80,6 @@ void LandmarkManager::parse_landmarks(
 
     // Add
     landmarks_map_[landmark_id].push_back(pose);
-    RCLCPP_INFO_STREAM(logger, "id: " << landmark_id);
-    RCLCPP_INFO_STREAM(
-      logger,
-      "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z);
-    RCLCPP_INFO_STREAM(
-      logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", "
-                     << pose.orientation.z << ", " << pose.orientation.w);
   }
 }