Merge pull request #187 from project-march/fix/PM-215-fix-destructor-ethercat

destructor ethercat

Author: RutgerVanBeek

march_hardware/include/march_hardware/EtherCAT/EthercatMaster.h

@@ -9,33 +9,41 @@
 
 namespace march4cpp
 {
+/**
+ * Base class of the ethercat master supported with the SOEM library
+ * @param ifname Network interface name, check ifconfig.
+ * @param IOmap Holds the mapping of the SOEM message.
+ * @param expectedWKC The expected working counter of the ethercat train.
+ * @param ecatCycleTimems The ethercat cycle time.
+ * @param maxSlaveIndex The maximum amount of slaves connected to the train.
+ */
 class EthercatMaster
 {
-  std::string ifname;      // Network interface name, check ifconfig
-  char IOmap[4096];        // Holds the mapping of the SOEM message
-  int expectedWKC;         // Expected working counter
-  std::thread EcatThread;  // Handler for parallel thread
+  std::string ifname;
+  char IOmap[4096];
+  int expectedWKC;
 
+  std::thread EcatThread;
   std::vector<Joint>* jointListPtr;
+
   int maxSlaveIndex;
   int ecatCycleTimems;
 
 public:
-  bool isOperational = false;  // Is SOEM in operational state
+  bool isOperational = false;
 
   explicit EthercatMaster(std::vector<Joint>* jointListPtr, std::string ifname, int maxSlaveIndex, int ecatCycleTime);
+  ~EthercatMaster();
 
   void start();
-  void stop();
+  void ethercatMasterInitiation();
+  void ethercatSlaveInitiation();
 
-  // Parallel thread
   void ethercatLoop();
+  void SendReceivePDO();
+  static void monitorSlaveConnection();
 
-  void sendProcessData();
-
-  int receiveProcessData();
-
-  void monitorSlaveConnection();
+  void stop();
 };
 
 }  // namespace march4cpp

march_hardware/src/EtherCAT/EthercatMaster.cpp

@@ -1,8 +1,5 @@
 // Copyright 2019 Project March.
 
-//
-// EtherCAT master class source. Interfaces with SOEM
-//
 #include <string>
 #include <vector>
 
