Merge pull request #130 from shadow-robot/F#DXTL-242_right_arm_and_ha…

…nd_integration 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.
shadow-robot · Feb 6, 2025 · bc52a99 · bc52a99
2 parents 249c28f + fcc8708
commit bc52a99
Show file tree

Hide file tree

Showing 8 changed files with 30 additions and 15 deletions.
diff --git a/ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_hardware.h b/ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_hardware.h
@@ -2,6 +2,7 @@
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2025 Shadow Robot Company Ltd.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
@@ -199,6 +200,9 @@ class EthercatHardwareDiagnosticsPublisher
   EthernetInterfaceInfo ethernet_interface_info_;
   vector<diagnostic_msgs::KeyValue> 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
@@ -2,6 +2,7 @@
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
+*  Copyright (c) 2025 Shadow Robot Company Ltd.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
@@ -438,6 +439,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 +546,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;
@@ -1054,13 +1068,11 @@ void EthercatHardware::collectDiagnostics()
     oob_com_->txandrx(&frame);
 
     // Worry about locking for single value?
-    //diagnostics_.device_count_ = status.get_adp();
-
+    // diagnostics_.device_count_ = status.get_adp();
   }
 
   for (unsigned i = 0; i < slaves_.size(); ++i)
   {
-
     boost::shared_ptr<EthercatDevice> d(slaves_[i]);
     d->collectDiagnostics(oob_com_);
   }

diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h
@@ -2,7 +2,7 @@
 *
 * Software License Agreement (BSD License)
 *
-*  Copyright (c) 2020, Shadow Robot Company Ltd.
+*  Copyright (c) 2019-2020, 2025 Shadow Robot Company Ltd.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without

diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp
@@ -37,7 +37,7 @@
 
 #include <string>
 #include <vector>
-//#include <ros_ethercat_model/actuator_command_interface.h>
+// #include <ros_ethercat_model/actuator_command_interface.h>
 
 #include <ros/ros.h>
 
@@ -68,7 +68,8 @@ typedef struct __attribute__((__packed__)) ActuatorOdometry
     volatile uint32_t odo_2;
     volatile uint32_t odo_3;
     volatile uint32_t odo_4;
-} ActuatorOdometry;
+}
+ActuatorOdometry;
 
 
 class ActuatorState
@@ -127,7 +128,6 @@ class ActuatorState
   double motor_voltage_;  //!< Motor voltage (in volts)
   ActuatorCommandMode command_type_;  // switch between pwm and effort
   ActuatorOdometry odometry_;
-
 };
 
 class ActuatorCommand

diff --git a/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp b/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp
@@ -6,7 +6,7 @@
 *
 * Software License Agreement (BSD License)
 *
-*  Copyright (c) 2017, Shadow Robot Company Ltd.
+*  Copyright (c) 2017, 2019, 2020, 2025 Shadow Robot Company Ltd.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
@@ -72,8 +72,8 @@ class ImuState
     data_.linear_acceleration = linear_acceleration_;
     data_.linear_acceleration_covariance = linear_acceleration_covariance_;
   };
-  ImuState(){}
-  
+  ImuState() {}
+
   void initialiseToZero(void)
   {
     INIT_ARRAY(orientation_, 0.0);
@@ -83,7 +83,6 @@ class ImuState
     INIT_ARRAY(angular_velocity_covariance_, 0.0);
     INIT_ARRAY(linear_acceleration_covariance_, 0.0);
   }
-
 };
 };  // namespace ros_ethercat_model
 

diff --git a/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp b/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp
@@ -6,7 +6,7 @@
 *
 * Software License Agreement (BSD License)
 *
-*  Copyright (c) 2014, Shadow Robot Company Ltd.
+*  Copyright (c) 2014-2019, 2024-2025 Shadow Robot Company Ltd.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without

diff --git a/ros_ethercat_model/include/ros_ethercat_model/ros_ethercat.hpp b/ros_ethercat_model/include/ros_ethercat_model/ros_ethercat.hpp
@@ -6,7 +6,7 @@
 *
 * Software License Agreement (BSD License)
 *
-*  Copyright (c) 2014, Shadow Robot Company Ltd.
+*  Copyright (c) 2014, 2016, 2017, 2019, 2022, 2025 Shadow Robot Company Ltd.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without

diff --git a/ros_ethercat_model/launch/joint_state_publisher.launch b/ros_ethercat_model/launch/joint_state_publisher.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <!--
  Software License Agreement (BSD License)
- Copyright © 2022 belongs to Shadow Robot Company Ltd.
+ Copyright © 2014-2015, 2018-2023, 2025 belongs to Shadow Robot Company Ltd.
  All rights reserved.
  Redistribution and use in source and binary forms, with or without modification,
  are permitted provided that the following conditions are met: