refactor(ar_tag_based_localizer): refactor pub/sub and so on (autowar…

…efoundation#5854) * Fixed ar_tag_based_localizer pub/sub Signed-off-by: Shintaro SAKODA <[email protected]> * Remove dependency on image_transport Signed-off-by: Shintaro SAKODA <[email protected]> --------- Signed-off-by: Shintaro SAKODA <[email protected]>
tier4 · Dec 15, 2023 · 46d3bd3 · 46d3bd3
1 parent 2f30f78
commit 46d3bd3
Show file tree

Hide file tree

Showing 7 changed files with 50 additions and 77 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
@@ -1,17 +1,13 @@
 <?xml version="1.0"?>
 <launch>
   <group>
-    <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"/>
-        <arg name="output_image" value="/localization/pose_estimator/ar_tag_detected_image"/>
-        <arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>
-        <arg name="param_file" value="$(find-pkg-share ar_tag_based_localizer)/config/ar_tag_based_localizer.param.yaml"/>
-      </include>
-    </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"/>
+      <arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="param_file" value="$(find-pkg-share ar_tag_based_localizer)/config/ar_tag_based_localizer.param.yaml"/>
+    </include>
   </group>
 </launch>
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
@@ -8,18 +8,24 @@
   <arg name="input_camera_info" default="~/input/camera_info"/>
   <arg name="input_ekf_pose" default="~/input/ekf_pose"/>
 
-  <arg name="output_image" default="~/debug/result"/>
   <arg name="output_pose_with_covariance" default="~/output/pose_with_covariance"/>
 
+  <arg name="debug_image" default="~/debug/image"/>
+  <arg name="debug_mapped_tag" default="~/debug/mapped_tag"/>
+  <arg name="debug_detected_tag" default="~/debug/detected_tag"/>
+
   <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)"/>
 
-    <remap from="~/debug/result" to="$(var output_image)"/>
     <remap from="~/output/pose_with_covariance" to="$(var output_pose_with_covariance)"/>
 
+    <remap from="~/debug/image" to="$(var debug_image)"/>
+    <remap from="~/debug/mapped_tag" to="$(var debug_mapped_tag)"/>
+    <remap from="~/debug/detected_tag" to="$(var debug_detected_tag)"/>
+
     <param from="$(var param_file)"/>
   </node>
 </launch>
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,6 @@
   <depend>aruco</depend>
   <depend>cv_bridge</depend>
   <depend>diagnostic_msgs</depend>
-  <depend>image_transport</depend>
   <depend>landmark_manager</depend>
   <depend>lanelet2_extension</depend>
   <depend>localization_util</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
@@ -66,10 +66,6 @@
 
 ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options)
 : Node("ar_tag_based_localizer", options), cam_info_received_(false)
-{
-}
-
-bool ArTagBasedLocalizer::setup()
 {
   /*
     Declare node parameters
@@ -92,7 +88,7 @@ bool ArTagBasedLocalizer::setup()
   } else {
     // Error
     RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode);
-    return false;
+    return;
   }
   ekf_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
     this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_);
@@ -111,59 +107,50 @@ bool ArTagBasedLocalizer::setup()
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
 
-  /*
-    Initialize image transport
-  */
-  it_ = std::make_unique<image_transport::ImageTransport>(shared_from_this());
-
   /*
     Subscribers
   */
+  using std::placeholders::_1;
   map_bin_sub_ = this->create_subscription<HADMapBin>(
     "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal),
-    std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1));
+    std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1));
 
   rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
   qos_sub.best_effort();
-  pose_array_pub_ = this->create_publisher<PoseArray>("~/debug/detected_landmarks", qos_sub);
   image_sub_ = this->create_subscription<Image>(
-    "~/input/image", qos_sub,
-    std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1));
+    "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, _1));
   cam_info_sub_ = this->create_subscription<CameraInfo>(
-    "~/input/camera_info", qos_sub,
-    std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1));
+    "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, _1));
   ekf_pose_sub_ = this->create_subscription<PoseWithCovarianceStamped>(
-    "~/input/ekf_pose", qos_sub,
-    std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1));
+    "~/input/ekf_pose", qos_sub, std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, _1));
 
   /*
     Publishers
   */
-  rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10));
-  qos_marker.transient_local();
-  qos_marker.reliable();
-  marker_pub_ = this->create_publisher<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<PoseWithCovarianceStamped>("~/output/pose_with_covariance", qos_pub);
-  diag_pub_ = this->create_publisher<DiagnosticArray>("/diagnostics", qos_pub);
+  const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable();
+  const rclcpp::QoS qos_pub_periodic(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
+  pose_pub_ = this->create_publisher<PoseWithCovarianceStamped>(
+    "~/output/pose_with_covariance", qos_pub_periodic);
+  image_pub_ = this->create_publisher<Image>("~/debug/image", qos_pub_periodic);
+  mapped_tag_pose_pub_ = this->create_publisher<MarkerArray>("~/debug/mapped_tag", qos_pub_once);
+  detected_tag_pose_pub_ =
+    this->create_publisher<PoseArray>("~/debug/detected_tag", qos_pub_periodic);
+  diag_pub_ = this->create_publisher<DiagnosticArray>("/diagnostics", qos_pub_periodic);
 
   RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!");
-  return true;
 }
 
 void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg)
 {
   landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger());
   const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg();
