Major refactoring of diagnostics (CCNYRoboticsLab#20)

- Remove all diagnostics logic from phidget_api
- Add diagnostics to phidgets_imu
  - Connect/disconnect/error states are propogated from callbacks
  - Periodic updates for diagnostics through processImuData()
  - Add Frequency and Timestamp (drift) diagnostics for imu/data_raw
  topic

Author: mani-monaj

phidgets_api/include/phidgets_api/phidget.h

@@ -33,8 +33,6 @@
 
 #include <libphidgets/phidget21.h>
 
-#include <diagnostic_updater/diagnostic_updater.h>
-#include <diagnostic_updater/publisher.h>
 #include <iostream>
 #include <algorithm>
 #include <string>
@@ -50,21 +48,6 @@ 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)*/
     int open(int serial_number);
@@ -111,10 +94,6 @@ 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

@@ -14,11 +14,6 @@
   <author>Ivan Dryanovski</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>
 
   <build_depend>libusb-1.0-dev</build_depend>
   <build_depend>libphidgets</build_depend>

phidgets_api/src/phidget.cpp

@@ -4,7 +4,6 @@ namespace phidgets {
 
 Phidget::Phidget()
 {
-  updater.add("IMU Driver Status", this, &phidgets::Phidget::phidgetsDiagnostics);
 
 }
 
@@ -18,7 +17,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)
@@ -28,7 +27,6 @@ void Phidget::init(CPhidgetHandle handle)
 
 int Phidget::open(int serial_number)
 {
-  updater.setHardwareID("none");
   return CPhidget_open(handle_, serial_number);
 }
 
@@ -97,23 +95,16 @@ 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());
 }
 
@@ -135,27 +126,4 @@ 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/CMakeLists.txt

@@ -1,15 +1,15 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(phidgets_imu)
 
-find_package(catkin REQUIRED COMPONENTS geometry_msgs nodelet pluginlib phidgets_api roscpp sensor_msgs std_msgs std_srvs tf)
+find_package(catkin REQUIRED COMPONENTS diagnostic_msgs diagnostic_updater geometry_msgs nodelet pluginlib phidgets_api roscpp sensor_msgs std_msgs std_srvs tf)
 
 find_package(Boost REQUIRED COMPONENTS thread)
 
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES phidgets_imu 
   DEPENDS Boost
-  CATKIN_DEPENDS geometry_msgs nodelet pluginlib phidgets_api roscpp sensor_msgs std_msgs std_srvs tf
+  CATKIN_DEPENDS diagnostic_msgs diagnostic_updater geometry_msgs nodelet pluginlib phidgets_api roscpp sensor_msgs std_msgs std_srvs tf
 )
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})

phidgets_imu/include/phidgets_imu/imu_ros_i.h

@@ -4,11 +4,14 @@
 #include <ros/ros.h>
 #include <ros/service_server.h>
 #include <boost/thread/mutex.hpp>
+#include <boost/shared_ptr.hpp>
 #include <tf/transform_datatypes.h>
 #include <sensor_msgs/Imu.h>
 #include <std_srvs/Empty.h>
 #include <std_msgs/Bool.h>
 #include <geometry_msgs/Vector3Stamped.h>
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
 #include <phidgets_api/imu.h>
 
 using namespace std;
@@ -38,6 +41,16 @@ class ImuRosI : public Imu
     ros::Publisher  cal_publisher_;
     ros::ServiceServer cal_srv_;
 
+    /**@brief updater object of class Update. Used to add diagnostic tasks, set ID etc. refer package API.
+     * Added for diagnostics */
+    diagnostic_updater::Updater diag_updater_;
+    boost::shared_ptr<diagnostic_updater::TopicDiagnostic> imu_publisher_diag_ptr_;
+
+    // diagnostics
+    bool is_connected_;
+    int error_number_;
+    double target_publish_freq_;
+
     bool initialized_;
     boost::mutex mutex_;
     ros::Time last_imu_time_;
@@ -72,7 +85,16 @@ class ImuRosI : public Imu
     void calibrate();
     void initDevice();
     void dataHandler(CPhidgetSpatial_SpatialEventDataHandle* data, int count);
+    void attachHandler();
+    void detachHandler();
+    void errorHandler(int error);
     void processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i);
+
+    /**@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);
 };
 
 } //namespace phidgets

phidgets_imu/package.xml

@@ -23,6 +23,9 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>tf</build_depend>
+  <build_depend>diagnostic_updater</build_depend>
+  <build_depend>diagnostic_msgs</build_depend>
+
   <run_depend>geometry_msgs</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>pluginlib</run_depend>
@@ -32,6 +35,8 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>tf</run_depend>
+  <run_depend>diagnostic_updater</run_depend>
+  <run_depend>diagnostic_msgs</run_depend>
 
   <export>
     <nodelet plugin="${prefix}/phidgets_imu_nodelet.xml" />