From 3392fc20f0bac73f191a4bdaf495d820eb8cd6a5 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Wed, 24 Sep 2025 17:30:36 -0400 Subject: [PATCH 01/12] First pass adding additional diagnostics for Puma motor controllers. Untested at this stage, but compiles --- .../puma_motor_driver/CMakeLists.txt | 2 ++ .../include/puma_motor_driver/driver.hpp | 13 +++++++ .../puma_motor_driver/multi_puma_node.hpp | 35 +++++++++++++++++++ .../puma_motor_driver/package.xml | 1 + .../puma_motor_driver/src/driver.cpp | 16 +++++++++ .../puma_motor_driver/src/multi_puma_node.cpp | 34 +++++++++++++++++- 6 files changed, 100 insertions(+), 1 deletion(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt b/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt index 90af4c45..0088c8f7 100644 --- a/clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt +++ b/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 diff --git a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp index b103b1cc..3a3317ac 100644 --- a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp +++ b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp @@ -32,8 +32,13 @@ 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" +// Used to smooth out voltage/current/velocity data for diagnostics since it is infrequently updated +#define DIAGNOSTICS_LOW_PASS 0.9 + namespace puma_motor_driver { @@ -460,6 +465,10 @@ class Driver } }; + // Diagnostics + void runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + void driverUpdateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat, bool updating); + private: std::shared_ptr interface_; std::shared_ptr nh_; @@ -521,6 +530,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 can_feedback_rate_; // Shared ptr prevents copy errors of FrequencyStatus + std::shared_ptr can_feedback_freq_status_; }; } // namespace puma_motor_driver diff --git a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp index 14c10de1..d70a48b9 100644 --- a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp +++ b/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 + #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 interface_; std::vector drivers_; @@ -149,6 +156,34 @@ class MultiPumaNode rclcpp::Subscription::SharedPtr cmd_sub_; rclcpp::TimerBase::SharedPtr run_timer_; + // Diagnostic Updater + diagnostic_updater::Updater updater_; + + // Diagnostic labels + const std::map 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 FAULT_FLAG_LABELS_ = { + {PumaStatus::FAULT_CURRENT, "Current"}, + {PumaStatus::FAULT_TEMPERATURE, "Temperature"}, + {PumaStatus::FAULT_BUS_VOLTAGE, "Bus voltage"}, + {PumaStatus::FAULT_BRIDGE_DRIVER, "Bridge driver"}, + }; + const std::map 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 diff --git a/clearpath_motor_drivers/puma_motor_driver/package.xml b/clearpath_motor_drivers/puma_motor_driver/package.xml index 65a15db6..9a048b9d 100644 --- a/clearpath_motor_drivers/puma_motor_driver/package.xml +++ b/clearpath_motor_drivers/puma_motor_driver/package.xml @@ -13,6 +13,7 @@ can_msgs clearpath_motor_msgs clearpath_ros2_socketcan_interface + diagnostic_updater rclcpp sensor_msgs std_msgs diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp index f9b078f7..2a987b99 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp +++ b/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 #include "rclcpp/rclcpp.hpp" +// must match firmware +#define CAN_FEEDBACK_RATE 50.0 + namespace puma_motor_driver { @@ -74,6 +77,9 @@ Driver::Driver( encoder_cpr_(1), gear_ratio_(1) { + can_feedback_rate_ = std::make_shared(CAN_FEEDBACK_RATE); + can_feedback_freq_status_ = std::make_shared( + 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) @@ -1026,4 +1032,14 @@ 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); + + // TODO: add any additional fields (e.g. voltage, current, velocity)? +} + } // namespace puma_motor_driver diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index 0777ac70..c6809075 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/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 + #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"); @@ -213,6 +216,35 @@ 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) +{ + // 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", 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_) { From b44ce0203560033510bc8720a03a1eb28f797a5d Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 10:53:40 -0400 Subject: [PATCH 02/12] Start the diagnostic task --- .../puma_motor_driver/src/multi_puma_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index c6809075..df98c64f 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -121,6 +121,10 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) run_timer_ = this->create_wall_timer( std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); + + // Add diagnostic tasks + 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() From 12831514d53dcd671bc0d83e0bc190d6d450ef2b Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 11:55:23 -0400 Subject: [PATCH 03/12] Make the fault labels include the word "fault" for clarity. Fix initializing the diagnostic tasks. Add a default summary for when things are fine --- .../include/puma_motor_driver/multi_puma_node.hpp | 8 ++++---- .../puma_motor_driver/src/multi_puma_node.cpp | 13 ++++++++++--- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp index d70a48b9..f2c4a038 100644 --- a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp +++ b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp @@ -168,10 +168,10 @@ class MultiPumaNode {PumaStatus::MODE_VCOMP, "V-Comp"}, }; const std::map FAULT_FLAG_LABELS_ = { - {PumaStatus::FAULT_CURRENT, "Current"}, - {PumaStatus::FAULT_TEMPERATURE, "Temperature"}, - {PumaStatus::FAULT_BUS_VOLTAGE, "Bus voltage"}, - {PumaStatus::FAULT_BRIDGE_DRIVER, "Bridge driver"}, + {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 PUMA_MOTOR_LABELS_ = { {"front_left_wheel_joint", "Front Left"}, diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index df98c64f..27056c29 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -122,9 +122,13 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) run_timer_ = this->create_wall_timer( std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); - // Add diagnostic tasks - 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)); + // 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() @@ -227,6 +231,9 @@ void MultiPumaNode::publishStatus() */ void MultiPumaNode::driverDiagnostic(DiagnosticStatusWrapper & stat, int i) { + // Assume we're OK. This will be merged over later on if we aren't + stat.mergeSummary(DiagnosticStatusWrapper::OK, "OK"); + // basic stats stat.add("CAN ID", (int)status_msg_.drivers[i].device_number); stat.add("Joint Name", status_msg_.drivers[i].device_name); From 76266d4eb88c38a905e07fb4edb633586ddab661 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 11:57:25 -0400 Subject: [PATCH 04/12] Use summary, not mergeSummary for the initial message --- .../puma_motor_driver/src/multi_puma_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index 27056c29..e704f933 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -232,7 +232,7 @@ void MultiPumaNode::publishStatus() void MultiPumaNode::driverDiagnostic(DiagnosticStatusWrapper & stat, int i) { // Assume we're OK. This will be merged over later on if we aren't - stat.mergeSummary(DiagnosticStatusWrapper::OK, "OK"); + stat.summary(DiagnosticStatusWrapper::OK, "OK"); // basic stats stat.add("CAN ID", (int)status_msg_.drivers[i].device_number); From e4504400436dce49624d174bd86f05ad9e994f58 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 12:03:58 -0400 Subject: [PATCH 05/12] Add units to the Bus Voltage field --- .../puma_motor_driver/src/multi_puma_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index e704f933..9481cde1 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -237,7 +237,7 @@ void MultiPumaNode::driverDiagnostic(DiagnosticStatusWrapper & stat, int i) // 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", status_msg_.drivers[i].bus_voltage); + 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); From 6fa3a3693fda80297b13a58b5041e2ef73b852e9 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 12:55:51 -0400 Subject: [PATCH 06/12] Remove unused define, fix formatting --- .../puma_motor_driver/src/driver.cpp | 11 +++++++---- .../puma_motor_driver/src/multi_puma_node.cpp | 3 +-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp index 2a987b99..03c0b31b 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp @@ -31,9 +31,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include "rclcpp/rclcpp.hpp" -// must match firmware -#define CAN_FEEDBACK_RATE 50.0 - namespace puma_motor_driver { @@ -79,7 +76,13 @@ Driver::Driver( { can_feedback_rate_ = std::make_shared(CAN_FEEDBACK_RATE); can_feedback_freq_status_ = std::make_shared( - diagnostic_updater::FrequencyStatusParam(can_feedback_rate_.get(), can_feedback_rate_.get(), 0.1, 5)); + 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) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index 9481cde1..27be2358 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -124,8 +124,7 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) // Setup diagnostics updater_.setHardwareID("Puma"); - for (uint8_t i = 0; i < joint_names_.size(); i++) - { + 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)); } From 61ea4f55694018f3e1a3348ad3c28ec86baa0c70 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 13:20:41 -0400 Subject: [PATCH 07/12] Run frequency stats --- .../puma_motor_driver/include/puma_motor_driver/driver.hpp | 3 --- clearpath_motor_drivers/puma_motor_driver/src/driver.cpp | 5 +++-- .../puma_motor_driver/src/multi_puma_node.cpp | 2 ++ 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp index 3a3317ac..e740dfac 100644 --- a/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp +++ b/clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp @@ -36,9 +36,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "puma_motor_driver/can_proto.hpp" -// Used to smooth out voltage/current/velocity data for diagnostics since it is infrequently updated -#define DIAGNOSTICS_LOW_PASS 0.9 - namespace puma_motor_driver { diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp index 03c0b31b..c4b52cbf 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp +++ b/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 #include "rclcpp/rclcpp.hpp" +// must match firmware +#define CAN_FEEDBACK_RATE 20.0 + namespace puma_motor_driver { @@ -1041,8 +1044,6 @@ Driver::Field * Driver::cfgFieldForMessage(uint32_t api) void Driver::runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat) { can_feedback_freq_status_->run(stat); - - // TODO: add any additional fields (e.g. voltage, current, velocity)? } } // namespace puma_motor_driver diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index 27be2358..2de99775 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -233,6 +233,8 @@ 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); From 48fc477f72b090434bc298d12eccc340257f30c8 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 13:26:04 -0400 Subject: [PATCH 08/12] Tick every time we receive a puma message --- .../puma_motor_driver/src/multi_puma_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index 2de99775..0d253255 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -313,6 +313,7 @@ void MultiPumaNode::run() // Process all received messages through the connected driver instances. while (interface_->recv(recv_msg_)) { + can_feedback_freq_status_->tick(); for (auto & driver : drivers_) { driver.processMessage(recv_msg_); } From d7103567311a85180bfbd8a140a5f5e7b8e17b6b Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 13:27:52 -0400 Subject: [PATCH 09/12] Copy & paste error --- clearpath_motor_drivers/puma_motor_driver/src/driver.cpp | 1 + .../puma_motor_driver/src/multi_puma_node.cpp | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp index c4b52cbf..43834baa 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp @@ -102,6 +102,7 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) Field * field = nullptr; uint32_t received_api = getApi(*received_msg); + can_feedback_freq_status_->tick(); if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) { field = cfgFieldForMessage(received_api); } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) { diff --git a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp index 0d253255..2de99775 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp @@ -313,7 +313,6 @@ void MultiPumaNode::run() // Process all received messages through the connected driver instances. while (interface_->recv(recv_msg_)) { - can_feedback_freq_status_->tick(); for (auto & driver : drivers_) { driver.processMessage(recv_msg_); } From b672220f4aca237a5566aad3aa67296db341506f Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 13:28:34 -0400 Subject: [PATCH 10/12] Move where we increment --- clearpath_motor_drivers/puma_motor_driver/src/driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp index 43834baa..7f3d032a 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp @@ -102,7 +102,6 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) Field * field = nullptr; uint32_t received_api = getApi(*received_msg); - can_feedback_freq_status_->tick(); if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) { field = cfgFieldForMessage(received_api); } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) { @@ -127,6 +126,7 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) std::copy_n(std::begin(received_msg->data), Field::FIELD_STRUCT_DATA_SIZE, std::begin(field->data)); field->received = true; + can_feedback_freq_status_->tick(); } double Driver::radPerSecToRpm() const From 9dc1fcb1859ccbedcab0febafa9cb0d43cadc273 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 25 Sep 2025 13:38:46 -0400 Subject: [PATCH 11/12] Update the CAN feedback rate to match what we're actually seeing on hardware (R100). Tick every time we get a control message that matches our control mode --- clearpath_motor_drivers/puma_motor_driver/src/driver.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp index 7f3d032a..7400c4d4 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp @@ -32,7 +32,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "rclcpp/rclcpp.hpp" // must match firmware -#define CAN_FEEDBACK_RATE 20.0 +#define CAN_FEEDBACK_RATE 40.0 namespace puma_motor_driver { @@ -108,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) { @@ -126,7 +131,6 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) std::copy_n(std::begin(received_msg->data), Field::FIELD_STRUCT_DATA_SIZE, std::begin(field->data)); field->received = true; - can_feedback_freq_status_->tick(); } double Driver::radPerSecToRpm() const From 0a00f060088b29e4d502fdfc341994c772fbf359 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 2 Oct 2025 11:05:31 -0400 Subject: [PATCH 12/12] Add additional diagnostics from the PumaFeedback messages --- clearpath_motor_drivers/puma_motor_driver/src/driver.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp index 7400c4d4..15bf7ce9 100644 --- a/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp +++ b/clearpath_motor_drivers/puma_motor_driver/src/driver.cpp @@ -1049,6 +1049,12 @@ Driver::Field * Driver::cfgFieldForMessage(uint32_t api) 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