@@ -24,7 +21,6 @@
 
 namespace march4cpp
 {
-// Constructor
 EthercatMaster::EthercatMaster(std::vector<Joint>* jointListPtr, std::string ifname, int maxSlaveIndex,
                                int ecatCycleTime)
   : jointListPtr(jointListPtr)
@@ -34,20 +30,33 @@ EthercatMaster::EthercatMaster(std::vector<Joint>* jointListPtr, std::string ifn
   this->ecatCycleTimems = ecatCycleTime;
 }
 
+EthercatMaster::~EthercatMaster()
+{
+  this->stop();
+}
+
+/**
+ * Initiate the ethercat train and start the loop
+ */
 void EthercatMaster::start()
 {
-  // TODO(Isha, Martijn) this method is really long, split into more methods
-  ROS_INFO("Trying to start EtherCAT");
+  EthercatMaster::ethercatMasterInitiation();
+  EthercatMaster::ethercatSlaveInitiation();
+}
 
-  // Initialise SOEM, bind socket to ifname
+/**
+ * Open the ethernet port with the given ifname and check amount of slaves
+ */
+void EthercatMaster::ethercatMasterInitiation()
+{
+  ROS_INFO("Trying to start EtherCAT");
   if (!ec_init(&ifname[0]))
   {
     ROS_FATAL("No socket connection on %s. Confirm that you have selected the right ifname", ifname.c_str());
     throw std::runtime_error("No socket connection on %s. Confirm that you have selected the right ifname");
   }
   ROS_INFO("ec_init on %s succeeded", ifname.c_str());
 
-  // Find and auto-config slaves
   if (ec_config_init(FALSE) <= 0)
   {
     ROS_FATAL("No slaves found, shutting down. Confirm that you have selected the right ifname.");
@@ -61,37 +70,38 @@ void EthercatMaster::start()
     ROS_FATAL("Slave configured with index %d while soem only found %d slave(s)", this->maxSlaveIndex, ec_slavecount);
     throw std::runtime_error("More slaves configured than soem could detect.");
   }
-  // TODO(Martijn) Check on type of slaves
+}
 
-  // Request and wait for slaves to be in preOP state
+/**
+ * Set the found slaves to pre-operational state, configure the slaves and move to safe-operational state. If everything
+ * went good move to operational state.
+ */
+void EthercatMaster::ethercatSlaveInitiation()
+{
+  ROS_INFO("Request pre-operational state for all slaves");
   ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4);
 
-  for (int i = 0; i < jointListPtr->size(); i++)
+  for (auto& joint : *jointListPtr)
   {
-    jointListPtr->at(i).initialize(ecatCycleTimems);
+    joint.initialize(ecatCycleTimems);
   }
 
-  // Configure the EtherCAT message structure depending on the PDO mapping of all the slaves
   ec_config_map(&IOmap);
-
   ec_configdc();
 
-  // Wait for all slaves to reach SAFE_OP state
+  ROS_INFO("Request safe-operational state for all slaves");
   ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
 
-  ROS_INFO("Request operational state for all slaves");
   expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
   ec_slave[0].state = EC_STATE_OPERATIONAL;
 
-  // send one valid process data to make outputs in slaves happy
   ec_send_processdata();
   ec_receive_processdata(EC_TIMEOUTRET);
 
-  // request OP state for all slaves
+  ROS_INFO("Request operational state for all slaves");
   ec_writestate(0);
   int chk = 40;
 
-  /* wait for all slaves to reach OP state */
   do
   {
     ec_send_processdata();
@@ -101,14 +111,12 @@ void EthercatMaster::start()
 
   if (ec_slave[0].state == EC_STATE_OPERATIONAL)
   {
-    // All slaves in operational state
     ROS_INFO("Operational state reached for all slaves");
     isOperational = true;
     EcatThread = std::thread(&EthercatMaster::ethercatLoop, this);
   }
   else
   {
-    // Not all slaves in operational state
     ROS_FATAL("Not all slaves reached operational state. Non-operational slave(s) listed below.");
     ec_readstate();
     for (int i = 1; i <= ec_slavecount; i++)
@@ -123,29 +131,26 @@ void EthercatMaster::start()
   }
 }
 
-void EthercatMaster::stop()
-{
-  ROS_INFO("Stopping EtherCAT");
-  isOperational = false;
-  EcatThread.join();
-  ec_slave[0].state = EC_STATE_INIT;
-  ec_writestate(0);
-  ec_close();
-}
-
+/**
+ * The ethercat train PDO loop. If the working counter is lower then expected 5% of the time the program displays an
+ * error
+ */
 void EthercatMaster::ethercatLoop()
 {
   uint32_t totalLoops = 0;
   uint32_t rateNotAchievedCount = 0;
   int rate = 1000 / ecatCycleTimems;
+
   while (isOperational)
   {
     auto start = boost::chrono::high_resolution_clock::now();
-    sendProcessData();
-    receiveProcessData();
+
+    SendReceivePDO();
     monitorSlaveConnection();
+
     auto stop = boost::chrono::high_resolution_clock::now();
     auto duration = boost::chrono::duration_cast<boost::chrono::microseconds>(stop - start);
+
     if (duration.count() > ecatCycleTimems * 1000)
     {
       rateNotAchievedCount++;
@@ -155,10 +160,11 @@ void EthercatMaster::ethercatLoop()
       usleep(ecatCycleTimems * 1000 - duration.count());
     }
     totalLoops++;
-    if (totalLoops >= 10 * rate)  // Every 10 seconds
+
+    if (totalLoops >= (10 * rate))
     {
       float rateNotAchievedPercentage = 100 * (static_cast<float>(rateNotAchievedCount) / totalLoops);
-      if (rateNotAchievedPercentage > 5)  // If percentage greater than 5 percent, do ROS_WARN instead of ROS_DEBUG
+      if (rateNotAchievedPercentage > 5)
       {
         ROS_WARN("EtherCAT rate of %d milliseconds per cycle was not achieved for %f percent of all cycles",
                  ecatCycleTimems, rateNotAchievedPercentage);
@@ -174,33 +180,50 @@ void EthercatMaster::ethercatLoop()
   }
 }
 
-void EthercatMaster::sendProcessData()
+/**
+ * Send the PDO and receive the working counter and check if this is lower then expected.
+ */
+void EthercatMaster::SendReceivePDO()
 {
   ec_send_processdata();
-}
-
-int EthercatMaster::receiveProcessData()
-{
   int wkc = ec_receive_processdata(EC_TIMEOUTRET);
   if (wkc < this->expectedWKC)
   {
     ROS_WARN_THROTTLE(1, "Working counter lower than expected. EtherCAT connection may not be optimal");
   }
-  return wkc;
 }
 
+/**
+ * Check if all the slaves are connected and in operational state.
+ */
 void EthercatMaster::monitorSlaveConnection()
 {
   for (int slave = 1; slave <= ec_slavecount; slave++)
   {
     ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
     if (!ec_slave[slave].state)
     {
-      // TODO(@Tim, @Isha, @Martijn) throw error when it happens multiple times in a short period of time.
       ROS_WARN_THROTTLE(1, "EtherCAT train lost connection from slave %d onwards", slave);
       return;
     }
   }
 }
 
+/**
+ * Stop the ethercat loop and terminate the thread
+ */
+void EthercatMaster::stop()
+{
+  if (this->isOperational)
+  {
+    ROS_INFO("Stopping EtherCAT");
+    isOperational = false;
+    EcatThread.join();
+
+    ec_slave[0].state = EC_STATE_INIT;
+    ec_writestate(0);
+    ec_close();
+  }
+}
+
 }  // namespace march4cpp

march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h

@@ -36,19 +36,18 @@ class MarchHardwareInterface : public march_hardware_interface::MarchHardware
 {
 public:
   MarchHardwareInterface(ros::NodeHandle& nh, AllowedRobot robotName);
-  ~MarchHardwareInterface();
 
   /**
    * @brief Initialize the HardwareInterface by registering position interfaces
    * for each joint.
    */
   void init();
-  void update(const ros::TimerEvent& e);
+  void update(const ros::Duration& elapsed_time);
 
   /**
    * @brief Read actual postion from the hardware.
    */
-  void read(ros::Duration elapsed_time = ros::Duration(0.01));
+  void read(const ros::Duration& elapsed_time = ros::Duration(0.01));
 
   /**
    * @brief Perform all safety checks that might crash the exoskeleton.
@@ -59,18 +58,16 @@ class MarchHardwareInterface : public march_hardware_interface::MarchHardware
    * @brief Write position commands to the hardware.
    * @param elapsed_time Duration since last write action
    */
-  void write(ros::Duration elapsed_time);
+  void write(const ros::Duration& elapsed_time);
 
 protected:
   ::march4cpp::MarchRobot marchRobot;
   ros::NodeHandle nh_;
-  ros::Timer non_realtime_loop_;
   ros::Duration control_period_;
   ros::Duration elapsed_time_;
   PositionJointInterface positionJointInterface;
   PositionJointSoftLimitsInterface positionJointSoftLimitsInterface;
   EffortJointSoftLimitsInterface effortJointSoftLimitsInterface;
-  double loop_hz_;
   bool hasPowerDistributionBoard = false;
   boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
   typedef boost::shared_ptr<realtime_tools::RealtimePublisher<march_shared_resources::ImcErrorState> > RtPublisherPtr;

march_hardware_interface/src/march_hardware_interface.cpp

@@ -29,16 +29,6 @@ namespace march_hardware_interface
 MarchHardwareInterface::MarchHardwareInterface(ros::NodeHandle& nh, AllowedRobot robotName)
   : nh_(nh), marchRobot(HardwareBuilder(robotName).createMarchRobot())
 {
-  init();
-  controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
-  nh_.param("/march/hardware_interface/loop_hz", loop_hz_, 100.0);
-  ros::Duration update_freq = ros::Duration(1.0 / loop_hz_);
-  non_realtime_loop_ = nh_.createTimer(update_freq, &MarchHardwareInterface::update, this);
-}
-
-MarchHardwareInterface::~MarchHardwareInterface()
-{
-  this->marchRobot.stopEtherCAT();
 }
 
 void MarchHardwareInterface::init()
@@ -218,15 +208,15 @@ void MarchHardwareInterface::init()
     }
   }
   ROS_INFO("Successfully actuated all joints");
+  controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
 }
 
