Add diagnostics back (#1)

* Add error handler to Spatial class

* Add diagnostics publishing to phidgets_spatial

* Separate diagnostics

* Fix HW_ID warning

phidgets_api/include/phidgets_api/spatial.hpp

@@ -50,7 +50,9 @@ class Spatial final
             data_handler,
         std::function<void(const double[4], double)> algorithm_data_handler,
         std::function<void()> attach_handler = nullptr,
-        std::function<void()> detach_handler = nullptr);
+        std::function<void()> detach_handler = nullptr,
+        std::function<void(Phidget_ErrorEventCode, const char *)> error_handler =
+            nullptr);
 
     ~Spatial();
 
@@ -87,6 +89,8 @@ class Spatial final
 
     virtual void attachHandler();
     virtual void detachHandler();
+    virtual void errorHandler(Phidget_ErrorEventCode error_code,
+                              const char *error_string);
 
   private:
     int32_t serial_number_;
@@ -99,6 +103,7 @@ class Spatial final
 
     std::function<void()> attach_handler_;
     std::function<void()> detach_handler_;
+    std::function<void(Phidget_ErrorEventCode, const char *)> error_handler_;
 
     PhidgetSpatialHandle spatial_handle_{nullptr};
 
@@ -111,6 +116,9 @@ class Spatial final
                                      double timestamp);
     static void AttachHandler(PhidgetHandle input_handle, void *ctx);
     static void DetachHandler(PhidgetHandle input_handle, void *ctx);
+    static void ErrorHandler(PhidgetHandle input_handle, void *ctx,
+                            Phidget_ErrorEventCode error_code,
+                            const char *error_string);
 };
 
 }  // namespace phidgets

phidgets_api/src/spatial.cpp

@@ -43,12 +43,14 @@ Spatial::Spatial(
                        double)>
         data_handler,
     std::function<void(const double[4], double)> algorithm_data_handler,
-    std::function<void()> attach_handler, std::function<void()> detach_handler)
+    std::function<void()> attach_handler, std::function<void()> detach_handler,
+    std::function<void(Phidget_ErrorEventCode, const char *)> error_handler)
     : serial_number_(serial_number),
       data_handler_(std::move(data_handler)),
       algorithm_data_handler_(std::move(algorithm_data_handler)),
       attach_handler_(std::move(attach_handler)),
-      detach_handler_(std::move(detach_handler))
+      detach_handler_(std::move(detach_handler)),
+        error_handler_(std::move(error_handler))
 {
     PhidgetReturnCode ret = PhidgetSpatial_create(&spatial_handle_);
     if (ret != EPHIDGET_OK)
@@ -102,6 +104,17 @@ Spatial::Spatial(
         }
     }
 
+    if (error_handler_ != nullptr)
+    {
+        ret = Phidget_setOnErrorHandler(
+            reinterpret_cast<PhidgetHandle>(spatial_handle_), ErrorHandler,
+            this);
+        if (ret != EPHIDGET_OK)
+        {
+            throw Phidget22Error("Failed to set error handler for Spatial", ret);
+        }
+    }
+
     if (serial_number_ == -1)
     {
         ret = Phidget_getDeviceSerialNumber(
@@ -245,6 +258,12 @@ void Spatial::detachHandler()
     detach_handler_();
 }
 
+void Spatial::errorHandler(Phidget_ErrorEventCode error_code,
+                              const char *error_string)
+{
+    error_handler_(error_code, error_string);
+}
+
 void Spatial::DataHandler(PhidgetSpatialHandle /* input_handle */, void *ctx,
                           const double acceleration[3],
                           const double angular_rate[3],
@@ -271,4 +290,10 @@ void Spatial::DetachHandler(PhidgetHandle /* input_handle */, void *ctx)
     ((Spatial *)ctx)->detachHandler();
 }
 
+void Spatial::ErrorHandler(PhidgetHandle /* input_handle */, void *ctx, Phidget_ErrorEventCode error_code,
+                           const char * error_string)
+{
+    ((Spatial *)ctx)->errorHandler(error_code, error_string);
+
+}
 }  // namespace phidgets

phidgets_spatial/CMakeLists.txt

