refactor(landmark_based_localizer): refactored landmark_tf_caster (#5414

) * Removed landmark_tf_caster node Signed-off-by: Shintaro SAKODA <[email protected]> * Added maintainer Signed-off-by: Shintaro SAKODA <[email protected]> * style(pre-commit): autofix * Renamed to landmark_parser Signed-off-by: Shintaro SAKODA <[email protected]> * Added include<map> Signed-off-by: Shintaro SAKODA <[email protected]> * style(pre-commit): autofix * Added publish_landmark_markers Signed-off-by: Shintaro SAKODA <[email protected]> * Removed unused package.xml Signed-off-by: Shintaro Sakoda <[email protected]> * Changed from depend to build_depend Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed a local variable name Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed Marker to MarkerArray 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 · Oct 26, 2023 · 64c8ac8 · 64c8ac8
1 parent 45792df
commit 64c8ac8
Show file tree

Hide file tree

Showing 18 changed files with 296 additions and 315 deletions.
diff --git a/...h/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/...h/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml
@@ -4,6 +4,7 @@
     <push-ros-namespace namespace="ar_tag_based_localizer"/>
     <group>
       <include file="$(find-pkg-share ar_tag_based_localizer)/launch/ar_tag_based_localizer.launch.xml">
+        <arg name="input_lanelet2_map" value="/map/vector_map"/>
         <arg name="input_image" value="/sensing/camera/traffic_light/image_raw"/>
         <arg name="input_camera_info" value="/sensing/camera/traffic_light/camera_info"/>
         <arg name="input_ekf_pose" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>
@@ -12,12 +13,5 @@
         <arg name="param_file" value="$(find-pkg-share ar_tag_based_localizer)/config/ar_tag_based_localizer.param.yaml"/>
       </include>
     </group>
-
-    <group>
-      <include file="$(find-pkg-share landmark_tf_caster)/launch/landmark_tf_caster.launch.xml">
-        <arg name="input_lanelet2_map" value="/map/vector_map"/>
-        <arg name="param_file" value="$(find-pkg-share landmark_tf_caster)/config/landmark_tf_caster.param.yaml"/>
-      </include>
-    </group>
   </group>
 </launch>
diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml
@@ -21,7 +21,6 @@
   <exec_depend>eagleye_rt</exec_depend>
   <exec_depend>ekf_localizer</exec_depend>
   <exec_depend>gyro_odometer</exec_depend>
-  <exec_depend>landmark_tf_caster</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pose_initializer</exec_depend>

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_tf_caster` node
+### `landmark_parser`
 
 The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`.
 
-The `landmark_tf_caster` node publishes the TF from the map to the landmark.
+The `landmark_parser` 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.
@@ -41,25 +41,9 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc
 - ar_tag_based_localizer
 - etc.
 
-## Inputs / Outputs
-
-### `landmark_tf_caster` node
-
-#### Input
-
-| Name                   | Type                                         | Description      |
-| :--------------------- | :------------------------------------------- | :--------------- |
-| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 |
-
-#### Output
-
-| Name        | Type                                   | Description        |
-| :---------- | :------------------------------------- | :----------------- |
-| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag |
-
 ## 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_tf_caster` 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_parser` can interpret.
 
 The four vertices of a landmark are defined counterclockwise.
 

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md
@@ -13,18 +13,20 @@ The positions and orientations of the AR-Tags are assumed to be written in the L
 
 #### Input
 
