From cffbe3288c3b747b8331bd129915fabe2f76e0fe Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 14 May 2024 19:22:30 +0200 Subject: [PATCH 1/8] Add diagnostics to camera driver --- .../bitbots_basler_camera/CMakeLists.txt | 12 ++-- .../config/camera_settings.yaml | 7 ++ .../bitbots_basler_camera/package.xml | 5 +- .../src/basler_camera.cpp | 67 +++++++++++++++---- 4 files changed, 71 insertions(+), 20 deletions(-) diff --git a/bitbots_misc/bitbots_basler_camera/CMakeLists.txt b/bitbots_misc/bitbots_basler_camera/CMakeLists.txt index e257a0e49..b49c2bf23 100644 --- a/bitbots_misc/bitbots_basler_camera/CMakeLists.txt +++ b/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() diff --git a/bitbots_misc/bitbots_basler_camera/config/camera_settings.yaml b/bitbots_misc/bitbots_basler_camera/config/camera_settings.yaml index 65d50a41a..ef79a3cc6 100644 --- a/bitbots_misc/bitbots_basler_camera/config/camera_settings.yaml +++ b/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] diff --git a/bitbots_misc/bitbots_basler_camera/package.xml b/bitbots_misc/bitbots_basler_camera/package.xml index 88508bfbe..1d7cc8afd 100644 --- a/bitbots_misc/bitbots_basler_camera/package.xml +++ b/bitbots_misc/bitbots_basler_camera/package.xml @@ -13,13 +13,14 @@ Hamburg Bit-Bots bitbots_docs + camera_info_manager cv_bridge + diagnostic_msgs + generate_parameter_library image_transport libopencv-dev rclcpp sensor_msgs - camera_info_manager - generate_parameter_library ament_cmake diff --git a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp index eeb81bfd2..c5a0f2313 100644 --- a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp +++ b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include #include #include @@ -27,29 +29,30 @@ class BaslerCamera { std::shared_ptr node_; image_transport::TransportHints transport_hints_; - std::unique_ptr image_pub_; + image_transport::CameraPublisher image_pub_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; std::unique_ptr camera_info_manager_; std::unique_ptr camera_; - double camera_tick_frequency_ = 1; + std::optional camera_tick_frequency_; Pylon::PylonAutoInitTerm autoInitTerm; - std::unique_ptr param_listener_; + pylon_camera_parameters::ParamListener param_listener_; pylon_camera_parameters::Params config_; public: BaslerCamera() : node_(std::make_shared("post_processor")), transport_hints_(node_.get()), - image_pub_(std::make_unique( - 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("/diagnostics", 1)), + param_listener_(node_) { // Load parameters - param_listener_ = std::make_unique(node_); - config_ = param_listener_->get_params(); + config_ = param_listener_.get_params(); // Set up camera info manager camera_info_manager_ = std::make_unique(node_.get(), config_.device_user_id, @@ -63,9 +66,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(1.0 / config_.fps), std::bind(&BaslerCamera::timer_callback, this)); @@ -93,6 +96,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 with id " + config_.device_user_id); // Wait rclcpp::sleep_for(std::chrono::duration_cast( std::chrono::duration(config_.reconnect_interval))); @@ -156,6 +161,15 @@ class BaslerCamera { camera_->ExposureTimeAbs.SetValue(config_.exposure); } + void publish_basic_diagnostics(unsigned char severity, const std::string& message) { + diagnostic_msgs::msg::DiagnosticStatus status; + status.level = severity; + status.message = message; + diagnostic_msgs::msg::DiagnosticArray diagnostics; + diagnostics.status.push_back(status); + diagnostics_pub_->publish(diagnostics); + } + void timer_callback() { // This smart pointer will receive the grab result data. CGrabResultPtr ptrGrabResult; @@ -165,16 +179,18 @@ class BaslerCamera { 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 +210,19 @@ 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); } 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 +255,29 @@ 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(i * step_height, j * step_width); + luminance += (pixel[0] + pixel[1] + pixel[2]) / (3 * 255.0 * sample_grid * sample_grid); + } + } + + // Warn if the image is too dark + 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); + } else { + // Everything is fine + publish_basic_diagnostics(diagnostic_msgs::msg::DiagnosticStatus::OK, "Camera is running"); + } } /** From 0173d0688ccb2f8a9dfc57832178b7d9a7a6eea1 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 14 May 2024 19:32:07 +0200 Subject: [PATCH 2/8] Fix terminal spam --- bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp b/bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp index 4b212118a..45f2ab430 100644 --- a/bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp +++ b/bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp @@ -57,7 +57,7 @@ void OdometryFuser::loop() { // This wait clogs the executor, leading to a deadlock. It locks because with the clogged executor, // the fused_time_ is not updated, because the callback updating it is not executed if (this->now() - fused_time_ > rclcpp::Duration::from_seconds(0.1)) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(this->get_logger(), *this->get_clock(), 2, + RCLCPP_WARN_SKIPFIRST_THROTTLE(this->get_logger(), *this->get_clock(), 2 * 1000, "Fused time is too old, this should only happen if we get no new data"); return; } From bc63963b01c162d6a211a21253341589cd24355c Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 14 May 2024 19:39:16 +0200 Subject: [PATCH 3/8] Add camera name --- bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp index c5a0f2313..18b68ea6f 100644 --- a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp +++ b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp @@ -163,6 +163,8 @@ class BaslerCamera { void publish_basic_diagnostics(unsigned char severity, const std::string& message) { diagnostic_msgs::msg::DiagnosticStatus status; + status.name = "Camera"; + status.hardware_id = config_.device_user_id; status.level = severity; status.message = message; diagnostic_msgs::msg::DiagnosticArray diagnostics; From 2fa89b0579067fbf1de0b60e6ae8cc96dad9123b Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 14 May 2024 19:43:41 +0200 Subject: [PATCH 4/8] Set timestamp for diagnostics --- bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp index 18b68ea6f..bed6c4134 100644 --- a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp +++ b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp @@ -168,6 +168,7 @@ class BaslerCamera { status.level = severity; status.message = message; diagnostic_msgs::msg::DiagnosticArray diagnostics; + diagnostics.header.stamp = node_->now(); diagnostics.status.push_back(status); diagnostics_pub_->publish(diagnostics); } From 42340b8f31dd0008ff1e658fbc5adbc0010f28ad Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 14 May 2024 19:22:00 +0000 Subject: [PATCH 5/8] Use global diagnostigs aggregator --- .../launch/ros_control.launch | 3 --- .../launch/ros_control_standalone.launch | 2 ++ .../src/basler_camera.cpp | 2 +- .../launch/rosbag_record.launch.py | 1 + .../bitbots_bringup/launch/teamplayer.launch | 3 +++ .../launch/vision_standalone.launch | 3 +++ .../bitbots_diagnostic/CMakeLists.txt | 10 +++++++++ .../bitbots_diagnostic}/config/analyzers.yaml | 6 +++++ .../launch/aggregator.launch | 7 ++++++ bitbots_misc/bitbots_diagnostic/package.xml | 22 +++++++++++++++++++ bitbots_misc/bitbots_diagnostic/setup.py | 7 ++++++ bitbots_motion/bitbots_rl_motion/package.xml | 1 - 12 files changed, 62 insertions(+), 5 deletions(-) create mode 100644 bitbots_misc/bitbots_diagnostic/CMakeLists.txt rename {bitbots_lowlevel/bitbots_ros_control => bitbots_misc/bitbots_diagnostic}/config/analyzers.yaml (88%) create mode 100644 bitbots_misc/bitbots_diagnostic/launch/aggregator.launch create mode 100644 bitbots_misc/bitbots_diagnostic/package.xml create mode 100644 bitbots_misc/bitbots_diagnostic/setup.py diff --git a/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch b/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch index e7230f8bd..43ebbb957 100644 --- a/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch +++ b/bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch @@ -15,9 +15,6 @@ - - - diff --git a/bitbots_lowlevel/bitbots_ros_control/launch/ros_control_standalone.launch b/bitbots_lowlevel/bitbots_ros_control/launch/ros_control_standalone.launch index 312f13b84..475475733 100644 --- a/bitbots_lowlevel/bitbots_ros_control/launch/ros_control_standalone.launch +++ b/bitbots_lowlevel/bitbots_ros_control/launch/ros_control_standalone.launch @@ -10,6 +10,8 @@ + + diff --git a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp index bed6c4134..a3415c4d9 100644 --- a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp +++ b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp @@ -163,7 +163,7 @@ class BaslerCamera { void publish_basic_diagnostics(unsigned char severity, const std::string& message) { diagnostic_msgs::msg::DiagnosticStatus status; - status.name = "Camera"; + status.name = "CAMERA"; status.hardware_id = config_.device_user_id; status.level = severity; status.message = message; diff --git a/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py b/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py index 7e4bee523..9eb9e6215 100644 --- a/bitbots_misc/bitbots_bringup/launch/rosbag_record.launch.py +++ b/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", diff --git a/bitbots_misc/bitbots_bringup/launch/teamplayer.launch b/bitbots_misc/bitbots_bringup/launch/teamplayer.launch index df12e5163..54c3c261c 100644 --- a/bitbots_misc/bitbots_bringup/launch/teamplayer.launch +++ b/bitbots_misc/bitbots_bringup/launch/teamplayer.launch @@ -20,6 +20,9 @@ + + + diff --git a/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch b/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch index 38839102e..f3a8ff0f1 100644 --- a/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch +++ b/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch @@ -10,6 +10,9 @@ + + + diff --git a/bitbots_misc/bitbots_diagnostic/CMakeLists.txt b/bitbots_misc/bitbots_diagnostic/CMakeLists.txt new file mode 100644 index 000000000..0b7a75f0d --- /dev/null +++ b/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() diff --git a/bitbots_lowlevel/bitbots_ros_control/config/analyzers.yaml b/bitbots_misc/bitbots_diagnostic/config/analyzers.yaml similarity index 88% rename from bitbots_lowlevel/bitbots_ros_control/config/analyzers.yaml rename to bitbots_misc/bitbots_diagnostic/config/analyzers.yaml index 0b9019bd7..0b61697ce 100644 --- a/bitbots_lowlevel/bitbots_ros_control/config/analyzers.yaml +++ b/bitbots_misc/bitbots_diagnostic/config/analyzers.yaml @@ -2,6 +2,12 @@ analyzers: ros__parameters: pub_rate: 10.0 # Optional path: '' # Optional, prepended to all diagnostic output + Camera: + type: diagnostic_aggregator/GenericAnalyzer + path: Servos + startswith: [ 'CAMERA'] + timeout: 0.2 + find_and_remove_prefix: ['CAMERA'] Servos: type: diagnostic_aggregator/GenericAnalyzer path: Servos diff --git a/bitbots_misc/bitbots_diagnostic/launch/aggregator.launch b/bitbots_misc/bitbots_diagnostic/launch/aggregator.launch new file mode 100644 index 000000000..63e77dceb --- /dev/null +++ b/bitbots_misc/bitbots_diagnostic/launch/aggregator.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/bitbots_misc/bitbots_diagnostic/package.xml b/bitbots_misc/bitbots_diagnostic/package.xml new file mode 100644 index 000000000..6c22c8392 --- /dev/null +++ b/bitbots_misc/bitbots_diagnostic/package.xml @@ -0,0 +1,22 @@ + + + + bitbots_diagnostic + 1.0.0 + This package sets up the diagnostic aggregator for the bit-bots software. + + Florian Vahl + Hamburg Bit-Bots + + MIT + + Hamburg Bit-Bots + + diagnostic_aggregator + + ament_cmake + + + ament_cmake + + diff --git a/bitbots_misc/bitbots_diagnostic/setup.py b/bitbots_misc/bitbots_diagnostic/setup.py new file mode 100644 index 000000000..40eb022b2 --- /dev/null +++ b/bitbots_misc/bitbots_diagnostic/setup.py @@ -0,0 +1,7 @@ +from distutils.core import setup + +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup() + +setup(**d) diff --git a/bitbots_motion/bitbots_rl_motion/package.xml b/bitbots_motion/bitbots_rl_motion/package.xml index 88510a310..38546d848 100644 --- a/bitbots_motion/bitbots_rl_motion/package.xml +++ b/bitbots_motion/bitbots_rl_motion/package.xml @@ -18,7 +18,6 @@ bitbots_docs bitbots_robot_description bitbots_utils - diagnostic_aggregator rclpy std_msgs 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 6/8] 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 From ccb83e52ee088119dce908de97f09955585fdc78 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Thu, 16 May 2024 10:46:53 +0000 Subject: [PATCH 7/8] Add missing depends on bitbots_diagnostic --- bitbots_lowlevel/bitbots_ros_control/package.xml | 1 + bitbots_misc/bitbots_bringup/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/bitbots_lowlevel/bitbots_ros_control/package.xml b/bitbots_lowlevel/bitbots_ros_control/package.xml index e7362bfed..23abbeb60 100644 --- a/bitbots_lowlevel/bitbots_ros_control/package.xml +++ b/bitbots_lowlevel/bitbots_ros_control/package.xml @@ -19,6 +19,7 @@ backward_ros bitbots_buttons + bitbots_diagnostic bitbots_docs bitbots_msgs bitbots_robot_description diff --git a/bitbots_misc/bitbots_bringup/package.xml b/bitbots_misc/bitbots_bringup/package.xml index 6ad096fef..cb31a78b4 100644 --- a/bitbots_misc/bitbots_bringup/package.xml +++ b/bitbots_misc/bitbots_bringup/package.xml @@ -20,6 +20,7 @@ audio_common bitbots_basler_camera bitbots_body_behavior + bitbots_diagnostic bitbots_dynamic_kick bitbots_dynup bitbots_hcm From fc80df4b205700056b8e0cf3416334d3c4fd0123 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 16 May 2024 16:55:57 +0200 Subject: [PATCH 8/8] Update bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp Co-authored-by: Jan Gutsche <34797331+jaagut@users.noreply.github.com> --- bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp index f4bff802c..c1f6b2e92 100644 --- a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp +++ b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp @@ -316,8 +316,8 @@ class BaslerCamera { // 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())}}; + {"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) {