From 297b7e340c73460d061fe7a3845e7b98d19c405d Mon Sep 17 00:00:00 2001 From: kkiyenga Date: Sun, 14 Jun 2015 17:43:37 -0400 Subject: [PATCH 1/2] Added first commit for diagnostics for the phidgets. Only imu is complete. --- phidgets_api/include/phidgets_api/phidget.h | 23 ++++++++++++- phidgets_api/package.xml | 5 +++ phidgets_api/src/phidget.cpp | 34 ++++++++++++++++++- phidgets_imu/include/phidgets_imu/imu_ros_i.h | 2 ++ phidgets_imu/src/imu_ros_i.cpp | 2 ++ 5 files changed, 64 insertions(+), 2 deletions(-) diff --git a/phidgets_api/include/phidgets_api/phidget.h b/phidgets_api/include/phidgets_api/phidget.h index 024ee94..61f395d 100644 --- a/phidgets_api/include/phidgets_api/phidget.h +++ b/phidgets_api/include/phidgets_api/phidget.h @@ -33,6 +33,8 @@ #include +#include +#include #include #include #include @@ -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); diff --git a/phidgets_api/package.xml b/phidgets_api/package.xml index d51c657..986d01a 100644 --- a/phidgets_api/package.xml +++ b/phidgets_api/package.xml @@ -17,5 +17,10 @@ Copyright (C) 2010 Phidgets Inc. catkin + diagnostics_updater + diagnostics_msgs + + diagnostics_updater + diagnostics_msgs diff --git a/phidgets_api/src/phidget.cpp b/phidgets_api/src/phidget.cpp index b935676..1ae2acc 100644 --- a/phidgets_api/src/phidget.cpp +++ b/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); } @@ -111,19 +113,49 @@ void Phidget::errorHandler(int error) int Phidget::AttachHandler(CPhidgetHandle handle, void *userptr) { ((Phidget*)userptr)->attachHandler(); + is_connected = true; + updater.force_update(); return 0; } int Phidget::DetachHandler(CPhidgetHandle handle, void *userptr) { ((Phidget*)userptr)->detachHandler(); + is_connected = false; + updater.force_update(); return 0; } int Phidget::ErrorHandler(CPhidgetHandle handle, void *userptr, int ErrorCode, const char *unknown) { ((Phidget*)userptr)->errorHandler(ErrorCode); + is_error = true; + updater.force_update(); + is_error = false; 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/include/phidgets_imu/imu_ros_i.h b/phidgets_imu/include/phidgets_imu/imu_ros_i.h index f9fb354..03028cb 100644 --- a/phidgets_imu/include/phidgets_imu/imu_ros_i.h +++ b/phidgets_imu/include/phidgets_imu/imu_ros_i.h @@ -11,6 +11,8 @@ #include #include +using namespace std; + namespace phidgets { const float G = 9.81; diff --git a/phidgets_imu/src/imu_ros_i.cpp b/phidgets_imu/src/imu_ros_i.cpp index bd09fc7..f4c6743 100644 --- a/phidgets_imu/src/imu_ros_i.cpp +++ b/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); From 8c89007cae9a16eaa6280f7a1d4dcb8cdd95ee66 Mon Sep 17 00:00:00 2001 From: Keshav Iyengar Date: Mon, 16 Nov 2015 11:51:19 -0500 Subject: [PATCH 2/2] Fixes for diagnostics pull request #20. * Added "/" for closing tags in package.xml * Moved lines to non-static function as required. --- phidgets_api/package.xml | 4 ++-- phidgets_api/src/phidget.cpp | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/phidgets_api/package.xml b/phidgets_api/package.xml index 986d01a..95915cb 100644 --- a/phidgets_api/package.xml +++ b/phidgets_api/package.xml @@ -17,8 +17,8 @@ Copyright (C) 2010 Phidgets Inc. catkin - diagnostics_updater - diagnostics_msgs + diagnostics_updater + diagnostics_msgs diagnostics_updater diagnostics_msgs diff --git a/phidgets_api/src/phidget.cpp b/phidgets_api/src/phidget.cpp index 1ae2acc..8e9d756 100644 --- a/phidgets_api/src/phidget.cpp +++ b/phidgets_api/src/phidget.cpp @@ -97,41 +97,41 @@ 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()); } int Phidget::AttachHandler(CPhidgetHandle handle, void *userptr) { ((Phidget*)userptr)->attachHandler(); - is_connected = true; - updater.force_update(); return 0; } int Phidget::DetachHandler(CPhidgetHandle handle, void *userptr) { ((Phidget*)userptr)->detachHandler(); - is_connected = false; - updater.force_update(); return 0; } int Phidget::ErrorHandler(CPhidgetHandle handle, void *userptr, int ErrorCode, const char *unknown) { ((Phidget*)userptr)->errorHandler(ErrorCode); - is_error = true; - updater.force_update(); - is_error = false; return 0; }