-  marker_pub_->publish(marker_msg);
+  mapped_tag_pose_pub_->publish(marker_msg);
 }
 
 void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
 {
   // check subscribers
-  if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) {
+  if ((image_pub_->get_subscription_count() == 0) && (pose_pub_->get_subscription_count() == 0)) {
     RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers");
     return;
   }
@@ -192,7 +179,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
   }
 
   // for debug
-  if (pose_array_pub_->get_subscription_count() > 0) {
+  if (detected_tag_pose_pub_->get_subscription_count() > 0) {
     PoseArray pose_array_msg;
     pose_array_msg.header.stamp = sensor_stamp;
     pose_array_msg.header.frame_id = "map";
@@ -201,7 +188,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
         tier4_autoware_utils::transformPose(landmark.pose, self_pose);
       pose_array_msg.poses.push_back(detected_marker_on_map);
     }
-    pose_array_pub_->publish(pose_array_msg);
+    detected_tag_pose_pub_->publish(pose_array_msg);
   }
 
   // calc new_self_pose
@@ -267,24 +254,12 @@ void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & m
     return;
   }
 
-  cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0);
-  camera_matrix.at<double>(0, 0) = msg->p[0];
-  camera_matrix.at<double>(0, 1) = msg->p[1];
-  camera_matrix.at<double>(0, 2) = msg->p[2];
-  camera_matrix.at<double>(0, 3) = msg->p[3];
-  camera_matrix.at<double>(1, 0) = msg->p[4];
-  camera_matrix.at<double>(1, 1) = msg->p[5];
-  camera_matrix.at<double>(1, 2) = msg->p[6];
-  camera_matrix.at<double>(1, 3) = msg->p[7];
-  camera_matrix.at<double>(2, 0) = msg->p[8];
-  camera_matrix.at<double>(2, 1) = msg->p[9];
-  camera_matrix.at<double>(2, 2) = msg->p[10];
-  camera_matrix.at<double>(2, 3) = msg->p[11];
-
-  cv::Mat distortion_coeff(4, 1, CV_64FC1);
-  for (int i = 0; i < 4; ++i) {
-    distortion_coeff.at<double>(i, 0) = 0;
-  }
+  // copy camera matrix
+  cv::Mat camera_matrix(3, 4, CV_64FC1);
+  std::copy(msg->p.begin(), msg->p.end(), camera_matrix.begin<double>());
+
+  // all 0
+  cv::Mat distortion_coeff(4, 1, CV_64FC1, 0.0);
 
   const cv::Size size(static_cast<int>(msg->width), static_cast<int>(msg->height));
 
@@ -361,12 +336,12 @@ std::vector<landmark_manager::Landmark> ArTagBasedLocalizer::detect_landmarks(
   }
 
   // for debug
-  if (image_pub_.getNumSubscribers() > 0) {
+  if (image_pub_->get_subscription_count() > 0) {
     cv_bridge::CvImage out_msg;
     out_msg.header.stamp = sensor_stamp;
     out_msg.encoding = sensor_msgs::image_encodings::RGB8;
     out_msg.image = in_image;
-    image_pub_.publish(out_msg.toImageMsg());
+    image_pub_->publish(*out_msg.toImageMsg());
   }
 
   return landmarks;

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
@@ -48,11 +48,12 @@
 #include "landmark_manager/landmark_manager.hpp"
 #include "localization_util/smart_pose_buffer.hpp"
 
-#include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -82,7 +83,6 @@ class ArTagBasedLocalizer : public rclcpp::Node
 
 public:
   explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
-  bool setup();
 
 private:
   void map_bin_callback(const HADMapBin::ConstSharedPtr & msg);
@@ -104,20 +104,17 @@ class ArTagBasedLocalizer : public rclcpp::Node
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 
-  // image transport
-  std::unique_ptr<image_transport::ImageTransport> it_;
-
   // Subscribers
   rclcpp::Subscription<HADMapBin>::SharedPtr map_bin_sub_;
   rclcpp::Subscription<Image>::SharedPtr image_sub_;
   rclcpp::Subscription<CameraInfo>::SharedPtr cam_info_sub_;
   rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr ekf_pose_sub_;
 
   // Publishers
-  rclcpp::Publisher<MarkerArray>::SharedPtr marker_pub_;
-  rclcpp::Publisher<PoseArray>::SharedPtr pose_array_pub_;
-  image_transport::Publisher image_pub_;
   rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+  rclcpp::Publisher<Image>::SharedPtr image_pub_;
+  rclcpp::Publisher<PoseArray>::SharedPtr detected_tag_pose_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr mapped_tag_pose_pub_;
   rclcpp::Publisher<DiagnosticArray>::SharedPtr diag_pub_;
 
   // Others

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp
@@ -48,7 +48,6 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
   std::shared_ptr<ArTagBasedLocalizer> ptr = std::make_shared<ArTagBasedLocalizer>();
-  ptr->setup();
   rclcpp::spin(ptr);
   rclcpp::shutdown();
 }
diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp
@@ -55,7 +55,8 @@ class TestArTagBasedLocalizer : public ::testing::Test
 
 TEST_F(TestArTagBasedLocalizer, test_setup)  // NOLINT
 {
-  EXPECT_TRUE(node_->setup());
+  // Check if the constructor finishes successfully
+  EXPECT_TRUE(true);
 }
 
 int main(int argc, char ** argv)