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..d489c9b 100644 --- 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 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..a525125 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..4dd5c52 100644 --- a/phidgets_imu/src/imu_ros_i.cpp +++ b/phidgets_imu/src/imu_ros_i.cpp @@ -1,3 +1,4 @@ +#include #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( "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( @@ -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