Feature: read and publish camera diagnostics

CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
   nodelet
   std_msgs
   sensor_msgs
+  diagnostic_msgs
   message_generation
   image_transport
   camera_info_manager
@@ -28,12 +29,14 @@ find_package(catkin REQUIRED COMPONENTS
 )
 find_package(Aravis REQUIRED)
 find_package(GLIB2 REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 generate_dynamic_reconfigure_options(cfg/CameraAravis.cfg)
 
 add_message_files(
    FILES
    CameraAutoInfo.msg
+   CameraDiagnostics.msg
    ExtendedCameraInfo.msg
 )
 
@@ -53,6 +56,7 @@ generate_messages(
    DEPENDENCIES
    std_msgs
    sensor_msgs
+   diagnostic_msgs
 )
 
 catkin_package(
@@ -66,14 +70,17 @@ include_directories(cfg
   include
   ${catkin_INCLUDE_DIRS}
   ${Aravis_INCLUDE_DIRS}
-  ${GLIB2_INCLUDE_DIRS})
+  ${GLIB2_INCLUDE_DIRS}
+  ${YAML_CPP_INCLUDE_DIRS}
+)
 
 set(LIBRARIES
   ${catkin_LIBRARIES}
   ${Aravis_LIBRARIES}
   glib-2.0
   gmodule-2.0
   gobject-2.0
+  ${YAML_CPP_LIBRARY_DIRS}
 )
 
 add_library(${PROJECT_NAME}

README.md

@@ -127,6 +127,32 @@ be executed after the parameters above are set.
 	- Type: ```string```
 	- Default: ```""```
 
+## Publishing camera diagnostics / status
+
+Camera_aravis allows to periodically monitor custom camera features and publish them in a designated
+topic named ```~/diagnostics``` in a message type as specified in 
+[CameraDiagnostics.msg](msg/CameraDiagnostics.msg). In order to configure and customize this 
+status monitoring, two launch parameters are provided:
+
+- ```diagnostic_publish_rate```: Rate at which to read and publish the diagnostic data.
+	- Type: ```double```
+	- Default: ```0.1``` (10 seconds)
+- ```diagnostic_yaml_url```: URL to yaml file specifying the camera features which are to be 
+monitored. If left empty (as default) no diagnostic features will be read and published.
+	- Type: ```string```
+	- Default: ```""```
+
+An example of such a diagnostic yaml file is given in 
+[camera_diagnostics.yaml](launch/camera_diagnostics.yaml). This file should hold a list of 
+```FeatureName``` together with a corresponding ```Type``` (bool, float, int, or string) for each
+feature which is to be monitored. If a feature is associated with a feature selector, one can 
+additionally specify a list of ```Selectors```. Each entry in this list should again have a 
+```FeatureName``` and ```Type```, as well as a ```Value``` to set.
+
+For each feature a key-value pair is constructed and published in the ```data``` field of the 
+message stated above. If a feature as a list of selectors, one key-value pair is constructed for
+each Feature-Selector pair.
+
 ## Known Issues
 
 ### Slow read of white balance and black level values

include/camera_aravis/camera_aravis_nodelet.h

@@ -54,13 +54,16 @@ extern "C" {
 #include <camera_info_manager/camera_info_manager.h>
 #include <boost/algorithm/string/trim.hpp>
 
+#include <yaml-cpp/yaml.h>
+
 #include <dynamic_reconfigure/server.h>
 #include <dynamic_reconfigure/SensorLevels.h>
 #include <tf/transform_listener.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <camera_aravis/CameraAravisConfig.h>
 #include <camera_aravis/CameraAutoInfo.h>
+#include <camera_aravis/CameraDiagnostics.h>
 #include <camera_aravis/ExtendedCameraInfo.h>
 
 #include <camera_aravis/get_integer_feature_value.h>
@@ -173,6 +176,8 @@ class CameraAravisNodelet : public nodelet::Nodelet
 
   void publishTfLoop(double rate);
 
+  void readAndPublishCameraDiagnostics(double rate) const;
+
   void discoverFeatures();
 
   static void parseStringArgs(std::string in_arg_string, std::vector<std::string> &out_args);
@@ -201,6 +206,13 @@ class CameraAravisNodelet : public nodelet::Nodelet
   std::thread tf_dyn_thread_;
   std::atomic_bool tf_thread_active_;
 
+  std::string diagnostic_yaml_url_ = "";
+  double diagnostic_publish_rate_ = 0.1;
+  YAML::Node  diagnostic_features_;
+  ros::Publisher diagnostic_pub_;
+  std::thread diagnostic_thread_;
+  std::atomic_bool diagnostic_thread_active_;
+
   CameraAutoInfo auto_params_;
   ros::Publisher auto_pub_;
   ros::Subscriber auto_sub_;

launch/camera_aravis.launch

@@ -30,6 +30,8 @@
       <param name="camera_info_url"      value="$(arg camera_info_url)"/>
       <param name="frame_id"             value="$(arg sensor_name)"/>
 
+      <param name="diagnostic_yaml_url"	 value="camera_diagnostics.yaml"/>
+
       <!-- use GenICam SFNC names as stream control parameters -->
       <param name="PixelFormat"          value="$(arg pixel_format)"/>
       <param name="Width"                value="$(arg width)"/>

launch/camera_diagnostics.yaml

@@ -0,0 +1,18 @@
+- FeatureName: DeviceTemperature
+  Type: float
+  Selectors:
+    - FeatureName: DeviceTemperatureSelector
+      Type: string
+      Value: Mainboard
+    - FeatureName: DeviceTemperatureSelector
+      Type: string
+      Value: Sensor
+- FeatureName: PtpEnable
+  Type: bool
+- FeatureName: PtpClockAccuracy
+  Type: string
+- FeatureName: PtpOperationMode
+  Type: string
+- FeatureName: PtpStatus
+  Type: string
+

msg/CameraDiagnostics.msg

@@ -0,0 +1,6 @@
+# Camera diagnostics message
+
+std_msgs/Header header
+
+# Array of key-value pairs holding the diagnostic data
+diagnostic_msgs/KeyValue[] data

package.xml

@@ -24,6 +24,7 @@
   <depend>nodelet</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>image_transport</depend>
   <depend>camera_info_manager</depend>
   <depend>dynamic_reconfigure</depend>

src/camera_aravis_nodelet.cpp

@@ -61,6 +61,12 @@ CameraAravisNodelet::~CameraAravisNodelet()
     tf_dyn_thread_.join();
   }
 
+  diagnostic_thread_active_ = false;
+  if (diagnostic_thread_.joinable())
+  {
+    diagnostic_thread_.join();
+  }
+
 
   for(int i=0; i < p_streams_.size(); i++) {
     guint64 n_completed_buffers = 0;
@@ -125,6 +131,10 @@ void CameraAravisNodelet::onInit()
   pnh.param("camera_info_url", calib_url_args, calib_url_args);
   parseStringArgs(calib_url_args, calib_urls);
 
+  diagnostic_yaml_url_ = pnh.param<std::string>("diagnostic_yaml_url", diagnostic_yaml_url_);
+  diagnostic_publish_rate_ = 
+    std::max(0.0, pnh.param<double>("diagnostic_publish_rate", 0.1));
+
   // Print out some useful info.
   ROS_INFO("Attached cameras:");
   arv_update_device_list();
@@ -335,6 +345,45 @@ void CameraAravisNodelet::onInit()
     }
   }
 
+  // read diagnostic features from yaml file and initialize publishing thread
+  if(!diagnostic_yaml_url_.empty() && diagnostic_publish_rate_ > 0.0)
+  {
+    try
+    {
+      diagnostic_features_ = YAML::LoadFile(diagnostic_yaml_url_);
+    }
+    catch(const YAML::BadFile& e)
+    {
+      ROS_WARN("YAML file cannot be loaded: %s", e.what());
+      ROS_WARN("Camera diagnostics will not be published.");
+    }
+    catch(const YAML::ParserException& e)
+    {
+      ROS_WARN("YAML file is malformed: %s", e.what());
+      ROS_WARN("Camera diagnostics will not be published.");
+    }
+
+    if(diagnostic_features_.size() > 0)
+    {
+      diagnostic_pub_ = getNodeHandle().advertise<CameraDiagnostics>(
+        ros::names::remap(this->getName() + "/diagnostics"), 1, true);
+
+      diagnostic_thread_active_ = true;
+      diagnostic_thread_ = std::thread(&CameraAravisNodelet::readAndPublishCameraDiagnostics, this, 
+                                       diagnostic_publish_rate_);
+    }
+    else
+    {
+      ROS_WARN("No diagnostic features specified.");
+      ROS_WARN("Camera diagnostics will not be published.");
+    }
+  }
+  else if(!diagnostic_yaml_url_.empty() && diagnostic_publish_rate_ <= 0.0)
+  {
+    ROS_WARN("diagnostic_publish_rate is <= 0.0");
+    ROS_WARN("Camera diagnostics will not be published.");
+  }
+
   // default calibration url is [DeviceSerialNumber/DeviceID].yaml
   ArvGcNode *p_gc_node;
   GError *error = NULL;
@@ -1441,6 +1490,154 @@ void CameraAravisNodelet::publishTfLoop(double rate)
   }
 }
 