-| Name                  | Type                                            | Description                                                                                                                                                                                                                                                               |
-| :-------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| `~/input/image`       | `sensor_msgs::msg::Image`                       | Camera Image                                                                                                                                                                                                                                                              |
-| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo`                  | Camera Info                                                                                                                                                                                                                                                               |
-| `~/input/ekf_pose`    | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. |
+| Name                   | Type                                            | Description                                                                                                                                                                                                                                                               |
+| :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin`    | Data of lanelet2                                                                                                                                                                                                                                                          |
+| `~/input/image`        | `sensor_msgs::msg::Image`                       | Camera Image                                                                                                                                                                                                                                                              |
+| `~/input/camera_info`  | `sensor_msgs::msg::CameraInfo`                  | Camera Info                                                                                                                                                                                                                                                               |
+| `~/input/ekf_pose`     | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. |
 
 #### Output
 
 | Name                            | Type                                            | Description                                                                               |
 | :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- |
 | `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose                                                                            |
 | `~/debug/result`                | `sensor_msgs::msg::Image`                       | [debug topic] Image in which marker detection results are superimposed on the input image |
+| `~/debug/marker`                | `visualization_msgs::msg::MarkerArray`          | [debug topic] Loaded landmarks to visualize in Rviz as thin boards                        |
 | `/tf`                           | `geometry_msgs::msg::TransformStamped`          | [debug topic] TF from camera to detected tag                                              |
 | `/diagnostics`                  | `diagnostic_msgs::msg::DiagnosticArray`         | Diagnostics outputs                                                                       |
 

diff --git a/...zer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/...zer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp
@@ -45,6 +45,8 @@
 #ifndef AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_
 #define AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_
 
+#include "landmark_parser/landmark_parser_core.hpp"
+
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -57,6 +59,7 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
@@ -68,6 +71,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   bool setup();
 
 private:
+  void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg);
   void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg);
   void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg);
   void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg);
@@ -92,11 +96,13 @@ class ArTagBasedLocalizer : public rclcpp::Node
   std::unique_ptr<image_transport::ImageTransport> it_;
 
   // Subscribers
+  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_bin_sub_;
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
   rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr ekf_pose_sub_;
 
   // Publishers
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
   image_transport::Publisher image_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diag_pub_;
@@ -106,6 +112,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   aruco::CameraParameters cam_param_;
   bool cam_info_received_;
   geometry_msgs::msg::PoseWithCovarianceStamped latest_ekf_pose_{};
+  std::map<std::string, geometry_msgs::msg::Pose> landmark_map_;
 };
 
 #endif  // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_
diff --git a/.../landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/.../landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml
@@ -3,6 +3,7 @@
   <arg name="node_name" default="ar_tag_based_localizer"/>
 
   <!-- Topic names -->
+  <arg name="input_lanelet2_map" default="~/input/lanelet2_map"/>
   <arg name="input_image" default="~/input/image"/>
   <arg name="input_camera_info" default="~/input/camera_info"/>
   <arg name="input_ekf_pose" default="~/input/ekf_pose"/>
@@ -11,6 +12,7 @@
   <arg name="output_pose_with_covariance" default="~/output/pose_with_covariance"/>
 
   <node pkg="ar_tag_based_localizer" exec="ar_tag_based_localizer" name="$(var node_name)" output="log">
+    <remap from="~/input/lanelet2_map" to="$(var input_lanelet2_map)"/>
     <remap from="~/input/image" to="$(var input_image)"/>
     <remap from="~/input/camera_info" to="$(var input_camera_info)"/>
     <remap from="~/input/ekf_pose" to="$(var input_ekf_pose)"/>

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
@@ -6,6 +6,8 @@
   <description>The ar_tag_based_localizer package</description>
   <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
   <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Kento Yabuuchi</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
@@ -15,6 +17,8 @@
   <depend>cv_bridge</depend>
   <depend>diagnostic_msgs</depend>
   <depend>image_transport</depend>
+  <depend>landmark_parser</depend>
+  <depend>lanelet2_extension</depend>
   <depend>rclcpp</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>

diff --git a/...ation/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/...ation/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp
@@ -110,6 +110,9 @@ bool ArTagBasedLocalizer::setup()
   /*
     Subscribers
   */
+  map_bin_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
+    "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal),
+    std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1));
   rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
   qos_sub.best_effort();
   image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
@@ -125,6 +128,11 @@ bool ArTagBasedLocalizer::setup()
   /*
     Publishers
   */
+  rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10));
+  qos_marker.transient_local();
+  qos_marker.reliable();
+  marker_pub_ =
+    this->create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/marker", qos_marker);
   rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
   image_pub_ = it_->advertise("~/debug/result", 1);
   pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
@@ -136,6 +144,15 @@ bool ArTagBasedLocalizer::setup()
   return true;
 }
 
+void ArTagBasedLocalizer::map_bin_callback(
+  const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg)
+{
+  landmark_map_ = parse_landmark(msg, "apriltag_16h5", this->get_logger());
+  const visualization_msgs::msg::MarkerArray marker_msg =
+    convert_to_marker_array_msg(landmark_map_);
+  marker_pub_->publish(marker_msg);
+}
+
 void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg)
 {
   if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) {
@@ -290,16 +307,20 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(
   }
 
   // Transform map to tag
-  geometry_msgs::msg::TransformStamped map_to_tag_tf;
-  try {
-    map_to_tag_tf =
-      tf_buffer_->lookupTransform("map", "tag_" + msg.header.frame_id, tf2::TimePointZero);
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_INFO(
-      this->get_logger(), "Could not transform map to tag_%s: %s", msg.header.frame_id.c_str(),
-      ex.what());
+  if (landmark_map_.count(msg.header.frame_id) == 0) {
+    RCLCPP_INFO_STREAM(
+      this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in landmark_map_");
     return;
   }
+  const geometry_msgs::msg::Pose & tag_pose = landmark_map_.at(msg.header.frame_id);
+  geometry_msgs::msg::TransformStamped map_to_tag_tf;
+  map_to_tag_tf.header.stamp = msg.header.stamp;
+  map_to_tag_tf.header.frame_id = "map";
+  map_to_tag_tf.child_frame_id = msg.header.frame_id;
+  map_to_tag_tf.transform.translation.x = tag_pose.position.x;
+  map_to_tag_tf.transform.translation.y = tag_pose.position.y;
+  map_to_tag_tf.transform.translation.z = tag_pose.position.z;
+  map_to_tag_tf.transform.rotation = tag_pose.orientation;
 
   // Transform camera to base_link
   geometry_msgs::msg::TransformStamped camera_to_base_link_tf;