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
akrobotics · Jul 14, 2016 · 60b5976 · 60b5976
1 parent 337ef11
commit 60b5976
Show file tree

Hide file tree

Showing 7 changed files with 110 additions and 64 deletions.
diff --git a/phidgets_api/include/phidgets_api/phidget.h b/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);

diff --git a/phidgets_api/package.xml b/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>

diff --git a/phidgets_api/src/phidget.cpp b/phidgets_api/src/phidget.cpp
@@ -4,8 +4,6 @@ namespace phidgets {
 
 Phidget::Phidget()
 {
-  updater.add("IMU Driver Status", this, &phidgets::Phidget::phidgetsDiagnostics);
-
 }
 
 Phidget::~Phidget()
@@ -28,7 +26,6 @@ void Phidget::init(CPhidgetHandle handle)
 
 int Phidget::open(int serial_number)
 {
-  updater.setHardwareID("none");
   return CPhidget_open(handle_, serial_number);
 }
 
@@ -97,23 +94,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();
+  printf("Phidget detached (serial# %d)\n", getDeviceSerialNumber());
 }
 
 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 +125,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
diff --git a/phidgets_imu/CMakeLists.txt b/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})

diff --git a/phidgets_imu/include/phidgets_imu/imu_ros_i.h b/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

diff --git a/phidgets_imu/package.xml b/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" />

diff --git a/phidgets_imu/src/imu_ros_i.cpp b/phidgets_imu/src/imu_ros_i.cpp
@@ -1,3 +1,4 @@
+#include <boost/make_shared.hpp>
 #include "phidgets_imu/imu_ros_i.h"
 
 namespace phidgets {
@@ -6,6 +7,9 @@ ImuRosI::ImuRosI(ros::NodeHandle nh, ros::NodeHandle nh_private):
   Imu(),
   nh_(nh), 
   nh_private_(nh_private),
+  is_connected(false),
+  error_number(0),
+  target_publish_freq_(0.0),
   initialized_(false)
 {
   ROS_INFO ("Starting Phidgets IMU");
@@ -45,6 +49,20 @@ ImuRosI::ImuRosI(ros::NodeHandle nh, ros::NodeHandle nh_private):
   cal_publisher_ = nh_.advertise<std_msgs::Bool>(
     "imu/is_calibrated", 5);
 
+  // Set up the topic publisher diagnostics monitor for imu/data_raw
+  // 1. The frequency status component monitors if imu data is published
+  // within 10% tolerance of the desired frequency of 1.0 / period
+  // 2. The timstamp status component monitors the delay between
+  // the header.stamp of the imu message and the real (ros) time
+  // the maximum tolerable drift is +- 100ms
+  target_publish_freq_ = 1000.0 / static_cast<double>(period_);
+  imu_publisher_diag_ptr_ = boost::make_shared<diagnostic_updater::TopicDiagnostic>(
+        "imu/data_raw",
+        boost::ref(diag_updater_),
+        diagnostic_updater::FrequencyStatusParam(&target_publish_freq_, &target_publish_freq_, 0.1, 5),
+        diagnostic_updater::TimeStampStatusParam(-0.1, 0.1)
+        );
+
   // **** advertise services
 
   cal_srv_ = nh_.advertiseService(
@@ -79,6 +97,10 @@ ImuRosI::ImuRosI(ros::NodeHandle nh, ros::NodeHandle nh_private):
   // signal that we have no orientation estimate (see Imu.msg)
   imu_msg_.orientation_covariance[0] = -1;
 
+  // init diagnostics, we set the hardware id properly when the device is connected
+  diag_updater_.setHardwareID("none");
+  diag_updater_.add("IMU Driver Status", this, &phidgets::ImuRosI::phidgetsDiagnostics);
+
   initDevice();
 
   if (has_compass_params)
@@ -108,8 +130,9 @@ void ImuRosI::initDevice()
 	int result = waitForAttachment(10000);
 	if(result)
 	{
-          phidgets::Phidget::is_connected = false;
-          phidgets::Phidget::updater.force_update();
+    is_connected = false;
+    error_number = result;
+    diag_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_api/share/setup-udev.sh script.", err);
@@ -120,6 +143,11 @@ void ImuRosI::initDevice()
 
   // calibrate on startup
   calibrate();
+
+  // set the hardware id for diagnostics
+  diag_updater_.setHardwareIDf("%s-%d",
+                         getDeviceName().c_str(),
+                         getDeviceSerialNumber());
 }
 
 bool ImuRosI::calibrateService(std_srvs::Empty::Request  &req,
@@ -184,6 +212,7 @@ void ImuRosI::processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i
   imu_msg->angular_velocity.z = data[i]->angularRate[2] * (M_PI / 180.0);
 
   imu_publisher_.publish(imu_msg);
+  imu_publisher_diag_ptr_->tick(time_now);
 
   // **** create and publish magnetic field message
 
@@ -209,6 +238,9 @@ void ImuRosI::processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i
   }
 
   mag_publisher_.publish(mag_msg);
+
+  // diagnostics
+  diag_updater_.update();
 }
 
 void ImuRosI::dataHandler(CPhidgetSpatial_SpatialEventDataHandle *data, int count)
@@ -217,5 +249,51 @@ void ImuRosI::dataHandler(CPhidgetSpatial_SpatialEventDataHandle *data, int coun
     processImuData(data, i);
 }
 
+//  Added for diagnostics
+void ImuRosI::attachHandler()
+{
+  Imu::attachHandler();
+  is_connected = true;
+  // Reset error number to no error if the prev error was disconnect
+  if (error_number == 13) error_number = 0;
+  diag_updater_.force_update();
+}
+
+void ImuRosI::detachHandler()
+{
+  Imu::detachHandler();
+  is_connected = false;
+  diag_updater_.force_update();
+}
+
+void ImuRosI::errorHandler(int error)
+{
+  Imu::errorHandler(error);
+  error_number = error;
+  diag_updater_.force_update();
+}
+
+void ImuRosI::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 the USB.");
+  }
+
+  if (error_number != 0)
+  {
+    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The Phidget reports error.");
+    stat.add("Error Number", error_number);
+    stat.add("Error message", getErrorDescription(error_number));
+  }
+}
+
 } // namespace phidgets