+void CameraAravisNodelet::readAndPublishCameraDiagnostics(double rate) const
+{
+  ROS_INFO("Publishing camera diagnostics at %g Hz", rate);
+
+  ros::Rate loop_rate(rate);
+
+  CameraDiagnostics camDiagnosticMsg;
+  camDiagnosticMsg.header.frame_id = config_.frame_id;
+
+  // function to get feature value at given name and of given type as string
+  auto getFeatureValueAsStrFn = [&](const std::string &name, const std::string &type) 
+    -> std::string
+  {
+    std::string valueStr = "";
+
+    if(type == "float")
+      valueStr = std::to_string(arv_device_get_float_feature_value(p_device_, name.c_str()));
+    else if (type == "int")
+      valueStr = std::to_string(arv_device_get_integer_feature_value(p_device_, name.c_str()));
+    else if (type == "bool")
+      valueStr = arv_device_get_boolean_feature_value(p_device_, name.c_str()) ? "true" : "false";
+    else 
+      valueStr = arv_device_get_string_feature_value(p_device_, name.c_str());
+
+    return valueStr;
+  };
+
+  // function to set feature value at given name and of given type from string
+  auto setFeatureValueFromStrFn = [&](const std::string &name, const std::string &type, 
+                                    const std::string &valueStr) 
+  {
+    if(type == "float")
+      arv_device_set_float_feature_value(p_device_, name.c_str(), std::stod(valueStr));
+    else if (type == "int")
+      arv_device_set_integer_feature_value(p_device_, name.c_str(), std::stoi(valueStr));
+    else if (type == "bool")
+      arv_device_set_boolean_feature_value(p_device_, name.c_str(), 
+        (valueStr == "true" || valueStr == "True" || valueStr == "TRUE"));
+    else 
+      arv_device_set_string_feature_value(p_device_, name.c_str(), valueStr.c_str());
+  };
+
+  while (ros::ok() && diagnostic_thread_active_)
+  {
+    camDiagnosticMsg.header.stamp = ros::Time::now();
+    ++camDiagnosticMsg.header.seq;
+
+    camDiagnosticMsg.data.clear();
+
+    int featureIdx = 1;
+    for (auto featureDict : diagnostic_features_) {
+      std::string featureName = featureDict["FeatureName"].IsDefined() 
+                                ? featureDict["FeatureName"].as<std::string>()
+                                : "";
+      std::string featureType = featureDict["Type"].IsDefined() 
+                                ? featureDict["Type"].as<std::string>()
+                                : "";
+
+      if(featureName.empty() || featureType.empty())
+      {
+        ROS_WARN_ONCE(
+          "Diagnostic feature at index %i does not have a field 'FeatureName' or 'Type'.",
+          featureIdx);
+        ROS_WARN_ONCE("Diagnostic feature will be skipped.");
+      }
+      else
+      {
+        // convert type string to lower case
+        std::transform(featureType.begin(), featureType.end(), featureType.begin(),
+          [](unsigned char c){ return std::tolower(c); });
+
+        // if feature name is found in implemented_feature list and if enabled
+        if (implemented_features_.find(featureName) != implemented_features_.end() &&
+            implemented_features_.at(featureName))
+        {
+          diagnostic_msgs::KeyValue kvPair;
+
+          // if 'selectors' which correspond to the featureName are defined
+          if(featureDict["Selectors"].IsDefined() && featureDict["Selectors"].size() > 0)
+          {
+            int selectorIdx = 1;
+            for(auto selectorDict : featureDict["Selectors"])
+            {
+              std::string selectorFeatureName = selectorDict["FeatureName"].IsDefined() 
+                                                ? selectorDict["FeatureName"].as<std::string>()
+                                                : "";
+              std::string selectorType = selectorDict["Type"].IsDefined() 
+                                         ? selectorDict["Type"].as<std::string>()
+                                         : "";
+              std::string selectorValue = selectorDict["Value"].IsDefined() 
+                                          ? selectorDict["Value"].as<std::string>()
+                                          : "";
+
+              if(selectorFeatureName.empty() || selectorType.empty() || selectorValue.empty())
+              {
+                ROS_WARN_ONCE(
+                  "Diagnostic feature selector at index %i of feature at index %i does not have a "
+                  "field 'FeatureName', 'Type' or 'Value'.",
+                  selectorIdx, featureIdx);
+                ROS_WARN_ONCE("Diagnostic feature will be skipped.");
+              }
+              else
+              {
+                if (implemented_features_.find(selectorFeatureName) != implemented_features_.end() &&
+                    implemented_features_.at(selectorFeatureName))
+                {
+                  setFeatureValueFromStrFn(selectorFeatureName, selectorType, selectorValue);
+
+                  kvPair.key = featureName + "-" + selectorValue;
+                  kvPair.value = getFeatureValueAsStrFn(featureName, featureType);
+
+                  camDiagnosticMsg.data.push_back(diagnostic_msgs::KeyValue(kvPair));
+                }
+                else
+                {
+                  ROS_WARN_ONCE("Diagnostic feature selector with name '%s' is not implemented.", 
+                                selectorFeatureName.c_str());
+                }
+
+                selectorIdx++;
+              }
+            }
+          }
+          else
+          {
+            kvPair.key = featureName;
+            kvPair.value = getFeatureValueAsStrFn(featureName, featureType);
+
+            camDiagnosticMsg.data.push_back(diagnostic_msgs::KeyValue(kvPair));
+          }
+        }
+        else
+        {
+          ROS_WARN_ONCE("Diagnostic feature with name '%s' is not implemented.", 
+          featureName.c_str());
+        }
+      }
+
+      featureIdx++;
+    }
+
+    diagnostic_pub_.publish(camDiagnosticMsg);
+
+    loop_rate.sleep();
+  }
+
+}
+
 void CameraAravisNodelet::discoverFeatures()
 {
   implemented_features_.clear();