From aa6bd7ea07a8600339d9aa914675eef6dd60cdd7 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Wed, 15 May 2024 15:28:34 +0200 Subject: [PATCH] Add more detailed diagnostic --- .pre-commit-config.yaml | 1 + .../src/basler_camera.cpp | 73 ++++++++++++++++--- .../bitbots_diagnostic/config/analyzers.yaml | 4 +- sync_includes_wolfgang_nuc.yaml | 1 + 4 files changed, 67 insertions(+), 12 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ff2fbd628..2c0c5e37a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,6 +15,7 @@ repos: - "-i" - id: cppcheck args: + - "--inline-suppr" - "--suppress=missingInclude" - "--suppress=unmatchedSuppression" - "--suppress=unusedFunction" diff --git a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp index a3415c4d9..f4bff802c 100644 --- a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp +++ b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp @@ -44,6 +44,22 @@ class BaslerCamera { pylon_camera_parameters::ParamListener param_listener_; pylon_camera_parameters::Params config_; + const std::map 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 + 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("post_processor")), @@ -66,7 +82,7 @@ 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"); + publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Could not initialize camera", {}); exit(1); } // Setup timer for publishing @@ -97,7 +113,7 @@ class BaslerCamera { 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 with id " + config_.device_user_id); + "Could not find the device '" + config_.device_user_id + "'!", {}); // Wait rclcpp::sleep_for(std::chrono::duration_cast( std::chrono::duration(config_.reconnect_interval))); @@ -161,12 +177,28 @@ class BaslerCamera { camera_->ExposureTimeAbs.SetValue(config_.exposure); } - void publish_basic_diagnostics(unsigned char severity, const std::string& message) { + void publish_basic_diagnostics(unsigned char severity, const std::string& message, + const std::map& 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 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); @@ -180,6 +212,10 @@ class BaslerCamera { // Field to store the trigger time rclcpp::Time trigger_time; + // Camera metrics + std::optional temperature; + std::optional temperature_state; + try { // Check if the config has changed if (param_listener_.is_old(config_)) { @@ -193,7 +229,7 @@ class BaslerCamera { if (!camera_->IsOpen() or camera_->IsCameraDeviceRemoved()) { auto message = "Camera connection lost. Reinitializing camera"; RCLCPP_WARN(node_->get_logger(), message); - publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::STALE, message); + publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::STALE, message, {}); initilize_camera(); } @@ -213,7 +249,7 @@ 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"); + publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Error while grabbing image", {}); return; } @@ -221,11 +257,16 @@ class BaslerCamera { auto image_camera_stamp = (__int128_t)ptrGrabResult->GetTimeStamp(); 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"); + "An exception occurred during image acquisition", {}); return; } @@ -272,14 +313,26 @@ class BaslerCamera { } } - // Warn if the image is too dark - if (luminance < config_.misc.darkness_threshold) { + // Build map for additional diagnostics metrics + const std::map diagnostics_metrics = { + {"temperature", std::to_string(temperature.value())}, + {"luminance", std::to_string(luminance)}, + {"temperature_state", TEMP_STATE_2_STRING.at(temperature_state.value())}}; + + // 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); + 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"); + publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::OK, "Camera is running", diagnostics_metrics); } } diff --git a/bitbots_misc/bitbots_diagnostic/config/analyzers.yaml b/bitbots_misc/bitbots_diagnostic/config/analyzers.yaml index 0b61697ce..cefcb730a 100644 --- a/bitbots_misc/bitbots_diagnostic/config/analyzers.yaml +++ b/bitbots_misc/bitbots_diagnostic/config/analyzers.yaml @@ -1,10 +1,10 @@ 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: Servos + path: Camera startswith: [ 'CAMERA'] timeout: 0.2 find_and_remove_prefix: ['CAMERA'] diff --git a/sync_includes_wolfgang_nuc.yaml b/sync_includes_wolfgang_nuc.yaml index d636ba0c1..6a393a532 100644 --- a/sync_includes_wolfgang_nuc.yaml +++ b/sync_includes_wolfgang_nuc.yaml @@ -8,6 +8,7 @@ include: - bitbots_misc: - bitbots_basler_camera - bitbots_bringup + - bitbots_diagnostic - bitbots_docs - bitbots_extrinsic_calibration - bitbots_ipm