Added first commit for diagnostics for the phidgets.

phidgets_api/include/phidgets_api/phidget.h

@@ -33,6 +33,8 @@
 
 #include <phidgets_api/phidget21.h>
 
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
 #include <iostream>
 #include <algorithm>
 #include <string>
@@ -48,8 +50,23 @@ class Phidget
     Phidget();
     ~Phidget();
 
+    /**@brief updater object of class Update. Used to add diagnostic tasks, set ID etc. refer package API.
+     * Added for diagnostics */
+    diagnostic_updater::Updater updater;
+
+    /**@brief This bool is public to allow to know when 1000s condition in imu_ros_i.cpp is over and need
+     * to report a connection error.
+     * Added for diagnostics */
+    bool is_connected;
+
+    /**@brief Main diagnostic method that takes care of collecting diagnostic data.
+     * @param stat The stat param is what is the diagnostic tasks are added two. Internally published by the
+     * 		    diagnostic_updater package.
+     * Added for diagnostics */
+    void phidgetsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);
+
     /**@brief Open a connection to a Phidget
-     * @param serial_number THe serial number of the phidget to which to attach (-1 will connect to any)*/
+     * @param serial_number The serial number of the phidget to which to attach (-1 will connect to any)*/
     int open(int serial_number);
 
     /**@brief Close the connection to the phidget */
@@ -94,6 +111,10 @@ class Phidget
 
   private:
 
+    // Added for diagnostics
+    bool is_error;
+    int error_number;
+
     static int AttachHandler(CPhidgetHandle handle, void *userptr);
     static int DetachHandler(CPhidgetHandle handle, void *userptr);
     static int ErrorHandler (CPhidgetHandle handle, void *userptr, int ErrorCode, const char *unknown);

phidgets_api/package.xml

@@ -17,5 +17,10 @@
   <author email="patrick@phidgets.com">Copyright (C) 2010 Phidgets Inc. </author>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>diagnostics_updater</build_depend>
+  <build_depend>diagnostics_msgs</build_depend>
+
+  <run_depend>diagnostics_updater</run_depend>
+  <run_depend>diagnostics_msgs</run_depend>
 
 </package>

phidgets_api/src/phidget.cpp

@@ -4,6 +4,7 @@ namespace phidgets {
 
 Phidget::Phidget()
 {
+  updater.add("IMU Driver Status", this, &phidgets::Phidget::phidgetsDiagnostics);
 
 }
 
@@ -17,7 +18,7 @@ void Phidget::registerHandlers()
 {
   CPhidget_set_OnAttach_Handler(handle_, &Phidget::AttachHandler, this); 
   CPhidget_set_OnDetach_Handler(handle_, &Phidget::DetachHandler, this); 
-	CPhidget_set_OnError_Handler (handle_, &Phidget::ErrorHandler,  this);
+  CPhidget_set_OnError_Handler (handle_, &Phidget::ErrorHandler,  this);
 }
 
 void Phidget::init(CPhidgetHandle handle)
@@ -27,6 +28,7 @@ void Phidget::init(CPhidgetHandle handle)
 
 int Phidget::open(int serial_number)
 {
+  updater.setHardwareID("none");
   return CPhidget_open(handle_, serial_number);
 }
 
@@ -95,16 +97,23 @@ std::string Phidget::getErrorDescription(int errorCode)
 
 void Phidget::attachHandler()
 {
+  	is_connected = true;
+  	updater.force_update();
 	printf("Phidget attached (serial# %d)\n", getDeviceSerialNumber());
 }
 
 void Phidget::detachHandler()
 {
 	printf("Phidget detached (serial# %d)\n", getDeviceSerialNumber());
+        is_connected = false;
+        updater.force_update();
 }
 
 void Phidget::errorHandler(int error)
 {
+        is_error = true;
+        updater.force_update();
+        is_error = false;
 	printf("Phidget error [%d]: %s\n", error, getErrorDescription(error).c_str());
 }
 
@@ -126,4 +135,27 @@ int Phidget::ErrorHandler(CPhidgetHandle handle, void *userptr, int ErrorCode, c
   return 0;
 }
 
+//  Added for diagnostics
+void Phidget::phidgetsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+  if (is_connected)
+  {
+    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "The Phidget is connected.");
+    stat.add("Device Serial Number", getDeviceSerialNumber());
+    stat.add("Device Name", getDeviceName());
+    stat.add("Device Type", getDeviceType());
+  }
+  else
+  {
+    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The Phidget is not connected. Check USB.");
+  }
+
+  if (is_error && error_number != 0)
+  {
+    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The Phidget is in Error.");
+    stat.addf("Error Number","%f",error_number);
+    stat.add("Error message",getErrorDescription(error_number));
+  }
+}
+
 } //namespace phidgets

phidgets_imu/include/phidgets_imu/imu_ros_i.h

@@ -11,6 +11,8 @@
 #include <geometry_msgs/Vector3Stamped.h>
 #include <phidgets_api/imu.h>
 
+using namespace std;
+
 namespace phidgets {
 
 const float G = 9.81;

phidgets_imu/src/imu_ros_i.cpp

@@ -73,6 +73,8 @@ void ImuRosI::initDevice()
 	int result = waitForAttachment(10000);
 	if(result)
 	{
+          phidgets::Phidget::is_connected = false;
+          phidgets::Phidget::updater.force_update();
 	  const char *err;
 		CPhidget_getErrorDescription(result, &err);
 		ROS_FATAL("Problem waiting for IMU attachment: %s Make sure the USB cable is connected and you have executed the phidgets_c_api/setup-udev.sh script.", err);