refactor(landmark_parser): refactored landmark manager (#5511)

* Refactored landmark_parser Signed-off-by: Shintaro Sakoda <[email protected]> * Renamed landmark_parser to landmark_manager Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed tag_id Signed-off-by: Shintaro Sakoda <[email protected]> * Refactored ar_tag_based_localizer Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix * Added [[nodiscard]] Signed-off-by: Shintaro Sakoda <[email protected]> * Refactored landmark parsing and conversion Signed-off-by: Shintaro SAKODA <[email protected]> * style(pre-commit): autofix * Added namespace Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed include Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Signed-off-by: Shintaro SAKODA <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Nov 13, 2023 · ea12d77 · ea12d77
1 parent 6e60821
commit ea12d77
Show file tree

Hide file tree

Showing 8 changed files with 51 additions and 27 deletions.
diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md
@@ -23,11 +23,11 @@ This calculated ego pose is passed to the EKF, where it is fused with the twist
 
 ![node diagram](./doc_image/node_diagram.drawio.svg)
 
-### `landmark_parser`
+### `landmark_manager`
 
 The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`.
 
-The `landmark_parser` is a utility package to load landmarks from the map.
+The `landmark_manager` is a utility package to load landmarks from the map.
 
 - Translation : The center of the four vertices of the landmark
 - Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3.
@@ -43,7 +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_parser` can interpret.
+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.
 

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
@@ -17,7 +17,7 @@
   <depend>cv_bridge</depend>
   <depend>diagnostic_msgs</depend>
   <depend>image_transport</depend>
-  <depend>landmark_parser</depend>
+  <depend>landmark_manager</depend>
   <depend>lanelet2_extension</depend>
   <depend>localization_util</depend>
   <depend>rclcpp</depend>

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
@@ -148,8 +148,13 @@ bool ArTagBasedLocalizer::setup()
 
 void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg)
 {
-  landmark_map_ = parse_landmark(msg, "apriltag_16h5", this->get_logger());
-  const MarkerArray marker_msg = convert_to_marker_array_msg(landmark_map_);
+  const std::vector<landmark_manager::Landmark> landmarks =
+    landmark_manager::parse_landmarks(msg, "apriltag_16h5", this->get_logger());
+  for (const landmark_manager::Landmark & landmark : landmarks) {
+    landmark_map_[landmark.id] = landmark.pose;
+  }
+
+  const MarkerArray marker_msg = landmark_manager::convert_landmarks_to_marker_array_msg(landmarks);
   marker_pub_->publish(marker_msg);
 }
 

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp
@@ -45,7 +45,7 @@
 #ifndef AR_TAG_BASED_LOCALIZER_HPP_
 #define AR_TAG_BASED_LOCALIZER_HPP_
 
-#include "landmark_parser/landmark_parser_core.hpp"
+#include "landmark_manager/landmark_manager.hpp"
 
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>

diff --git a/..._localizer/landmark_parser/CMakeLists.txt → ...localizer/landmark_manager/CMakeLists.txt b/..._localizer/landmark_parser/CMakeLists.txt → ...localizer/landmark_manager/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(landmark_parser)
+project(landmark_manager)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -12,8 +12,8 @@ endif()
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
-ament_auto_add_library(landmark_parser
-  src/landmark_parser_core.cpp
+ament_auto_add_library(landmark_manager
+  src/landmark_manager.cpp
 )
 
 ament_auto_package(

diff --git a/.../landmark_parser/landmark_parser_core.hpp → ...ude/landmark_manager/landmark_manager.hpp b/.../landmark_parser/landmark_parser_core.hpp → ...ude/landmark_manager/landmark_manager.hpp
@@ -12,23 +12,35 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_
-#define LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_
+#ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
+#define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
 #include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
-#include <map>
 #include <string>
+#include <vector>
 
-std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
+namespace landmark_manager
+{
+
+struct Landmark
+{
+  std::string id;
+  geometry_msgs::msg::Pose pose;
+};
+
+std::vector<Landmark> parse_landmarks(
   const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
   const std::string & target_subtype, const rclcpp::Logger & logger);
 
-visualization_msgs::msg::MarkerArray convert_to_marker_array_msg(
-  const std::map<std::string, geometry_msgs::msg::Pose> & landmarks);
+visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg(
+  const std::vector<Landmark> & landmarks);
+
+}  // namespace landmark_manager
 
-#endif  // LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_
+#endif  // LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_
diff --git a/...sed_localizer/landmark_parser/package.xml → ...ed_localizer/landmark_manager/package.xml b/...sed_localizer/landmark_parser/package.xml → ...ed_localizer/landmark_manager/package.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>landmark_parser</name>
+  <name>landmark_manager</name>
   <version>0.0.0</version>
-  <description>The landmark_parser package</description>
+  <description>The landmark_manager package</description>
   <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
   <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
   <maintainer email="[email protected]">Yamato Ando</maintainer>
@@ -19,6 +19,7 @@
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>rclcpp</depend>
+  <depend>tf2_eigen</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/...dmark_parser/src/landmark_parser_core.cpp → ...landmark_manager/src/landmark_manager.cpp b/...dmark_parser/src/landmark_parser_core.cpp → ...landmark_manager/src/landmark_manager.cpp
@@ -12,16 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "landmark_parser/landmark_parser_core.hpp"
+#include "landmark_manager/landmark_manager.hpp"
 
 #include "lanelet2_extension/utility/message_conversion.hpp"
 
 #include <Eigen/Core>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/primitives/Polygon.h>
 
-std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
+namespace landmark_manager
+{
+
+std::vector<Landmark> parse_landmarks(
   const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
   const std::string & target_subtype, const rclcpp::Logger & logger)
 {
@@ -32,7 +36,7 @@ std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
   lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared<lanelet::LaneletMap>()};
   lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr);
 
-  std::map<std::string, geometry_msgs::msg::Pose> landmark_map;
+  std::vector<Landmark> landmarks;
 
   for (const auto & poly : lanelet_map_ptr->polygonLayer) {
     const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")};
@@ -93,8 +97,8 @@ std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
     pose.orientation.z = q.z();
     pose.orientation.w = q.w();
 
-    // Add to map
-    landmark_map[marker_id] = pose;
+    // Add
+    landmarks.push_back(Landmark{marker_id, pose});
     RCLCPP_INFO_STREAM(logger, "id: " << marker_id);
     RCLCPP_INFO_STREAM(
       logger,
@@ -104,11 +108,11 @@ std::map<std::string, geometry_msgs::msg::Pose> parse_landmark(
                      << pose.orientation.z << ", " << pose.orientation.w);
   }
 
-  return landmark_map;
+  return landmarks;
 }
 
-visualization_msgs::msg::MarkerArray convert_to_marker_array_msg(
-  const std::map<std::string, geometry_msgs::msg::Pose> & landmarks)
+visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg(
+  const std::vector<Landmark> & landmarks)
 {
   int32_t id = 0;
   visualization_msgs::msg::MarkerArray marker_array;
@@ -152,3 +156,5 @@ visualization_msgs::msg::MarkerArray convert_to_marker_array_msg(
   }
   return marker_array;
 }
+
+}  // namespace landmark_manager