From d1a86e644adc635a29f1044c987081d9037906dd Mon Sep 17 00:00:00 2001
From: Yamato Ando <yamato.ando@gmail.com>
Date: Fri, 12 Jan 2024 18:05:46 +0900
Subject: [PATCH] feat(landmark_manager): add get_landmarrks func (#6063)

Signed-off-by: yamato-ando <Yamato ANDO>
Co-authored-by: yamato-ando <Yamato ANDO>
---
 .../landmark_manager/landmark_manager.hpp        |  2 ++
 .../landmark_manager/src/landmark_manager.cpp    | 16 ++++++++++++++++
 2 files changed, 18 insertions(+)

diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp
index 7e78ed713dddc..c2b0751d91f9a 100644
--- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp
+++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp
@@ -44,6 +44,8 @@ class LandmarkManager
     const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
     const std::string & target_subtype);
 
+  [[nodiscard]] std::vector<landmark_manager::Landmark> get_landmarks() const;
+
   [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const;
 
   [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose(
diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp
index 7ddd66efea0a6..57bfcde461af6 100644
--- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp
+++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp
@@ -83,6 +83,22 @@ void LandmarkManager::parse_landmarks(
   }
 }
 
+std::vector<landmark_manager::Landmark> LandmarkManager::get_landmarks() const
+{
+  std::vector<landmark_manager::Landmark> landmarks;
+
+  landmark_manager::Landmark landmark;
+  for (const auto & [landmark_id_str, landmark_poses] : landmarks_map_) {
+    for (const auto & pose : landmark_poses) {
+      landmark.id = landmark_id_str;
+      landmark.pose = pose;
+      landmarks.push_back(landmark);
+    }
+  }
+
+  return landmarks;
+}
+
 visualization_msgs::msg::MarkerArray LandmarkManager::get_landmarks_as_marker_array_msg() const
 {
   int32_t id = 0;