Merge pull request #24 from akrobotics/imu-diag

muhrix · web-flow · commit 809f1bded2d0 · 2016-11-20T17:45:26.000Z
Add IMU diagnostics (related to #20)
diff --git a/phidgets_api/include/phidgets_api/phidget.h b/phidgets_api/include/phidgets_api/phidget.h
@@ -49,7 +49,7 @@ class Phidget
     ~Phidget();
 
     /**@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 */
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,13 +4,18 @@
 #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;
+
 namespace phidgets {
 
 const float G = 9.81;
@@ -36,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_;
@@ -70,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,11 +1,15 @@
+#include <boost/make_shared.hpp>
 #include "phidgets_imu/imu_ros_i.h"
 
 namespace phidgets {
 
 ImuRosI::ImuRosI(ros::NodeHandle nh, ros::NodeHandle nh_private):
   Imu(),
-  nh_(nh), 
+  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,13 +49,27 @@ 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(
     "imu/calibrate", &ImuRosI::calibrateService, this);
 
   // **** initialize variables and device
-  
+
   imu_msg_.header.frame_id = frame_id_;
 
   // build covariance matrices
@@ -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,7 +130,10 @@ void ImuRosI::initDevice()
 	int result = waitForAttachment(10000);
 	if(result)
 	{
-	  const char *err;
+		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);
 	}
@@ -118,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,
@@ -144,7 +174,7 @@ void ImuRosI::calibrate()
 void ImuRosI::processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i)
 {
   // **** calculate time from timestamp
-  ros::Duration time_imu(data[i]->timestamp.seconds + 
+  ros::Duration time_imu(data[i]->timestamp.seconds +
                          data[i]->timestamp.microseconds * 1e-6);
 
   ros::Time time_now = time_zero_ + time_imu;
@@ -159,14 +189,14 @@ void ImuRosI::processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i
   // **** initialize if needed
 
   if (!initialized_)
-  { 
+  {
     last_imu_time_ = time_now;
     initialized_ = true;
   }
 
   // **** create and publish imu message
 
-  boost::shared_ptr<ImuMsg> imu_msg = 
+  boost::shared_ptr<ImuMsg> imu_msg =
     boost::make_shared<ImuMsg>(imu_msg_);
 
   imu_msg->header.stamp = time_now;
@@ -182,12 +212,13 @@ 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
 
-  boost::shared_ptr<MagMsg> mag_msg = 
+  boost::shared_ptr<MagMsg> mag_msg =
     boost::make_shared<MagMsg>();
-  
+
   mag_msg->header.frame_id = frame_id_;
   mag_msg->header.stamp = time_now;
 
@@ -205,8 +236,11 @@ void ImuRosI::processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i
     mag_msg->vector.y = nan;
     mag_msg->vector.z = nan;
   }
-   
+
   mag_publisher_.publish(mag_msg);
+
+  // diagnostics
+  diag_updater_.update();
 }
 
 void ImuRosI::dataHandler(CPhidgetSpatial_SpatialEventDataHandle *data, int count)
@@ -215,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
 
