Basler camera diagnostics (#455)

.pre-commit-config.yaml

@@ -15,6 +15,7 @@ repos:
           - "-i"
       - id: cppcheck
         args:
+          - "--inline-suppr"
           - "--suppress=missingInclude"
           - "--suppress=unmatchedSuppression"
           - "--suppress=unusedFunction"

bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch

@@ -15,9 +15,6 @@
         <param name="only_pressure" value="$(var only_pressure)"/>
     </node>
 
-    <node pkg="diagnostic_aggregator" exec="aggregator_node" args="--ros-args --log-level WARN">
-        <param from="$(find-pkg-share bitbots_ros_control)/config/analyzers.yaml" />
-    </node>
     <node pkg="bitbots_ros_control" exec="led_error_blink.py" name="error_blink" output="screen"/>
 
     <node pkg="bitbots_ros_control" name="button_zero" exec="zero_on_button.py" output="screen"/>

bitbots_lowlevel/bitbots_ros_control/launch/ros_control_standalone.launch

@@ -10,6 +10,8 @@
         <arg name="sim" value="$(var sim)"/>
     </include>
 
+    <include file="$(find-pkg-share bitbots_diagnostic)/launch/aggregator.launch" />
+
     <include file="$(find-pkg-share bitbots_robot_description)/launch/load_robot_description.launch" >
         <arg name="sim" value="$(var sim)"/>
         <arg name="robot_type" value="$(var robot_type)"/>

bitbots_lowlevel/bitbots_ros_control/package.xml

@@ -19,6 +19,7 @@
 
   <depend>backward_ros</depend>
   <depend>bitbots_buttons</depend>
+  <depend>bitbots_diagnostic</depend>
   <depend>bitbots_docs</depend>
   <depend>bitbots_msgs</depend>
   <depend>bitbots_robot_description</depend>

bitbots_misc/bitbots_basler_camera/CMakeLists.txt

@@ -10,14 +10,15 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(backward_ros REQUIRED)
 find_package(bitbots_docs REQUIRED)
+find_package(camera_info_manager REQUIRED)
 find_package(cv_bridge REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(OpenCV REQUIRED)
+find_package(pylon 7.1.0 REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(pylon 7.1.0 REQUIRED)
-find_package(camera_info_manager REQUIRED)
-find_package(generate_parameter_library REQUIRED)
 
 add_compile_options(-Wall -Werror -Wno-unused)
 
@@ -35,12 +36,13 @@ ament_target_dependencies(
   ament_cmake
   ament_index_cpp
   bitbots_docs
+  camera_info_manager
   cv_bridge
+  diagnostic_msgs
+  generate_parameter_library
   image_transport
   rclcpp
   sensor_msgs
-  camera_info_manager
-  generate_parameter_library
   OpenCV)
 
 enable_bitbots_docs()

bitbots_misc/bitbots_basler_camera/config/camera_settings.yaml

@@ -74,3 +74,10 @@ pylon_camera_parameters:
       read_only: true
       validation:
         bounds<>: [0, 10000]
+  misc:
+    darkness_threshold:
+      type: double
+      default_value: 0.1
+      description: "This threshold is used to determine if the image is too dark, which means that we might forgot the camera cover."
+      validation:
+        bounds<>: [0, 1]

bitbots_misc/bitbots_basler_camera/package.xml

@@ -13,13 +13,14 @@
   <author email="[email protected]">Hamburg Bit-Bots</author>
 
   <depend>bitbots_docs</depend>
+  <depend>camera_info_manager</depend>
   <depend>cv_bridge</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>generate_parameter_library</depend>
   <depend>image_transport</depend>
   <depend>libopencv-dev</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
-  <depend>camera_info_manager</depend>
-  <depend>generate_parameter_library</depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp

@@ -6,6 +6,8 @@
 #include <camera_info_manager/camera_info_manager.hpp>
 #include <cmath>
 #include <cv_bridge/cv_bridge.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <image_transport/image_transport.hpp>
 #include <iostream>
 #include <memory>
@@ -27,29 +29,46 @@ class BaslerCamera {
   std::shared_ptr<rclcpp::Node> node_;
 
   image_transport::TransportHints transport_hints_;
-  std::unique_ptr<image_transport::CameraPublisher> image_pub_;
+  image_transport::CameraPublisher image_pub_;
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
 
   std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
 
   std::unique_ptr<Pylon::CBaslerUniversalInstantCamera> camera_;
 
-  double camera_tick_frequency_ = 1;
+  std::optional<double> camera_tick_frequency_;
 
   Pylon::PylonAutoInitTerm autoInitTerm;
 
-  std::unique_ptr<pylon_camera_parameters::ParamListener> param_listener_;
+  pylon_camera_parameters::ParamListener param_listener_;
   pylon_camera_parameters::Params config_;
 
+  const std::map<const Basler_UniversalCameraParams::TemperatureStateEnums, const std::string> TEMP_STATE_2_STRING = {
+      {Basler_UniversalCameraParams::TemperatureStateEnums::TemperatureState_Ok, "OK"},
+      {Basler_UniversalCameraParams::TemperatureStateEnums::TemperatureState_Critical, "critical"},
+      {Basler_UniversalCameraParams::TemperatureStateEnums::TemperatureState_Error, "error"},
+  };
+
+  const std::map<const Basler_UniversalCameraParams::TemperatureStateEnums, const unsigned char>
+      TEMP_STATE_2_DIAGNOSTIC_STATE = {
+          {Basler_UniversalCameraParams::TemperatureStateEnums::TemperatureState_Ok,
+           diagnostic_msgs::msg::DiagnosticStatus::OK},
+          {Basler_UniversalCameraParams::TemperatureStateEnums::TemperatureState_Critical,
+           diagnostic_msgs::msg::DiagnosticStatus::WARN},
+          {Basler_UniversalCameraParams::TemperatureStateEnums::TemperatureState_Error,
+           diagnostic_msgs::msg::DiagnosticStatus::ERROR},
+  };
+
  public:
   BaslerCamera()
       : node_(std::make_shared<rclcpp::Node>("post_processor")),
         transport_hints_(node_.get()),
-        image_pub_(std::make_unique<image_transport::CameraPublisher>(
-            image_transport::create_camera_publisher(node_.get(), "camera/image_proc"))) {
+        image_pub_(image_transport::create_camera_publisher(node_.get(), "camera/image_proc")),
+        diagnostics_pub_(node_->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 1)),
+        param_listener_(node_) {
     // Load parameters
-    param_listener_ = std::make_unique<pylon_camera_parameters::ParamListener>(node_);
-    config_ = param_listener_->get_params();
+    config_ = param_listener_.get_params();
 
     // Set up camera info manager
     camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(node_.get(), config_.device_user_id,
@@ -63,9 +82,9 @@ class BaslerCamera {
       // Error handling.
       RCLCPP_ERROR(node_->get_logger(), "An exception occurred: %s", e.GetDescription());
       RCLCPP_ERROR(node_->get_logger(), "Could not initialize camera");
+      publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Could not initialize camera", {});
       exit(1);
     }
-
     // Setup timer for publishing
     timer_ = node_->create_wall_timer(std::chrono::duration<double>(1.0 / config_.fps),
                                       std::bind(&BaslerCamera::timer_callback, this));
@@ -93,6 +112,8 @@ class BaslerCamera {
     while (rclcpp::ok() && !our_device_info) {
       RCLCPP_ERROR(node_->get_logger(), "Could not find device with user id '%s'", config_.device_user_id.c_str());
       RCLCPP_ERROR(node_->get_logger(), "Retrying in %f seconds", config_.reconnect_interval);
+      publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::STALE,
+                                "Could not find the device '" + config_.device_user_id + "'!", {});
       // Wait
       rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(
           std::chrono::duration<double>(config_.reconnect_interval)));
@@ -156,25 +177,59 @@ class BaslerCamera {
     camera_->ExposureTimeAbs.SetValue(config_.exposure);
   }
 
+  void publish_basic_diagnostics(unsigned char severity, const std::string& message,
+                                 const std::map<const std::string, const std::string>& additional_data) {
+    // Add general infos
+    diagnostic_msgs::msg::DiagnosticStatus status;
+    status.name = "CAMERA";
+    status.hardware_id = config_.device_user_id;
+
+    // Add overall status
+    status.level = severity;
+    status.message = message;
+
+    // Add additional data (e.g. temperature)
+    std::vector<diagnostic_msgs::msg::KeyValue> keyValues;
+    for (auto const& [key, value] : additional_data) {  // cppcheck-suppress unassignedVariable
+      diagnostic_msgs::msg::KeyValue key_value;
+      key_value.key = key;
+      key_value.value = value;
+      keyValues.push_back(key_value);
+    }
+    status.values = keyValues;
+
+    // Publish the diagnostics array (only one status, as we only have one camera)
+    diagnostic_msgs::msg::DiagnosticArray diagnostics;
+    diagnostics.header.stamp = node_->now();
+    diagnostics.status.push_back(status);
+    diagnostics_pub_->publish(diagnostics);
+  }
+
   void timer_callback() {
     // This smart pointer will receive the grab result data.
     CGrabResultPtr ptrGrabResult;
 
     // Field to store the trigger time
     rclcpp::Time trigger_time;
 
+    // Camera metrics
+    std::optional<float> temperature;
+    std::optional<Basler_UniversalCameraParams::TemperatureStateEnums> temperature_state;
+
     try {
       // Check if the config has changed
-      if (param_listener_->is_old(config_)) {
+      if (param_listener_.is_old(config_)) {
         // Update the camera parameters
-        config_ = param_listener_->get_params();
+        config_ = param_listener_.get_params();
         // Apply the new camera parameters
         apply_camera_parameters();
       }
 
       // Try to reinitialize the camera if the connection is lost
       if (!camera_->IsOpen() or camera_->IsCameraDeviceRemoved()) {
-        RCLCPP_WARN(node_->get_logger(), "Camera connection lost. Reinitializing camera");
+        auto message = "Camera connection lost. Reinitializing camera";
+        RCLCPP_WARN(node_->get_logger(), message);
+        publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::STALE, message, {});
         initilize_camera();
       }
 
@@ -194,16 +249,24 @@ class BaslerCamera {
       if (!ptrGrabResult->GrabSucceeded()) {
         RCLCPP_ERROR(node_->get_logger(), "Error: %x %s", ptrGrabResult->GetErrorCode(),
                      ptrGrabResult->GetErrorDescription().c_str());
+        publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Error while grabbing image", {});
         return;
       }
 
       // Adjust the time stamp of the image
       auto image_camera_stamp = (__int128_t)ptrGrabResult->GetTimeStamp();
-      auto image_age_in_seconds = (image_camera_stamp - camera_time_stamp_at_capture) / camera_tick_frequency_;
+      auto image_age_in_seconds = (image_camera_stamp - camera_time_stamp_at_capture) / camera_tick_frequency_.value();
       trigger_time = ros_time_stamp_at_capture + rclcpp::Duration::from_seconds(image_age_in_seconds);
+
+      // Get the camera temperature
+      temperature = camera_->TemperatureAbs.GetValue();
+      temperature_state = camera_->TemperatureState.GetValue();
+
     } catch (GenICam::GenericException& e) {
       // Error handling.
       RCLCPP_ERROR(node_->get_logger(), "An exception occurred: %s", e.GetDescription());
+      publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::ERROR,
+                                "An exception occurred during image acquisition", {});
       return;
     }
 
@@ -236,7 +299,41 @@ class BaslerCamera {
     color_msg.image = binned;
 
     // Publish the image
-    image_pub_->publish(color_msg.toImageMsg(), camera_info);
+    image_pub_.publish(color_msg.toImageMsg(), camera_info);
+
+    // Check if image is too dark
+    float luminance = 0;
+    int sample_grid = 5;
+    int step_height = binned.size().height / sample_grid;
+    int step_width = binned.size().width / sample_grid;
+    for (int i = 0; i < sample_grid; i++) {
+      for (int j = 0; j < sample_grid; j++) {
+        cv::Vec3b pixel = binned.at<cv::Vec3b>(i * step_height, j * step_width);
+        luminance += (pixel[0] + pixel[1] + pixel[2]) / (3 * 255.0 * sample_grid * sample_grid);
+      }
+    }
+
+    // Build map for additional diagnostics metrics
+    const std::map<const std::string, const std::string> diagnostics_metrics = {
+        {"temperature", std::to_string(temperature.value())},
+        {"temperature_state", TEMP_STATE_2_STRING.at(temperature_state.value())},
+        {"luminance", std::to_string(luminance)}};
+
+    // Warn if the camera is too hot or the image is too dark
+    if (temperature_state.value() != Basler_UniversalCameraParams::TemperatureStateEnums::TemperatureState_Ok) {
+      auto message = "Camera temperature (" + std::to_string((int)round(temperature.value())) + "°C) is " +
+                     TEMP_STATE_2_STRING.at(temperature_state.value()) + "! Thresholds are 72°C and 78°C respectively.";
+      RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 10000, message.c_str());
+      publish_basic_diagnostics(TEMP_STATE_2_DIAGNOSTIC_STATE.at(temperature_state.value()), message,
+                                diagnostics_metrics);
+    } else if (luminance < config_.misc.darkness_threshold) {
+      auto message = "Image is too dark. Did you forget the camera cover?";
+      RCLCPP_WARN_ONCE(node_->get_logger(), message);
+      publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::WARN, message, diagnostics_metrics);
+    } else {
+      // Everything is fine
+      publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::OK, "Camera is running", diagnostics_metrics);
+    }
   }
 
   /**

bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py

@@ -49,6 +49,7 @@
     "/joint_states",
     "/motion_odometry",
     "/move_base/current_goal",
+    "/path",
     "/pose_with_covariance",
     "/robot_state",
     "/robots_relative_filtered",

bitbots_misc/bitbots_bringup/launch/teamplayer.launch

@@ -20,6 +20,9 @@
         <arg name="sim" value="$(var sim)" />
     </include>
 
+    <!-- load the diagnostic aggregator -->
+    <include file="$(find-pkg-share bitbots_diagnostic)/launch/aggregator.launch" />
+
     <!-- load the robot description -->
     <include file="$(find-pkg-share bitbots_robot_description)/launch/load_robot_description.launch">
         <arg name="sim" value="$(var sim)"/>

bitbots_misc/bitbots_bringup/launch/vision_standalone.launch

@@ -10,6 +10,9 @@
         <arg name="sim" value="$(var sim)" />
     </include>
 
+    <!-- Load the diagnostic aggregator-->
+    <include file="$(find-pkg-share bitbots_diagnostic)/launch/aggregator.launch" />
+
     <!-- Start the vision-->
     <include file="$(find-pkg-share bitbots_bringup)/launch/vision.launch">
         <arg name="sim" value="$(var sim)" />

bitbots_misc/bitbots_bringup/package.xml

@@ -20,6 +20,7 @@
     <exec_depend>audio_common</exec_depend>
     <exec_depend>bitbots_basler_camera</exec_depend>
     <exec_depend>bitbots_body_behavior</exec_depend>
+    <exec_depend>bitbots_diagnostic</exec_depend>
     <exec_depend>bitbots_dynamic_kick</exec_depend>
     <exec_depend>bitbots_dynup</exec_depend>
     <exec_depend>bitbots_hcm</exec_depend>

bitbots_misc/bitbots_diagnostic/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.5)
+project(bitbots_diagnostic)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+ament_package()

bitbots_lowlevel/bitbots_ros_control/config/analyzers.yaml → bitbots_misc/bitbots_diagnostic/config/analyzers.yaml

@@ -1,7 +1,13 @@
 analyzers:
   ros__parameters:
     pub_rate: 10.0 # Optional
-    path: '' # Optional, prepended to all diagnostic output
+    path: 'Robot' # Optional, prepended to all diagnostic output
+    Camera:
+      type: diagnostic_aggregator/GenericAnalyzer
+      path: Camera
+      startswith: [ 'CAMERA']
+      timeout: 0.2
+      find_and_remove_prefix: ['CAMERA']
     Servos:
       type: diagnostic_aggregator/GenericAnalyzer
       path: Servos

bitbots_misc/bitbots_diagnostic/launch/aggregator.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+    <node pkg="diagnostic_aggregator" exec="aggregator_node" args="--ros-args --log-level WARN">
+        <param from="$(find-pkg-share bitbots_diagnostic)/config/analyzers.yaml" />
+    </node>
+</launch>
+