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) {