Add additional Puma diagnostics (#270)

Author: civerachb-cpr

clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED)
 find_package(can_msgs REQUIRED)
 find_package(clearpath_motor_msgs REQUIRED)
 find_package(clearpath_ros2_socketcan_interface REQUIRED)
+find_package(diagnostic_updater REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -20,6 +21,7 @@ set(DEPENDENCIES
   can_msgs
   clearpath_motor_msgs
   clearpath_ros2_socketcan_interface
+  diagnostic_updater
   rclcpp
   std_msgs
   sensor_msgs

clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp

@@ -32,6 +32,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
 #include "clearpath_motor_msgs/msg/puma_status.hpp"
 
+#include "diagnostic_updater/update_functions.hpp"
+
 #include "puma_motor_driver/can_proto.hpp"
 
 namespace puma_motor_driver
@@ -460,6 +462,10 @@ class Driver
     }
   };
 
+  // Diagnostics
+  void runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void driverUpdateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat, bool updating);
+
 private:
   std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
   std::shared_ptr<rclcpp::Node> nh_;
@@ -521,6 +527,10 @@ class Driver
   Field * ictrlFieldForMessage(uint32_t api);
   Field * statusFieldForMessage(uint32_t api);
   Field * cfgFieldForMessage(uint32_t api);
+
+  // Frequency Status for diagnostics
+  std::shared_ptr<double> can_feedback_rate_; // Shared ptr prevents copy errors of FrequencyStatus
+  std::shared_ptr<diagnostic_updater::FrequencyStatus> can_feedback_freq_status_;
 };
 
 }  // namespace puma_motor_driver

clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp

@@ -24,6 +24,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #ifndef PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H
 #define PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H
 
+#include <map>
+
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/float64.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
@@ -35,6 +37,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
 #include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp"
 
+#include "diagnostic_updater/diagnostic_updater.hpp"
+
 #include "puma_motor_driver/driver.hpp"
 // #include "puma_motor_driver/diagnostic_updater.hpp"
 
@@ -121,6 +125,9 @@ class MultiPumaNode
   void run();
 
 private:
+  using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper;
+  using PumaStatus = clearpath_motor_msgs::msg::PumaStatus;
+
   std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
   std::vector<puma_motor_driver::Driver> drivers_;
 
@@ -149,6 +156,34 @@ class MultiPumaNode
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr cmd_sub_;
   rclcpp::TimerBase::SharedPtr run_timer_;
 
+  // Diagnostic Updater
+  diagnostic_updater::Updater updater_;
+
+  // Diagnostic labels
+  const std::map<uint8_t, std::string> MODE_FLAG_LABELS_ = {
+    {PumaStatus::MODE_VOLTAGE, "Voltage"},
+    {PumaStatus::MODE_CURRENT, "Current"},
+    {PumaStatus::MODE_SPEED, "Speed"},
+    {PumaStatus::MODE_POSITION, "Position"},
+    {PumaStatus::MODE_VCOMP, "V-Comp"},
+  };
+  const std::map<uint8_t, std::string> FAULT_FLAG_LABELS_ = {
+    {PumaStatus::FAULT_CURRENT, "Current Fault"},
+    {PumaStatus::FAULT_TEMPERATURE, "Temperature Fault"},
+    {PumaStatus::FAULT_BUS_VOLTAGE, "Bus Voltage Fault"},
+    {PumaStatus::FAULT_BRIDGE_DRIVER, "Bridge Driver Fault"},
+  };
+  const std::map<std::string, std::string> PUMA_MOTOR_LABELS_ = {
+    {"front_left_wheel_joint", "Front Left"},
+    {"front_right_wheel_joint", "Front Right"},
+    {"rear_left_wheel_joint", "Rear Left"},
+    {"rear_right_wheel_joint", "Rear Right"},
+  };
+
+
+  // Diagnostic Tasks
+  void driverDiagnostic(DiagnosticStatusWrapper & stat, int i);
+  void protectionDiagnostic(DiagnosticStatusWrapper & stat);
 };
 
 #endif // PUMA_MOTOR_DRIVER_PUMA_NODE_H

clearpath_motor_drivers/puma_motor_driver/package.xml

@@ -13,6 +13,7 @@
   <depend>can_msgs</depend>
   <depend version_gte="2.4.0">clearpath_motor_msgs</depend>
   <depend version_gte="2.1.1">clearpath_ros2_socketcan_interface</depend>
+  <depend>diagnostic_updater</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>

clearpath_motor_drivers/puma_motor_driver/src/driver.cpp

@@ -31,6 +31,9 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #include <math.h>
 #include "rclcpp/rclcpp.hpp"
 
