diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index 86f3750ad11d9..49588a19ac620 100644 --- a/localization/landmark_based_localizer/README.md +++ b/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 -... - - - - - - - - - - - - - - - - - - - - - - - - - - -... - - - - - - - - - - - - -... - -``` +See ## About `consider_orientation` 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 a1e2f3ec31dd3..43ac1e1098453 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 @@ -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 landmarks = detect_landmarks(msg); + const std::vector 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 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{}; + return std::vector{}; } // get transform from base_link to camera @@ -303,7 +303,7 @@ std::vector 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{}; + return std::vector{}; } // detect @@ -311,7 +311,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( detector_.detect(in_image, markers, cam_param_, marker_size_, false); // parse - std::vector landmarks; + std::vector landmarks; for (aruco::Marker & marker : markers) { // convert marker pose to tf @@ -327,7 +327,7 @@ std::vector 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 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 eb7406f8c77f8..f70821a39ffe8 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 @@ -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 detect_landmarks(const Image::ConstSharedPtr & msg); + std::vector detect_landmarks(const Image::ConstSharedPtr & msg); // Parameters float marker_size_{}; 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 fba419f746b5e..7e78ed713dddc 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 @@ -15,6 +15,8 @@ #ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#include "lanelet2_extension/localization/landmark.hpp" + #include #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 & detected_landmarks, - const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const; + const std::vector & 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, 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 875f04edd8c47..7ddd66efea0a6 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/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::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 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); } }