Merge pull request #282 from project-march/fix/ethercat-master-watchdog

Fix; ethercat master watchdog

Author: Roelemans

march_hardware/include/march_hardware/error/error_type.h

@@ -31,6 +31,7 @@ enum class ErrorType
   MISSING_REQUIRED_KEY = 119,
   INIT_URDF_FAILED = 120,
   INVALID_SW_STRING = 121,
+  SLAVE_LOST_TIMOUT = 122,
   UNKNOWN = 999,
 };
 

march_hardware/include/march_hardware/ethercat/ethercat_master.h

@@ -2,6 +2,7 @@
 #ifndef MARCH_HARDWARE_ETHERCAT_ETHERCATMASTER_H
 #define MARCH_HARDWARE_ETHERCAT_ETHERCATMASTER_H
 #include <atomic>
+#include <exception>
 #include <vector>
 #include <string>
 #include <thread>
@@ -23,7 +24,7 @@ namespace march
 class EthercatMaster
 {
 public:
-  EthercatMaster(std::string ifname, int max_slave_index, int cycle_time);
+  EthercatMaster(std::string ifname, int max_slave_index, int cycle_time, int slave_timeout);
   ~EthercatMaster();
 
   /* Delete copy constructor/assignment since the member thread can not be copied */
@@ -37,6 +38,8 @@ class EthercatMaster
   bool isOperational() const;
   void waitForPdo();
 
+  std::exception_ptr getLastException() const noexcept;
+
   /**
    * Returns the cycle time in milliseconds.
    */
@@ -75,13 +78,27 @@ class EthercatMaster
 
   /**
    * Sends the PDO and receives the working counter and check if this is lower than expected.
+   *
+   * @returns true if and only if all PDOs have been successfully sent and received, otherwise false.
    */
-  void sendReceivePdo();
+  bool sendReceivePdo();
 
   /**
    * Checks if all the slaves are connected and in operational state.
    */
-  static void monitorSlaveConnection();
+  void monitorSlaveConnection();
+
+  /**
+   * Attempts to recover a slave to operational state.
+   *
+   * @returns true when recovery was successfull, otherwise false.
+   */
+  bool attemptSlaveRecover(int slave);
+
+  /**
+   * Sets ethercat state to INIT and closes port.
+   */
+  void closeEthercat();
 
   /**
    * Sets the ethercat thread priority and scheduling
@@ -105,7 +122,12 @@ class EthercatMaster
   char io_map_[4096] = { 0 };
   int expected_working_counter_ = 0;
 
+  int latest_lost_slave_ = -1;
+  const int slave_watchdog_timeout_;
+  std::chrono::high_resolution_clock::time_point valid_slaves_timestamp_ms_;
+
   std::thread ethercat_thread_;
+  std::exception_ptr last_exception_;
 };
 
 }  // namespace march

march_hardware/include/march_hardware/march_robot.h

@@ -26,10 +26,12 @@ class MarchRobot
 public:
   using iterator = std::vector<Joint>::iterator;
 
-  MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime);
+  MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime,
+             int ecatSlaveTimeout);
 
   MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf,
-             std::unique_ptr<PowerDistributionBoard> powerDistributionBoard, ::std::string ifName, int ecatCycleTime);
+             std::unique_ptr<PowerDistributionBoard> powerDistributionBoard, ::std::string ifName, int ecatCycleTime,
+             int ecatSlaveTimeout);
 
   ~MarchRobot();
 
@@ -53,6 +55,8 @@ class MarchRobot
 
   bool isEthercatOperational();
 
+  std::exception_ptr getLastEthercatException() const noexcept;
+
   void waitForPdo();
 
   int getEthercatCycleTime() const;

march_hardware/src/error/error_type.cpp

@@ -51,6 +51,8 @@ const char* getErrorDescription(ErrorType type)
       return "Failed to load URDF from parameter server";
     case ErrorType::INVALID_SW_STRING:
       return "Slave has incorrect SW file";
+    case ErrorType::SLAVE_LOST_TIMOUT:
+      return "EtherCAT slave monitor timer elapsed, connection has been lost";
     default:
       return "Unknown error occurred. Please create/use a documented error";
   }

march_hardware/src/ethercat/ethercat_master.cpp

@@ -3,6 +3,7 @@
 #include "march_hardware/error/hardware_exception.h"
 
 #include <chrono>
+#include <exception>
 #include <sstream>
 #include <string>
 #include <vector>
@@ -13,8 +14,12 @@
 
 namespace march
 {
-EthercatMaster::EthercatMaster(std::string ifname, int max_slave_index, int cycle_time)
-  : is_operational_(false), ifname_(std::move(ifname)), max_slave_index_(max_slave_index), cycle_time_ms_(cycle_time)
+EthercatMaster::EthercatMaster(std::string ifname, int max_slave_index, int cycle_time, int slave_timeout)
+  : is_operational_(false)
+  , ifname_(std::move(ifname))
+  , max_slave_index_(max_slave_index)
+  , cycle_time_ms_(cycle_time)
+  , slave_watchdog_timeout_(slave_timeout)
 {
 }
 
@@ -36,12 +41,18 @@ int EthercatMaster::getCycleTime() const
 void EthercatMaster::waitForPdo()
 {
   std::unique_lock<std::mutex> lock(this->wait_on_pdo_condition_mutex_);
-  this->wait_on_pdo_condition_var_.wait(lock, [&] { return this->pdo_received_; });
+  this->wait_on_pdo_condition_var_.wait(lock, [&] { return this->pdo_received_ || !this->is_operational_; });
   this->pdo_received_ = false;
 }
 
+std::exception_ptr EthercatMaster::getLastException() const noexcept
+{
+  return this->last_exception_;
+}
+
 bool EthercatMaster::start(std::vector<Joint>& joints)
 {
+  this->last_exception_ = nullptr;
   this->ethercatMasterInitiation();
   return this->ethercatSlaveInitiation(joints);
 }
@@ -152,15 +163,15 @@ void EthercatMaster::ethercatLoop()
   {
     const auto begin_time = std::chrono::high_resolution_clock::now();
 
-    this->sendReceivePdo();
-    monitorSlaveConnection();
+    const bool pdo_received = this->sendReceivePdo();
+    this->monitorSlaveConnection();
 
     const auto end_time = std::chrono::high_resolution_clock::now();
     const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - begin_time);
 
     {
       std::lock_guard<std::mutex> lock(this->wait_on_pdo_condition_mutex_);
-      this->pdo_received_ = true;
+      this->pdo_received_ = pdo_received;
     }
     this->wait_on_pdo_condition_var_.notify_one();
 
@@ -185,30 +196,132 @@ void EthercatMaster::ethercatLoop()
       total_loops = 0;
       not_achieved_count = 0;
     }
+
+    const auto delta_t = std::chrono::high_resolution_clock::now() - this->valid_slaves_timestamp_ms_;
+    const auto slave_lost_duration = std::chrono::duration_cast<std::chrono::milliseconds>(delta_t);
+    const std::chrono::milliseconds slave_watchdog_timeout(this->slave_watchdog_timeout_);
+
+    if (slave_lost_duration > slave_watchdog_timeout)
+    {
+      this->last_exception_ = std::make_exception_ptr(error::HardwareException(
+          error::ErrorType::SLAVE_LOST_TIMOUT, "Slave connection lost for %i ms from slave %i and onwards.",
+          this->slave_watchdog_timeout_, this->latest_lost_slave_));
+      this->is_operational_ = false;
+      this->wait_on_pdo_condition_var_.notify_one();
+
+      this->closeEthercat();
+    }
   }
 }
 
-void EthercatMaster::sendReceivePdo()
+bool EthercatMaster::sendReceivePdo()
 {
-  ec_send_processdata();
-  int wkc = ec_receive_processdata(EC_TIMEOUTRET);
-  if (wkc < this->expected_working_counter_)
+  if (this->latest_lost_slave_ == -1)
   {
-    ROS_WARN_THROTTLE(1, "Working counter: %d  is lower than expected: %d", wkc, this->expected_working_counter_);
+    ec_send_processdata();
+    const int wkc = ec_receive_processdata(EC_TIMEOUTRET);
+    if (wkc < this->expected_working_counter_)
+    {
+      ROS_WARN_THROTTLE(1, "Working counter: %d  is lower than expected: %d", wkc, this->expected_working_counter_);
+      return false;
+    }
+    return true;
   }
+  return false;
 }
 
 void EthercatMaster::monitorSlaveConnection()
 {
+  ec_readstate();
   for (int slave = 1; slave <= ec_slavecount; slave++)
   {
-    ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
-    if (!ec_slave[slave].state)
+    if (ec_slave[slave].state != EC_STATE_OPERATIONAL)
     {
       ROS_WARN_THROTTLE(1, "EtherCAT train lost connection from slave %d onwards", slave);
-      return;
+
+      if (!this->attemptSlaveRecover(slave))
+      {
+        this->latest_lost_slave_ = slave;
+        return;
+      }
+    }
+  }
+
+  if (this->latest_lost_slave_ > -1)
+  {
+    ROS_INFO("All slaves returned to operational state.");
+  }
+
+  this->latest_lost_slave_ = -1;
+  this->valid_slaves_timestamp_ms_ = std::chrono::high_resolution_clock::now();
+}
+
+bool EthercatMaster::attemptSlaveRecover(int slave)
+{
+  if (ec_slave[slave].state != EC_STATE_OPERATIONAL)
+  {
+    if (ec_slave[slave].state == EC_STATE_PRE_OP)
+    {
+      ec_slave[slave].state = EC_STATE_SAFE_OP;
+      ec_writestate(slave);
+    }
+
+    if (ec_slave[slave].state == EC_STATE_SAFE_OP)
+    {
+      ec_slave[slave].state = EC_STATE_OPERATIONAL;
+      ec_writestate(slave);
+    }
+
+    if (ec_slave[slave].state > EC_STATE_NONE)
+    {
+      if (ec_reconfig_slave(slave, 500))
+      {
+        ec_slave[slave].islost = FALSE;
+      }
+    }
+
+    if (!ec_slave[slave].islost)
+    {
+      ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
+      if (ec_slave[slave].state == EC_STATE_NONE)
+      {
+        ec_slave[slave].islost = TRUE;
+        ROS_ERROR("Ethercat lost connection to slave %d", slave);
+      }
+    }
+  }
+
+  if (ec_slave[slave].islost)
+  {
+    if (ec_slave[slave].state == EC_STATE_NONE)
+    {
+      if (ec_recover_slave(slave, 500))
+      {
+        ec_slave[slave].islost = FALSE;
+      }
+    }
+    else
+    {
+      ec_slave[slave].islost = FALSE;
     }
   }
+
+  if (ec_slave[slave].state == EC_STATE_OPERATIONAL)
+  {
+    ROS_INFO("Slave %i resumed operational state", slave);
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+void EthercatMaster::closeEthercat()
+{
+  ec_slave[0].state = EC_STATE_INIT;
+  ec_writestate(0);
+  ec_close();
 }
 
 void EthercatMaster::stop()
@@ -219,9 +332,7 @@ void EthercatMaster::stop()
     this->is_operational_ = false;
     this->ethercat_thread_.join();
 
-    ec_slave[0].state = EC_STATE_INIT;
-    ec_writestate(0);
-    ec_close();
+    this->closeEthercat();
   }
 }
 

march_hardware/src/march_robot.cpp

@@ -14,20 +14,21 @@
 
 namespace march
 {
-MarchRobot::MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime)
+MarchRobot::MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime,
+                       int ecatSlaveTimeout)
   : jointList(std::move(jointList))
   , urdf_(std::move(urdf))
-  , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime)
+  , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime, ecatSlaveTimeout)
   , pdb_(nullptr)
 {
 }
 
 MarchRobot::MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf,
                        std::unique_ptr<PowerDistributionBoard> powerDistributionBoard, ::std::string ifName,
-                       int ecatCycleTime)
+                       int ecatCycleTime, int ecatSlaveTimeout)
   : jointList(std::move(jointList))
   , urdf_(std::move(urdf))
-  , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime)
+  , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime, ecatSlaveTimeout)
   , pdb_(std::move(powerDistributionBoard))
 {
 }
@@ -156,6 +157,11 @@ bool MarchRobot::isEthercatOperational()
   return ethercatMaster.isOperational();
 }
 
+std::exception_ptr MarchRobot::getLastEthercatException() const noexcept
+{
+  return this->ethercatMaster.getLastException();
+}
+
 void MarchRobot::waitForPdo()
 {
   this->ethercatMaster.waitForPdo();

march_hardware_builder/robots/march4.yaml

@@ -2,6 +2,7 @@
 march4:
   ifName: enp2s0
   ecatCycleTime: 4
+  ecatSlaveTimeout: 50
   joints:
     - left_ankle:
         actuationMode: torque

march_hardware_builder/robots/test_joint_linear.yaml

@@ -1,6 +1,7 @@
 testjoint_linear:
  ifName: enp2s0
  ecatCycleTime: 4
+ ecatSlaveTimeout: 50
  joints:
    - linear_joint:
        actuationMode: torque

march_hardware_builder/src/hardware_builder.cpp

@@ -65,13 +65,15 @@ std::unique_ptr<march::MarchRobot> HardwareBuilder::createMarchRobot()
   YAML::Node config = this->robot_config_[robot_name];
   const auto if_name = config["ifName"].as<std::string>();
   const auto cycle_time = config["ecatCycleTime"].as<int>();
+  const auto slave_timeout = config["ecatSlaveTimeout"].as<int>();
 
   std::vector<march::Joint> joints = this->createJoints(config["joints"], pdo_interface, sdo_interface);
 
   ROS_INFO_STREAM("Robot config:\n" << config);
   YAML::Node pdb_config = config["powerDistributionBoard"];
   auto pdb = HardwareBuilder::createPowerDistributionBoard(pdb_config, pdo_interface, sdo_interface);
-  return std::make_unique<march::MarchRobot>(std::move(joints), this->urdf_, std::move(pdb), if_name, cycle_time);
+  return std::make_unique<march::MarchRobot>(std::move(joints), this->urdf_, std::move(pdb), if_name, cycle_time,
+                                             slave_timeout);
 }
 
 march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::string& joint_name,