From ea12d77d1e762703201eb85cfea280223b7ab5c9 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 14 Nov 2023 08:49:29 +0900 Subject: [PATCH] refactor(landmark_parser): refactored landmark manager (#5511) * Refactored landmark_parser Signed-off-by: Shintaro Sakoda * Renamed landmark_parser to landmark_manager Signed-off-by: Shintaro Sakoda * Fixed tag_id Signed-off-by: Shintaro Sakoda * Refactored ar_tag_based_localizer Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Added [[nodiscard]] Signed-off-by: Shintaro Sakoda * Refactored landmark parsing and conversion Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Added namespace Signed-off-by: Shintaro Sakoda * Fixed include Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Signed-off-by: Shintaro SAKODA Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../landmark_based_localizer/README.md | 6 ++--- .../ar_tag_based_localizer/package.xml | 2 +- .../src/ar_tag_based_localizer.cpp | 9 +++++-- .../src/ar_tag_based_localizer.hpp | 2 +- .../CMakeLists.txt | 6 ++--- .../landmark_manager/landmark_manager.hpp} | 26 ++++++++++++++----- .../package.xml | 5 ++-- .../src/landmark_manager.cpp} | 22 ++++++++++------ 8 files changed, 51 insertions(+), 27 deletions(-) rename localization/landmark_based_localizer/{landmark_parser => landmark_manager}/CMakeLists.txt (79%) rename localization/landmark_based_localizer/{landmark_parser/include/landmark_parser/landmark_parser_core.hpp => landmark_manager/include/landmark_manager/landmark_manager.hpp} (64%) rename localization/landmark_based_localizer/{landmark_parser => landmark_manager}/package.xml (88%) rename localization/landmark_based_localizer/{landmark_parser/src/landmark_parser_core.cpp => landmark_manager/src/landmark_manager.cpp} (92%) diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index 35d8d78e7015c..e4c79b7941b12 100644 --- 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 index a22e7b1acf6dc..24b737c309016 100644 --- 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 @@ cv_bridge diagnostic_msgs image_transport - landmark_parser + landmark_manager lanelet2_extension localization_util rclcpp 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 index bc40fb1532297..a66277c699a00 100644 --- 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 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 index fe33a64b995a3..889e76eb78ad2 100644 --- 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 #include diff --git a/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt similarity index 79% rename from localization/landmark_based_localizer/landmark_parser/CMakeLists.txt rename to localization/landmark_based_localizer/landmark_manager/CMakeLists.txt index 41f7c00d61383..1b6126c892d2e 100644 --- a/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt +++ b/localization/landmark_based_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/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp similarity index 64% rename from localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp rename to localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index edf45c2a2997a..9a22ee13f60ae 100644 --- a/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/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 #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include +#include #include -#include #include +#include -std::map parse_landmark( +namespace landmark_manager +{ + +struct Landmark +{ + std::string id; + geometry_msgs::msg::Pose pose; +}; + +std::vector 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 & landmarks); +visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( + const std::vector & landmarks); + +} // namespace landmark_manager -#endif // LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ +#endif // LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ diff --git a/localization/landmark_based_localizer/landmark_parser/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml similarity index 88% rename from localization/landmark_based_localizer/landmark_parser/package.xml rename to localization/landmark_based_localizer/landmark_manager/package.xml index e3daa93f81220..1a35ae6a338d1 100644 --- a/localization/landmark_based_localizer/landmark_parser/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -1,9 +1,9 @@ - landmark_parser + landmark_manager 0.0.0 - The landmark_parser package + The landmark_manager package Shintaro Sakoda Masahiro Sakamoto Yamato Ando @@ -19,6 +19,7 @@ geometry_msgs lanelet2_extension rclcpp + tf2_eigen ament_cmake diff --git a/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp similarity index 92% rename from localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp rename to localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index c86b35b6115b4..6d1698daf5eae 100644 --- a/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp +++ b/localization/landmark_based_localizer/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 +#include #include #include -std::map parse_landmark( +namespace landmark_manager +{ + +std::vector 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 parse_landmark( lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - std::map landmark_map; + std::vector 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 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 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 & landmarks) +visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( + const std::vector & 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