Multi-master and non-zero master support

ethercat_driver/include/ethercat_driver/ethercat_driver.hpp

@@ -83,7 +83,7 @@ class EthercatDriver : public hardware_interface::SystemInterface
     "ethercat_interface", "ethercat_interface::EcSlave"};
 
   int control_frequency_;
-  ethercat_interface::EcMaster master_;
+  std::unique_ptr<ethercat_interface::EcMaster> master_;
   std::mutex ec_mutex_;
   bool activated_;
 };

ethercat_driver/src/ethercat_driver.cpp

@@ -33,6 +33,16 @@ CallbackReturn EthercatDriver::on_init(
   const std::lock_guard<std::mutex> lock(ec_mutex_);
   activated_ = false;
 
+  // Parse master_id from hardware parameters
+  int master_id = 0;  // Default to 0 if not specified
+  if (info_.hardware_parameters.find("master_id") != info_.hardware_parameters.end()) {
+    master_id = std::stoi(info_.hardware_parameters["master_id"]);
+  }
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Master ID: %d", master_id);
+
+  // Initialize master with the specified ID
+  master_ = std::make_unique<ethercat_interface::EcMaster>(master_id);
+
   hw_joint_states_.resize(info_.joints.size());
   for (uint j = 0; j < info_.joints.size(); j++) {
     hw_joint_states_[j].resize(
@@ -282,10 +292,10 @@ CallbackReturn EthercatDriver::on_activate(
 
   // start EC and wait until state operative
 
-  master_.setCtrlFrequency(control_frequency_);
+  master_->setCtrlFrequency(control_frequency_);
 
   for (auto i = 0ul; i < ec_modules_.size(); i++) {
-    master_.addSlave(
+    master_->addSlave(
       std::stod(ec_module_parameters_[i]["alias"]),
       std::stod(ec_module_parameters_[i]["position"]),
       ec_modules_[i].get());
@@ -295,7 +305,7 @@ CallbackReturn EthercatDriver::on_activate(
   for (auto i = 0ul; i < ec_modules_.size(); i++) {
     for (auto & sdo : ec_modules_[i]->sdo_config) {
       uint32_t abort_code;
-      int ret = master_.configSlaveSdo(
+      int ret = master_->configSlaveSdo(
         std::stod(ec_module_parameters_[i]["position"]),
         sdo,
         &abort_code
@@ -311,7 +321,7 @@ CallbackReturn EthercatDriver::on_activate(
     }
   }
 
-  if (!master_.activate()) {
+  if (!master_->activate()) {
     RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), "Activate EcMaster failed");
     return CallbackReturn::ERROR;
   }
@@ -328,7 +338,7 @@ CallbackReturn EthercatDriver::on_activate(
     clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
     // update EtherCAT bus
 
-    master_.update();
+    master_->update();
     RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
 
     // check if operational
@@ -340,7 +350,7 @@ CallbackReturn EthercatDriver::on_activate(
       running = false;
     }
     // calculate next shot. carry over nanoseconds into microseconds.
-    t.tv_nsec += master_.getInterval();
+    t.tv_nsec += master_->getInterval();
     while (t.tv_nsec >= 1000000000) {
       t.tv_nsec -= 1000000000;
       t.tv_sec++;
@@ -363,8 +373,11 @@ CallbackReturn EthercatDriver::on_deactivate(
 
   RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait...");
 
+  // Clean up all modules before stopping the master
+  ec_modules_.clear();
+
   // stop EC and disconnect
-  master_.stop();
+  master_->stop();
 
   RCLCPP_INFO(
     rclcpp::get_logger("EthercatDriver"), "System successfully stopped!");
@@ -379,7 +392,7 @@ hardware_interface::return_type EthercatDriver::read(
   // try to lock so we can avoid blocking the read/write loop on the lock.
   const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
   if (lock.owns_lock() && activated_) {
-    master_.readData();
+    master_->readData();
   }
   return hardware_interface::return_type::OK;
 }
@@ -391,7 +404,7 @@ hardware_interface::return_type EthercatDriver::write(
   // try to lock so we can avoid blocking the read/write loop on the lock.
   const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
   if (lock.owns_lock() && activated_) {
-    master_.writeData();
+    master_->writeData();
   }
   return hardware_interface::return_type::OK;
 }

ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst

@@ -83,6 +83,12 @@ Each PDO Channel has the following configuration flags:
     - Data conversion factor/scale (:code:`type` : :code:`double`).
   * - :code:`offset`
     - Data offset term (:code:`type` : :code:`double`).
+  * - :code:`peak_upper_limit`
+    - Peak Upper limit (:code:`type` : :code:`double`) applied to command_interface as saturation before multiplying factor and adding offset. default value is :code:`6.2831853` (2*PI).
+  * - :code:`peak_lower_limit`
+    - Peak Lower limit (:code:`type` : :code:`double`) applied to command_interface as saturation before multiplying factor and adding offset. default value is :code:`-6.2831853` (-2*PI).
+  * - :code:`peak_limit_multiplier`
+    - Peak limit multiplier (:code:`type` : :code:`double`) applied to peak limits to scale them slightly below the actual actuator limits (specified in peak upper/lower limits). This is useful to avoid hitting the limits during operation.
 
 
 .. warning:: For each channel, tags :code:`index`, :code:`sub_index` and :code:`type` are **mandatory** even if the channel is not used in order to fill the data layout expected by the module. All other tags can remain unset.

ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp

@@ -24,6 +24,7 @@
 #define CiA402D_TPDO_POSITION ((uint16_t) 0x6064)
 #define CiA402D_TPDO_STATUSWORD  ((uint16_t) 0x6041)
 #define CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY  ((uint16_t) 0x6061)
+#define CiA402D_TPDO_ERROR_CODE  ((uint16_t) 0x603f)
 
 #include <map>
 #include <string>

ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp

@@ -21,6 +21,7 @@
 #include <string>
 #include <unordered_map>
 #include <limits>
+#include <atomic>
 
 #include "yaml-cpp/yaml.h"
 #include "ethercat_interface/ec_slave.hpp"
@@ -51,6 +52,9 @@ class EcCiA402Drive : public GenericEcSlave
   int8_t mode_of_operation_ = -1;
 
 protected:
+  // Global aggregated count of faulted instances (process-wide)
+  static std::atomic<int> num_faulted_;
+
   uint32_t counter_ = 0;
   uint16_t last_status_word_ = -1;
   uint16_t status_word_ = 0;
@@ -60,10 +64,18 @@ class EcCiA402Drive : public GenericEcSlave
   bool initialized_ = false;
   bool auto_fault_reset_ = false;
   bool auto_state_transitions_ = true;
+  bool auto_state_transitions_cmd_ = false;
   bool fault_reset_ = false;
   int fault_reset_command_interface_index_ = -1;
   bool last_fault_reset_command_ = false;
   double last_position_ = std::numeric_limits<double>::quiet_NaN();
+  bool temperature_fault_detected_ = false;
+  uint16_t error_code_ = 0;
+  int auto_state_cmd_index_ = -1;
+  bool last_auto_state_cmd_ = false;
+
+  // Quick stop behavior controls
+  bool exclude_from_global_quick_stop_ = false; // Do not propagate this drive's faults globally if true
 
   /** returns device state based upon the status_word */
   DeviceState deviceState(uint16_t status_word);