-void MarchHardwareInterface::update(const ros::TimerEvent& e)
+void MarchHardwareInterface::update(const ros::Duration& elapsed_time)
 {
-  elapsed_time_ = ros::Duration(e.current_real - e.last_real);
-  read(elapsed_time_);
+  read(elapsed_time);
   validate();
-  controller_manager_->update(ros::Time::now(), elapsed_time_);
-  write(elapsed_time_);
+  controller_manager_->update(ros::Time::now(), elapsed_time);
+  write(elapsed_time);
 }
 
 void MarchHardwareInterface::validate()
@@ -238,7 +228,7 @@ void MarchHardwareInterface::validate()
   }
 }
 
-void MarchHardwareInterface::read(ros::Duration elapsed_time)
+void MarchHardwareInterface::read(const ros::Duration& elapsed_time)
 {
   for (int i = 0; i < num_joints_; i++)
   {
@@ -278,7 +268,7 @@ void MarchHardwareInterface::read(ros::Duration elapsed_time)
   }
 }
 
-void MarchHardwareInterface::write(ros::Duration elapsed_time)
+void MarchHardwareInterface::write(const ros::Duration& elapsed_time)
 {
   joint_effort_command_copy.clear();
   joint_effort_command_copy.resize(joint_effort_command_.size());