feat(landmark_manager): add get_landmarrks func (#6063)

Signed-off-by: yamato-ando <Yamato ANDO> Co-authored-by: yamato-ando <Yamato ANDO>
autowarefoundation · Jan 12, 2024 · d1a86e6 · d1a86e6
1 parent 08a02ba
commit d1a86e6
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git a/...n/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/...n/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
@@ -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;