feat(ar_tag_based_localizer): add diagnostic (autowarefoundation#5132)

* Added diagnostic Signed-off-by: Shintaro SAKODA <[email protected]> * Fixed key name Signed-off-by: Shintaro SAKODA <[email protected]> * Updated README.md Signed-off-by: Shintaro SAKODA <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Shintaro SAKODA <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
esteve · Sep 26, 2023 · 72e7855 · 72e7855
1 parent 32f0547
commit 72e7855
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 1 deletion.
diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md
@@ -25,7 +25,8 @@ The positions and orientations of the AR-Tags are assumed to be written in the L
 | :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- |
 | `~/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 |
-| `tf`                            | `geometry_msgs::msg::TransformStamped`          | [debug topic] TF from camera to detected tag                                              |
+| `/tf`                           | `geometry_msgs::msg::TransformStamped`          | [debug topic] TF from camera to detected tag                                              |
+| `/diagnostics`                  | `diagnostic_msgs::msg::DiagnosticArray`         | Diagnostics outputs                                                                       |
 
 ## How to launch
 

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
@@ -48,6 +48,7 @@
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -98,6 +99,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   // Publishers
   image_transport::Publisher image_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diag_pub_;
 
   // Others
   aruco::MarkerDetector detector_;

diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
@@ -13,6 +13,7 @@
 
   <depend>aruco</depend>
   <depend>cv_bridge</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>image_transport</depend>
   <depend>rclcpp</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
@@ -129,6 +129,8 @@ bool ArTagBasedLocalizer::setup()
   image_pub_ = it_->advertise("~/debug/result", 1);
   pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "~/output/pose_with_covariance", qos_pub);
+  diag_pub_ =
+    this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", qos_pub);
 
   RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!");
   return true;
@@ -199,6 +201,32 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha
     out_msg.image = in_image;
     image_pub_.publish(out_msg.toImageMsg());
   }
+
+  const int detected_tags = markers.size();
+
+  diagnostic_msgs::msg::DiagnosticStatus diag_status;
+
+  if (detected_tags > 0) {
+    diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+    diag_status.message = "AR tags detected. The number of tags: " + std::to_string(detected_tags);
+  } else {
+    diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status.message = "No AR tags detected.";
+  }
+
+  diag_status.name = "localization: " + std::string(this->get_name());
+  diag_status.hardware_id = this->get_name();
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "Number of Detected AR Tags";
+  key_value.value = std::to_string(detected_tags);
+  diag_status.values.push_back(key_value);
+
+  diagnostic_msgs::msg::DiagnosticArray diag_msg;
+  diag_msg.header.stamp = this->now();
+  diag_msg.status.push_back(diag_status);
+
+  diag_pub_->publish(diag_msg);
 }
 
 // wait for one camera info, then shut down that subscriber