+// must match firmware
+#define CAN_FEEDBACK_RATE 40.0
+
 namespace puma_motor_driver
 {
 
@@ -74,6 +77,15 @@ Driver::Driver(
   encoder_cpr_(1),
   gear_ratio_(1)
 {
+  can_feedback_rate_ = std::make_shared<double>(CAN_FEEDBACK_RATE);
+  can_feedback_freq_status_ = std::make_shared<diagnostic_updater::FrequencyStatus>(
+    diagnostic_updater::FrequencyStatusParam(
+      can_feedback_rate_.get(),
+      can_feedback_rate_.get(),
+      0.1,
+      5
+    )
+  );
 }
 
 void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg)
@@ -96,14 +108,19 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg)
     field = statusFieldForMessage(received_api);
   } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) {
     field = ictrlFieldForMessage(received_api);
+    can_feedback_freq_status_->tick();
   } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) {
     field = posFieldForMessage(received_api);
+    can_feedback_freq_status_->tick();
   } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) {
     field = vcompFieldForMessage(received_api);
+    can_feedback_freq_status_->tick();
   } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) {
     field = spdFieldForMessage(received_api);
+    can_feedback_freq_status_->tick();
   } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) {
     field = voltageFieldForMessage(received_api);
+    can_feedback_freq_status_->tick();
   }
 
   if (!field) {
@@ -1026,4 +1043,18 @@ Driver::Field * Driver::cfgFieldForMessage(uint32_t api)
   return &cfg_fields_[cfg_field_index];
 }
 
+/**
+ * @brief Runs the frequency diagnostic update to populate the status message
+ */
+void Driver::runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  can_feedback_freq_status_->run(stat);
+
+  stat.add("Duty cycle", lastDutyCycle());
+  stat.add("Current (A)", lastCurrent());
+  stat.add("Speed (rad/s)", lastSpeed());
+  stat.add("Position", lastPosition());
+  stat.add("Setpoint", lastSetpoint());
+}
+
 }  // namespace puma_motor_driver

clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp

@@ -21,13 +21,16 @@ OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTE
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
+#include <string>
+
 #include "puma_motor_driver/multi_puma_node.hpp"
 
 MultiPumaNode::MultiPumaNode(const std::string node_name)
 :Node(node_name),
   active_(false),
   status_count_(0),
-  desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED)
+  desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED),
+  updater_(this)
 {
   // Parameters
   this->declare_parameter("canbus_dev", "vcan0");
@@ -118,6 +121,13 @@ MultiPumaNode::MultiPumaNode(const std::string node_name)
 
   run_timer_ = this->create_wall_timer(
     std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this));
+
+  // Setup diagnostics
+  updater_.setHardwareID("Puma");
+  for (uint8_t i = 0; i < joint_names_.size(); i++) {
+    std::string name = "Puma Motor Driver " + std::to_string(i + 1) + " (" + joint_names_[i] + ")";
+    updater_.add(name, std::bind(&MultiPumaNode::driverDiagnostic, this, std::placeholders::_1, i));
+  }
 }
 
 bool MultiPumaNode::getFeedback()
@@ -213,6 +223,40 @@ void MultiPumaNode::publishStatus()
   }
 }
 
+/**
+ * @brief Diagnostic task to report details for each motor driver
+ *
+ * @param i Driver index number
+ */
+void MultiPumaNode::driverDiagnostic(DiagnosticStatusWrapper & stat, int i)
+{
+  // Assume we're OK. This will be merged over later on if we aren't
+  stat.summary(DiagnosticStatusWrapper::OK, "OK");
+
+  drivers_[i].runFreqStatus(stat);
+
+  // basic stats
+  stat.add("CAN ID", (int)status_msg_.drivers[i].device_number);
+  stat.add("Joint Name", status_msg_.drivers[i].device_name);
+  stat.add("Bus Voltage (V)", status_msg_.drivers[i].bus_voltage);
+  stat.add("Motor Temperature (C)", status_msg_.drivers[i].temperature);
+  stat.add("Output Voltage (V)", status_msg_.drivers[i].output_voltage);
+  stat.add("Analogue Input (V)", status_msg_.drivers[i].analog_input);
+
+  // control mode; convert to a string
+  stat.add("Mode", MODE_FLAG_LABELS_.at((int)status_msg_.drivers[i].mode));
+
+  // fault flags; these are a bit field
+  for (auto label : FAULT_FLAG_LABELS_) {
+    bool flag = (status_msg_.drivers[i].fault >> label.first) & 0x01;
+    stat.add(label.second, flag ? "True" : "False");
+    if (flag) {
+      // raise a warning if there's a fault
+      stat.mergeSummary(DiagnosticStatusWrapper::WARN, label.second);
+    }
+  }
+}
+
 void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
 {
   if (active_) {