From 409e7d2e0782f0a57b6d7fd3462f68defe0af7e3 Mon Sep 17 00:00:00 2001 From: Toni Oliver Date: Tue, 14 Jan 2025 12:37:10 +0000 Subject: [PATCH] Add a prefix to the EhtercatHardware diagnostics so that multiple nodes can publish without confusion. The prefix is based on the node's namespace. If the node is not namespaced the prefix will be empty. --- .../ros_ethercat_hardware/ethercat_hardware.h | 3 +++ ros_ethercat_hardware/src/ethercat_hardware.cpp | 15 ++++++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_hardware.h b/ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_hardware.h index 6297add..1961e16 100644 --- a/ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_hardware.h +++ b/ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_hardware.h @@ -199,6 +199,9 @@ class EthercatHardwareDiagnosticsPublisher EthernetInterfaceInfo ethernet_interface_info_; vector values_; diagnostic_updater::DiagnosticStatusWrapper status_; + + // Prefix to add to the status.name for diagnostics + string diagnostics_name_prefix_; }; class EthercatHardware diff --git a/ros_ethercat_hardware/src/ethercat_hardware.cpp b/ros_ethercat_hardware/src/ethercat_hardware.cpp index 111cae5..e2b5ed7 100644 --- a/ros_ethercat_hardware/src/ethercat_hardware.cpp +++ b/ros_ethercat_hardware/src/ethercat_hardware.cpp @@ -438,6 +438,19 @@ EthercatHardwareDiagnosticsPublisher::EthercatHardwareDiagnosticsPublisher(ros:: last_dropped_packet_count_(0), last_dropped_packet_time_(0) { + ros::NodeHandle n; + string node_namespace = n.getNamespace(); + // Remove leading slash if not empty. This will leave the variable empty if the namespace is "/" + // (i.e. node not namespaced). + if (!node_namespace.empty() && node_namespace[0] == '/') + { + node_namespace.erase(0, 1); + } + + if (!node_namespace.empty()) + { + diagnostics_name_prefix_ = node_namespace + " "; + } } EthercatHardwareDiagnosticsPublisher::~EthercatHardwareDiagnosticsPublisher() @@ -532,7 +545,7 @@ void EthercatHardwareDiagnosticsPublisher::publishDiagnostics() status_.clearSummary(); status_.clear(); - status_.name = "EtherCAT Master"; + status_.name = diagnostics_name_prefix_ + "EtherCAT Master"; // if (diagnostics_.motors_halted_) // { // std::ostringstream desc;