From 8ab34d5d444fcd8b42bf06b715608f0f8a73cf10 Mon Sep 17 00:00:00 2001 From: kkiyenga Date: Sun, 14 Jun 2015 17:43:37 -0400 Subject: [PATCH 1/3] 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 f358d53..f1fade7 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 107eb7a..151be0c 100644 --- a/phidgets_api/package.xml +++ b/phidgets_api/package.xml @@ -14,6 +14,11 @@ Ivan Dryanovski catkin + diagnostics_updater + diagnostics_msgs + + diagnostics_updater + diagnostics_msgs libusb-1.0-dev libphidgets 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 62c8959..d6872c0 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 16d205e..6f2f61a 100644 --- a/phidgets_imu/src/imu_ros_i.cpp +++ b/phidgets_imu/src/imu_ros_i.cpp @@ -108,6 +108,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_api/share/setup-udev.sh script.", err); From 337ef119c19469e4b1e71143502a7e20baa7787b Mon Sep 17 00:00:00 2001 From: Keshav Iyengar Date: Mon, 16 Nov 2015 11:51:19 -0500 Subject: [PATCH 2/3] 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 151be0c..ca07fd0 100644 --- a/phidgets_api/package.xml +++ b/phidgets_api/package.xml @@ -14,8 +14,8 @@ Ivan Dryanovski 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; } From ce5d4195b5956f9d196b8ddbe42080287c59a770 Mon Sep 17 00:00:00 2001 From: Mani Monajjemi Date: Thu, 14 Jul 2016 14:31:40 -0700 Subject: [PATCH 3/3] Major refactoring of diagnostics (#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 --- phidgets_api/include/phidgets_api/phidget.h | 21 ---- phidgets_api/package.xml | 5 - phidgets_api/src/phidget.cpp | 34 +----- phidgets_imu/CMakeLists.txt | 4 +- phidgets_imu/include/phidgets_imu/imu_ros_i.h | 22 ++++ phidgets_imu/package.xml | 5 + phidgets_imu/src/imu_ros_i.cpp | 100 ++++++++++++++++-- 7 files changed, 119 insertions(+), 72 deletions(-) diff --git a/phidgets_api/include/phidgets_api/phidget.h b/phidgets_api/include/phidgets_api/phidget.h index f1fade7..be04b48 100644 --- a/phidgets_api/include/phidgets_api/phidget.h +++ b/phidgets_api/include/phidgets_api/phidget.h @@ -33,8 +33,6 @@ #include -#include -#include #include #include #include @@ -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 index ca07fd0..107eb7a 100644 --- a/phidgets_api/package.xml +++ b/phidgets_api/package.xml @@ -14,11 +14,6 @@ Ivan Dryanovski catkin - diagnostics_updater - diagnostics_msgs - - diagnostics_updater - diagnostics_msgs libusb-1.0-dev libphidgets diff --git a/phidgets_api/src/phidget.cpp b/phidgets_api/src/phidget.cpp index 8e9d756..b935676 100644 --- a/phidgets_api/src/phidget.cpp +++ b/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 diff --git a/phidgets_imu/CMakeLists.txt b/phidgets_imu/CMakeLists.txt index 7eb9332..4434350 100644 --- a/phidgets_imu/CMakeLists.txt +++ b/phidgets_imu/CMakeLists.txt @@ -1,7 +1,7 @@ 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) @@ -9,7 +9,7 @@ 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 index d6872c0..1b806fb 100644 --- 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 #include #include +#include #include #include #include #include #include +#include +#include #include 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 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 index 5c5d298..86f81e0 100644 --- a/phidgets_imu/package.xml +++ b/phidgets_imu/package.xml @@ -23,6 +23,9 @@ std_msgs std_srvs tf + diagnostic_updater + diagnostic_msgs + geometry_msgs nodelet pluginlib @@ -32,6 +35,8 @@ std_msgs std_srvs tf + diagnostic_updater + diagnostic_msgs diff --git a/phidgets_imu/src/imu_ros_i.cpp b/phidgets_imu/src/imu_ros_i.cpp index 6f2f61a..0c5a5a2 100644 --- a/phidgets_imu/src/imu_ros_i.cpp +++ b/phidgets_imu/src/imu_ros_i.cpp @@ -1,11 +1,15 @@ +#include #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( "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(period_); + imu_publisher_diag_ptr_ = boost::make_shared( + "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,9 +130,10 @@ void ImuRosI::initDevice() int result = waitForAttachment(10000); if(result) { - phidgets::Phidget::is_connected = false; - phidgets::Phidget::updater.force_update(); - 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); } @@ -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, @@ -146,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; @@ -161,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 imu_msg = + boost::shared_ptr imu_msg = boost::make_shared(imu_msg_); imu_msg->header.stamp = time_now; @@ -184,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 mag_msg = + boost::shared_ptr mag_msg = boost::make_shared(); - + mag_msg->header.frame_id = frame_id_; mag_msg->header.stamp = time_now; @@ -207,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) @@ -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