Topic Diagnostics

.gitignore

@@ -0,0 +1 @@
+build

README.md

@@ -23,10 +23,10 @@ Install dependencies via `rosdep`.
 ### 1.0.x: Experimental
 
 #### Dependencies:
- 
-* gstreamer1.0-tools 
-* libgstreamer1.0-dev 
-* libgstreamer-plugins-base1.0-dev 
+
+* gstreamer1.0-tools
+* libgstreamer1.0-dev
+* libgstreamer-plugins-base1.0-dev
 * libgstreamer-plugins-good1.0-dev
 
 Ubuntu Install:
@@ -76,11 +76,16 @@ This can be run as both a node and a nodelet.
 * `~frame_id`: The [TF](http://www.ros.org/wiki/tf) frame ID.
 * `~reopen_on_eof`: Re-open the stream if it ends (EOF).
 * `~sync_sink`: Synchronize the app sink (sometimes setting this to `false` can resolve problems with sub-par framerates).
+* `~diagnostic_fps`: The expected frame rate of the camera used for diagnostic purposes. If the value is 0, diagnostics are turned off. (default: 0)
+* `~diagnostic_window`: Number of events to consider in the statistics. See [FrequencyStatusParam](http://docs.ros.org/api/diagnostic_updater/html/structdiagnostic__updater_1_1FrequencyStatusParam.html).
+* `~fps_tolerance`: The percent tolerance (+/- as a fraction of 1.0) allowed from the diagnostic_fps value. See [FrequencyStatusParam](http://docs.ros.org/api/diagnostic_updater/html/structdiagnostic__updater_1_1FrequencyStatusParam.html). (Default: 0.1)
+* `~min_delay`: the minimum accepted seconds between two frames. See [TimeStampStatusParam](http://docs.ros.org/api/diagnostic_updater/html/structdiagnostic__updater_1_1TimeStampStatusParam.html) (default: 0)
+* `~max_delay`: the maximum accepted seconds between two frames. See [TimeStampStatusParam](http://docs.ros.org/api/diagnostic_updater/html/structdiagnostic__updater_1_1TimeStampStatusParam.html) (default: (1.0 / diagnostic_fps) * fps_tolerance)
 
 C++ API (unstable)
 ------------------
 
-The gscam c++ library can be used, but it is not guaranteed to be stable. 
+The gscam c++ library can be used, but it is not guaranteed to be stable.
 
 Examples
 --------

include/gscam/gscam.h

@@ -15,7 +15,11 @@ extern "C"{
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/SetCameraInfo.h>
 
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
+
 #include <stdexcept>
+#include <string>
 
 namespace gscam {
 
@@ -28,6 +32,7 @@ namespace gscam {
     bool init_stream();
     void publish_stream();
     void cleanup_stream();
+    void diagnostic_update(const ros::TimerEvent&);
 
     void run();
 
@@ -48,6 +53,9 @@ namespace gscam {
     // Camera publisher configuration
     std::string frame_id_;
     int width_, height_;
+    double expected_fps_, fps_tolerance_;
+    double min_delay_, max_delay_;
+    int diagnostic_window_;
     std::string image_encoding_;
     std::string camera_name_;
     std::string camera_info_url_;
@@ -62,6 +70,10 @@ namespace gscam {
     // Case of a jpeg only publisher
     ros::Publisher jpeg_pub_;
     ros::Publisher cinfo_pub_;
+
+    diagnostic_updater::Updater updater_;
+    diagnostic_updater::TopicDiagnostic* freq_diagnostic_;
+    ros::Timer diagnostic_update_timer_;
   };
 
 }

src/gscam.cpp

@@ -42,6 +42,7 @@ namespace gscam {
 
   GSCam::~GSCam()
   {
+    delete freq_diagnostic_;
   }
 
   bool GSCam::configure()
@@ -83,7 +84,7 @@ namespace gscam {
     // Get the image encoding
     nh_private_.param("image_encoding", image_encoding_, sensor_msgs::image_encodings::RGB8);
     if (image_encoding_ != sensor_msgs::image_encodings::RGB8 &&
-        image_encoding_ != sensor_msgs::image_encodings::MONO8 && 
+        image_encoding_ != sensor_msgs::image_encodings::MONO8 &&
         image_encoding_ != "jpeg") {
       ROS_FATAL_STREAM("Unsupported image encoding: " + image_encoding_);
     }
@@ -104,6 +105,25 @@ namespace gscam {
       nh_private_.setParam("frame_id",frame_id_);
     }
 
+    // Get diagnostic params. If expected_fps is > 0, diagnostics will be enabled
+    nh_private_.param("diagnostic_fps", expected_fps_, 0.0);
+    nh_private_.param("fps_tolerance", fps_tolerance_, 0.1);
+    nh_private_.param("min_delay", min_delay_, 0.0);
+    nh_private_.param("max_delay", max_delay_, (1.0/expected_fps_)*fps_tolerance_);
+    nh_private_.param("diagnostic_window", diagnostic_window_, 10);
+
+    if (expected_fps_> 0) {
+      ROS_INFO_STREAM("Setting up diagnostics at " << expected_fps_ << " fps.");
+      updater_.setHardwareID(frame_id_);
+      freq_diagnostic_ = new diagnostic_updater::TopicDiagnostic("camera", updater_,
+        diagnostic_updater::FrequencyStatusParam(
+          &expected_fps_, &expected_fps_, fps_tolerance_, diagnostic_window_),
+        diagnostic_updater::TimeStampStatusParam(0, max_delay_));
+
+      diagnostic_update_timer_ = nh_.createTimer(ros::Duration(1.0),
+        &GSCam::diagnostic_update, this);
+    }
+
     return true;
   }
 
@@ -132,19 +152,19 @@ namespace gscam {
 #if (GST_VERSION_MAJOR == 1)
     // http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html
     if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
-        caps = gst_caps_new_simple( "video/x-raw", 
+        caps = gst_caps_new_simple( "video/x-raw",
             "format", G_TYPE_STRING, "RGB",
-            NULL); 
+            NULL);
     } else if (image_encoding_ == sensor_msgs::image_encodings::MONO8) {
-        caps = gst_caps_new_simple( "video/x-raw", 
+        caps = gst_caps_new_simple( "video/x-raw",
             "format", G_TYPE_STRING, "GRAY8",
-            NULL); 
+            NULL);
     } else if (image_encoding_ == "jpeg") {
         caps = gst_caps_new_simple("image/jpeg", NULL, NULL);
     }
 #else
     if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
-        caps = gst_caps_new_simple( "video/x-raw-rgb", NULL,NULL); 
+        caps = gst_caps_new_simple( "video/x-raw-rgb", NULL,NULL);
     } else if (image_encoding_ == sensor_msgs::image_encodings::MONO8) {
         caps = gst_caps_new_simple("video/x-raw-gray", NULL, NULL);
     } else if (image_encoding_ == "jpeg") {
@@ -263,7 +283,7 @@ namespace gscam {
     ROS_INFO("Started stream.");
 
     // Poll the data as fast a spossible
-    while(ros::ok()) 
+    while(ros::ok())
     {
       // This should block until a new frame is awake, this way, we'll run at the
       // actual capture framerate of the device.
@@ -395,10 +415,19 @@ namespace gscam {
         gst_buffer_unref(buf);
       }
 
+      if (expected_fps_ > 0) {
+        freq_diagnostic_->tick(cinfo->header.stamp);
+        updater_.update();
+      }
+
       ros::spinOnce();
     }
   }
 
+  void GSCam::diagnostic_update(const ros::TimerEvent&) {
+    updater_.update();
+  }
+
   void GSCam::cleanup_stream()
   {
     // Clean up
@@ -436,7 +465,7 @@ namespace gscam {
         break;
       }
     }
-
+    ros::shutdown();
   }
 
   // Example callbacks for appsink

src/gscam_node.cpp

@@ -6,9 +6,15 @@ int main(int argc, char** argv) {
   ros::init(argc, argv, "gscam_publisher");
   ros::NodeHandle nh, nh_private("~");
 
+  ROS_INFO("Testing node!!!!");
+
   gscam::GSCam gscam_driver(nh, nh_private);
-  gscam_driver.run();
+  // gscam_driver.run();
+  boost::thread gscam_thread = boost::thread(boost::bind(&gscam::GSCam::run, &gscam_driver));
+
+  ros::spin();
+
+  gscam_thread.join();
 
   return 0;
 }
-