@@ -17,6 +17,8 @@ find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(diagnostic_updater REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
 
 include_directories(include)
 
@@ -28,6 +30,8 @@ ament_target_dependencies(phidgets_spatial
   sensor_msgs
   std_msgs
   std_srvs
+  diagnostic_updater
+  diagnostic_msgs
 )
 
 rclcpp_components_register_nodes(phidgets_spatial

phidgets_spatial/include/phidgets_spatial/spatial_ros_i.hpp

@@ -39,6 +39,9 @@
 #include <sensor_msgs/msg/magnetic_field.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_srvs/srv/empty.hpp>
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "diagnostic_updater/publisher.hpp"
+#include "diagnostic_msgs/msg/diagnostic_status.hpp"
 
 #include "phidgets_api/spatial.hpp"
 
@@ -61,6 +64,15 @@ class SpatialRosI final : public rclcpp::Node
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
     rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr
         magnetic_field_pub_;
+
+    /**@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_{this};
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> imu_pub_diag_ptr_;
+    bool is_connected_ = false;
+    int error_number_ = 0;
+    const char *error_msg_;
+
     void timerCallback();
     rclcpp::TimerBase::SharedPtr timer_;
     double publish_rate_;
@@ -117,6 +129,19 @@ class SpatialRosI final : public rclcpp::Node
                                       double timestamp);
     void attachCallback();
     void detachCallback();
+    void errorCallback(Phidget_ErrorEventCode error_code, const char *error_msg);
+
+     /**@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 Method to check the connection status of the device.
+     * @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 checkConnection(diagnostic_updater::DiagnosticStatusWrapper &stat);
 };
 
 }  // namespace phidgets

phidgets_spatial/package.xml

@@ -23,6 +23,8 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>diagnostic_msgs</depend>
 
   <exec_depend>launch</exec_depend>
 

phidgets_spatial/src/spatial_ros_i.cpp

@@ -251,26 +251,33 @@ SpatialRosI::SpatialRosI(const rclcpp::NodeOptions &options)
                 std::bind(&SpatialRosI::spatialAlgorithmDataCallback, this,
                           std::placeholders::_1, std::placeholders::_2);
         }
-
+        diag_updater_.setHardwareID("none");
         spatial_ = std::make_unique<Spatial>(
             serial_num, hub_port, false,
             std::bind(&SpatialRosI::spatialDataCallback, this,
                       std::placeholders::_1, std::placeholders::_2,
                       std::placeholders::_3, std::placeholders::_4),
             algorithm_data_handler,
             std::bind(&SpatialRosI::attachCallback, this),
-            std::bind(&SpatialRosI::detachCallback, this));
+            std::bind(&SpatialRosI::detachCallback, this),
+            std::bind(&SpatialRosI::errorCallback, this, std::placeholders::_1,
+                      std::placeholders::_2));
 
         RCLCPP_INFO(get_logger(), "Connected to serial %d",
                     spatial_->getSerialNumber());
 
         spatial_->setDataInterval(data_interval_ms);
 
+        is_connected_ = true;
+
         cal_publisher_ = this->create_publisher<std_msgs::msg::Bool>(
             "imu/is_calibrated", rclcpp::SystemDefaultsQoS().transient_local());
 
         calibrate();
 
+        // set the hardware id for diagnostics
+        diag_updater_.setHardwareIDf("phidget_spatial-%d", spatial_->getSerialNumber());
+
         if (use_orientation)
         {
             spatial_->setSpatialAlgorithm(spatial_algorithm);
@@ -319,13 +326,29 @@ SpatialRosI::SpatialRosI(const rclcpp::NodeOptions &options)
     magnetic_field_pub_ =
         this->create_publisher<sensor_msgs::msg::MagneticField>("imu/mag", 1);
 
+    // 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
+    imu_pub_diag_ptr_ = std::make_shared<diagnostic_updater::TopicDiagnostic>(
+            "imu/data_raw",
+            std::ref(diag_updater_),
+            diagnostic_updater::FrequencyStatusParam(&publish_rate_, &publish_rate_, 0.1, 5),
+            diagnostic_updater::TimeStampStatusParam(-0.1, 0.1)
+            );
+
     if (publish_rate_ > 0.0)
     {
         double pub_msec = 1000.0 / publish_rate_;
         timer_ = this->create_wall_timer(
             std::chrono::milliseconds(static_cast<int64_t>(pub_msec)),
             std::bind(&SpatialRosI::timerCallback, this));
     }
+
+    diag_updater_.add("IMU Driver Status", this, &SpatialRosI::phidgetsDiagnostics);
+    diag_updater_.add("Connection Status", this, &SpatialRosI::checkConnection);
 }
 
 void SpatialRosI::calibrate()
@@ -419,6 +442,7 @@ void SpatialRosI::publishLatest()
     msg->orientation.z = last_quat_z_;
 
     imu_pub_->publish(std::move(msg));
+    imu_pub_diag_ptr_->tick(ros_time);
 
     // Fill out and publish magnetic message
     mag_msg->header.frame_id = frame_id_;
@@ -596,6 +620,11 @@ void SpatialRosI::attachCallback()
 {
     RCLCPP_INFO(get_logger(), "Phidget Spatial attached.");
 
+    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();
+
     // Set data interval. This is in attachCallback() because it has to be
     // repeated on reattachment.
     spatial_->setDataInterval(data_interval_ns_ / 1000 / 1000);
@@ -609,9 +638,52 @@ void SpatialRosI::attachCallback()
 
 void SpatialRosI::detachCallback()
 {
+    is_connected_ = false;
+    diag_updater_.force_update();
     RCLCPP_INFO(get_logger(), "Phidget Spatial detached.");
 }
 
+void SpatialRosI::errorCallback(Phidget_ErrorEventCode error_code, const char *error_msg)
+{
+    error_number_ = error_code;
+    error_msg_ = error_msg;
+
+    diag_updater_.force_update();
+    RCLCPP_ERROR(get_logger(), "Phidget Spatial: %s (%d)", error_msg, error_code);
+}
+
+void SpatialRosI::phidgetsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+    if (error_number_ == 4)
+    {
+        stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "The Phidget reports a warning.");
+        stat.add("Error Number", error_number_);
+        stat.add("Error message", "An error occurred when trying to dispatch an event.");
+    } else if (error_number_ != 0)
+    {
+        stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "The Phidget reports error.");
+        stat.add("Error Number", error_number_);
+        stat.add("Error message", *error_msg_);
+    } else
+    {
+        stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "The Phidget reports no errors.");
+    }
+}
+
+void SpatialRosI::checkConnection(diagnostic_updater::DiagnosticStatusWrapper &stat)
+{
+    if (is_connected_)
+    {
+        stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "The Phidget is connected.");
+        stat.add("Device Serial Number", spatial_->getSerialNumber());
+        stat.add("Device Name", "PhidgetSpatial");
+        stat.add("Device Type", "IMU");
+    } else
+    {
+        stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "The Phidget is not connected. Check the USB.");
+    }
+}
+
 }  // namespace phidgets
 
 RCLCPP_COMPONENTS_REGISTER_NODE(phidgets::SpatialRosI)