Merged in unstable/diagnostics (pull request #10)

added diagnostics to check if device is found and calibrated

Approved-by: Uli Klank <uli.klank@gmx.net>

CMakeLists.txt

@@ -9,12 +9,12 @@ set(
      camera_control_msgs
      camera_info_manager
      cv_bridge
+     diagnostic_updater
      image_geometry
      image_transport
      roscpp
      roslaunch
      sensor_msgs
-     #std_srvs
 )
 
 find_package(Pylon QUIET)

include/pylon_camera/pylon_camera_node.h

@@ -53,6 +53,9 @@
 #include <camera_control_msgs/SetSleeping.h>
 #include <camera_control_msgs/GrabImagesAction.h>
 
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
+
 namespace pylon_camera
 {
 
@@ -372,6 +375,13 @@ class PylonCameraNode
 
     bool is_sleeping_;
     boost::recursive_mutex grab_mutex_;
+
+    /// diagnostics:
+    diagnostic_updater::Updater diagnostics_updater_;
+    void diagnostics_timer_callback_(const ros::TimerEvent&);
+    ros::Timer diagnostics_trigger_;
+    void create_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);
+    void create_camera_info_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);
 };
 
 }  // namespace pylon_camera

package.xml

@@ -25,6 +25,7 @@
   <build_depend>camera_control_msgs</build_depend>
   <build_depend>camera_info_manager</build_depend>
   <build_depend>cv_bridge</build_depend>
+  <build_depend>diagnostic_updater</build_depend>
   <build_depend>image_geometry</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>pylon</build_depend>
@@ -37,6 +38,7 @@
   <run_depend>actionlib</run_depend>
   <run_depend>camera_control_msgs</run_depend>
   <run_depend>camera_info_manager</run_depend>
+  <run_depend>diagnostic_updater</run_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_geometry</run_depend>
   <run_depend>image_transport</run_depend>

src/pylon_camera/pylon_camera_node.cpp

@@ -34,6 +34,8 @@
 #include <vector>
 #include "boost/multi_array.hpp"
 
+using diagnostic_msgs::DiagnosticStatus;
+
 namespace pylon_camera
 {
 
@@ -81,9 +83,48 @@ PylonCameraNode::PylonCameraNode()
       brightness_exp_lut_(),
       is_sleeping_(false)
 {
+    diagnostics_updater_.setHardwareID("none");
+    diagnostics_updater_.add("camera_availability", this, &PylonCameraNode::create_diagnostics);
+    diagnostics_updater_.add("intrinsic_calibration", this, &PylonCameraNode::create_camera_info_diagnostics);
+    diagnostics_trigger_ = nh_.createTimer(ros::Duration(2), &PylonCameraNode::diagnostics_timer_callback_, this);
+
     init();
 }
 
+void PylonCameraNode::create_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+    if (pylon_camera_parameter_set_.deviceUserID().empty())
+    {
+        return;
+    }
+
+    if (pylon_camera_)
+    {
+        diagnostics_updater_.setHardwareID( pylon_camera_->deviceUserID());
+        stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Device is connected");
+    }
+    else
+    {
+        diagnostics_updater_.setHardwareID(pylon_camera_parameter_set_.deviceUserID());
+        stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No camera connected");
+    }
+}
+
+void PylonCameraNode::create_camera_info_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+    if (camera_info_manager_->isCalibrated())
+    {
+        stat.summaryf(DiagnosticStatus::OK, "Intrinsic calibration found");
+    }else{
+        stat.summaryf(DiagnosticStatus::ERROR, "No intrinsic calibration found");
+    }
+}
+
+void PylonCameraNode::diagnostics_timer_callback_(const ros::TimerEvent&)
+{
+    diagnostics_updater_.update();
+}
+
 void PylonCameraNode::init()
 {
     // reading all necessary parameter to open the desired camera from the