feat: add diagnostics (#19)

* feat: add diagnostics
* set update_rate

CMakeLists.txt

@@ -51,7 +51,8 @@ ament_target_dependencies(v4l2_camera
   "rclcpp"
   "rclcpp_components"
   "image_transport"
-  "camera_info_manager")
+  "camera_info_manager"
+  "diagnostic_updater")
 
 target_compile_options(v4l2_camera PRIVATE -Werror)
 

include/v4l2_camera/v4l2_camera.hpp

@@ -20,6 +20,7 @@
 
 #include <camera_info_manager/camera_info_manager.hpp>
 #include <image_transport/image_transport.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
 
 #include <ostream>
 #include <rclcpp/rclcpp.hpp>
@@ -120,6 +121,10 @@ class V4L2Camera : public rclcpp::Node
   bool publish_next_frame_;
   bool use_image_transport_;
 
+  diagnostic_updater::Updater diagnostic_updater_{this};
+  rclcpp::Time last_capture_stamp_{rclcpp::Time(0)};
+  double capture_rate_{0.0};
+
 #ifdef ENABLE_CUDA
   // Memory region to communicate with GPU
   std::allocator<GPUMemoryManager> allocator_;
@@ -143,8 +148,8 @@ class V4L2Camera : public rclcpp::Node
   bool checkCameraInfo(
     sensor_msgs::msg::Image const & img,
     sensor_msgs::msg::CameraInfo const & ci);
+  void updateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
 };
-
 }  // namespace v4l2_camera
 
 #endif  // V4L2_CAMERA__V4L2_CAMERA_HPP_

package.xml

@@ -20,7 +20,8 @@
   <depend>sensor_msgs</depend>
   <depend>image_transport</depend>
   <depend>camera_info_manager</depend>
-
+  <depend>diagnostic_updater</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/v4l2_camera.cpp

@@ -103,6 +103,10 @@ V4L2Camera::V4L2Camera(rclcpp::NodeOptions const & options)
   // Read parameters and set up callback
   createParameters();
 
+  diagnostic_updater_.setHardwareID(get_name());
+  diagnostic_updater_.add("capture_status", this, &V4L2Camera::updateDiagnostics);
+  diagnostic_updater_.setPeriod(0.1);
+
   // Start the camera
   if (!camera_->start()) {
     return;
@@ -119,6 +123,7 @@ V4L2Camera::V4L2Camera(rclcpp::NodeOptions const & options)
           std::this_thread::sleep_for(std::chrono::milliseconds(10));
           continue;
         }
+
         if(publish_next_frame_ == false){
           continue;
         }
@@ -144,7 +149,10 @@ V4L2Camera::V4L2Camera(rclcpp::NodeOptions const & options)
         ci->header.stamp = stamp;
         ci->header.frame_id = camera_frame_id_;
         publish_next_frame_ = publish_rate_ < 0;
-
+        if(last_capture_stamp_.seconds() > 0){
+          capture_rate_ = 1.0 / (rclcpp::Time(stamp) - last_capture_stamp_).seconds();
+        }
+        last_capture_stamp_ = rclcpp::Time(stamp);
         if (use_image_transport_) {
           camera_transport_pub_.publish(*img, *ci);
         } else {
@@ -590,6 +598,25 @@ bool V4L2Camera::checkCameraInfo(
   return ci.width == img.width && ci.height == img.height;
 }
 
+void V4L2Camera::updateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  using diagnostic_msgs::msg::DiagnosticStatus;
+
+  auto current_stamp = get_clock()->now();
+  if(current_stamp.seconds() - last_capture_stamp_.seconds() > 5.0){
+    stat.summary(DiagnosticStatus::STALE, "timeout");
+  }else if (capture_rate_ < 1.0) {
+    stat.summary(DiagnosticStatus::ERROR, "error");
+  } else if (capture_rate_ < 5.0) {
+    stat.summary(DiagnosticStatus::WARN, "warn");
+  } else {
+    stat.summary(DiagnosticStatus::OK, "ok");
+  }
+  stat.addf("last_capture_stamp", "%.2f", last_capture_stamp_.seconds());
+  stat.addf("now", "%.2f", current_stamp.seconds());
+  stat.addf("Capture rate", "%.2f Hz", capture_rate_);
+}
+
 #ifdef ENABLE_CUDA
 sensor_msgs::msg::Image::UniquePtr V4L2Camera::convertOnGpu(sensor_msgs::msg::Image const & img)
 {