[ros-users] [PATCH][camera1394] Adding diagnostics support t…

Top Page
Attachments:
Message as email
+ (text/plain)
+ camera1394_diagnostics_support.patch (text/x-patch)
Delete this message
Reply to this message
Author: User discussions
Date:  
To: User discussions
Subject: [ros-users] [PATCH][camera1394] Adding diagnostics support to camera1394
Hello there,
Here is a very naive implementation of diagnostics support for camera1394.

Rationale: Firewire cameras are often unstable on Linux. It is really bothersome
to start a demo, launching a bunch of nodes and realizing that the grabbing just
don't work and you have to reset the bus or worse to recover.

It is a simple use of diagnostic_updater::TopicDiagnostic to make sure
that if image_raw publishing rate drops
before a certain rate, it will trigger an error in the diagnostics
topics. On the same idea, it could as well trigger an
error when the initialization fails.

The implementation is untested but it compiles. It would like first to
have feedbacks regarding the patch
before going further.

Thanks,

Attached: path proposal, based on current trunk (r38171).
--
Thomas Moulard
Index: src/nodes/driver1394.cpp
===================================================================
--- src/nodes/driver1394.cpp    (revision 38745)
+++ src/nodes/driver1394.cpp    (working copy)
@@ -81,7 +81,16 @@
     cinfo_(new camera_info_manager::CameraInfoManager(camera_nh_)),
     calibration_matches_(true),
     it_(new image_transport::ImageTransport(camera_nh_)),
-    image_pub_(it_->advertiseCamera("image_raw", 1))
+    image_pub_(it_->advertiseCamera("image_raw", 1)),
+    diagnostics_(),
+    topic_diagnostics_min_freq_(0.),
+    topic_diagnostics_max_freq_(1000.),
+    topic_diagnostics_("image_raw", diagnostics_,
+               diagnostic_updater::FrequencyStatusParam
+               (&topic_diagnostics_min_freq_,
+            &topic_diagnostics_max_freq_, 0.1, 10),
+               diagnostic_updater::TimeStampStatusParam
+               ())
   {}


   Camera1394Driver::~Camera1394Driver()
@@ -194,6 +203,12 @@
       }
   }


+  /** Update diagnostics topics */
+  void Camera1394Driver::updateDiagnostics()
+  {
+    diagnostics_.update();
+  }
+
   /** Publish camera stream topics
    *
    *  @param image points to latest camera frame
@@ -201,7 +216,7 @@
   void Camera1394Driver::publish(const sensor_msgs::ImagePtr &image)
   {
     image->header.frame_id = config_.frame_id;
-      
+
     // get current CameraInfo data
     sensor_msgs::CameraInfoPtr
       ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
@@ -239,7 +254,7 @@


     // @todo log a warning if (filtered) time since last published
     // image is not reasonably close to configured frame_rate
-      
+
     // Publish via image_transport
     image_pub_.publish(image, ci);
   }
@@ -344,6 +359,21 @@
           }
       }


+    // Update diagnostics.
+    diagnostics_.setHardwareID(camera_name_);
+    if (state_ == Driver::CLOSED)
+      {
+    topic_diagnostics_min_freq_ = 0.;
+    topic_diagnostics_max_freq_ = 0.;
+      }
+    else
+      {
+    // Allow a 10% error margin.
+    double delta = newconfig.frame_rate * 0.1;
+    topic_diagnostics_min_freq_ = newconfig.frame_rate - delta;
+    topic_diagnostics_max_freq_ = newconfig.frame_rate + delta;
+      }
+
     config_ = newconfig;                // save new parameters
     reconfiguring_ = false;             // let poll() run again


Index: src/nodes/driver1394.h
===================================================================
--- src/nodes/driver1394.h    (revision 38745)
+++ src/nodes/driver1394.h    (working copy)
@@ -39,6 +39,8 @@


 #include <ros/ros.h>
 #include <camera_info_manager/camera_info_manager.h>
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
 #include <driver_base/driver.h>
 #include <dynamic_reconfigure/server.h>
 #include <image_transport/image_transport.h>
@@ -66,6 +68,7 @@
                    ros::NodeHandle camera_nh);
   ~Camera1394Driver();
   void poll(void);
+  void updateDiagnostics(void);
   void setup(void);
   void shutdown(void);


@@ -104,6 +107,11 @@
boost::shared_ptr<image_transport::ImageTransport> it_;
image_transport::CameraPublisher image_pub_;

+ /** diagnostics updater */
+ diagnostic_updater::Updater diagnostics_;
+ double topic_diagnostics_min_freq_;
+ double topic_diagnostics_max_freq_;
+ diagnostic_updater::TopicDiagnostic topic_diagnostics_;
}; // end class Camera1394Driver

 }; // end namespace camera1394_driver
Index: src/nodes/nodelet.cpp
===================================================================
--- src/nodes/nodelet.cpp    (revision 38745)
+++ src/nodes/nodelet.cpp    (working copy)
@@ -65,6 +65,7 @@
         NODELET_INFO("shutting down driver thread");
         running_ = false;
         deviceThread_->join();
+        diagnosticsThread_->join();
         NODELET_INFO("driver thread stopped");
       }
     dvr_->shutdown();
@@ -73,10 +74,12 @@
 private:
   virtual void onInit();
   virtual void devicePoll();
+  virtual void updateDiagnostics();


   volatile bool running_;               ///< device is running
   boost::shared_ptr<camera1394_driver::Camera1394Driver> dvr_;
   boost::shared_ptr<boost::thread> deviceThread_;
+  boost::shared_ptr<boost::thread> diagnosticsThread_;
 };


 /** Nodelet initialization.
@@ -95,6 +98,11 @@
   running_ = true;
   deviceThread_ = boost::shared_ptr< boost::thread >
     (new boost::thread(boost::bind(&Camera1394Nodelet::devicePoll, this)));
+
+  // spawn diagnostics thread
+  diagnosticsThread_ = boost::shared_ptr< boost::thread >
+    (new boost::thread
+     (boost::bind(&Camera1394Nodelet::updateDiagnostics, this)));
 }


 /** Nodelet device poll thread main function. */
@@ -106,6 +114,15 @@
     }
 }


+/** Separate diagnostic thread main function. */
+void Camera1394Nodelet::updateDiagnostics()
+{
+  while (running_)
+    {
+      dvr_->updateDiagnostics();
+    }
+}
+
 // Register this plugin with pluginlib.  Names must match nodelet_velodyne.xml.
 //
 // parameters are: package, class name, class type, base class type