Merge branch 'develop'

.travis.yml

@@ -33,7 +33,7 @@ env:
     - CI_ENV=`bash <(curl -s https://codecov.io/env)`
     - DOCKER_RUN_OPTS='$CI_ENV'
     - TARGET_CMAKE_ARGS='-DCMAKE_BUILD_TYPE=Debug -DENABLE_COVERAGE_TESTING=ON'
-    - UPSTREAM_WORKSPACE='.rosinstall -march/march_data_collector -march/march_gait_scheduler -march/march_gain_scheduling -march/march_gait_selection -march/march_launch -march/march_safety -march/march_state_machine'
+    - UPSTREAM_WORKSPACE='.rosinstall -march/march_data_collector -march/march_gain_scheduling -march/march_gait_scheduler -march/march_gait_selection -march/march_launch -march/march_moveit -march/march_safety -march/march_shared_classes -march/march_state_machine'
     - AFTER_RUN_TARGET_TEST='cd "$target_ws" && bash <(curl -s https://codecov.io/bash) -R src/hardware-interface -F test && bash <(curl -s https://codecov.io/bash) -R src/hardware-interface -F production'
 
 jobs:

march_hardware/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(march_hardware)
 
 add_compile_options(-std=c++14 -Wall -Wextra -Werror)
@@ -12,6 +12,9 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
     INCLUDE_DIRS include
     CATKIN_DEPENDS
+    roscpp
+    soem
+    urdf
     LIBRARIES ${PROJECT_NAME}
     CFG_EXTRAS
     ${PROJECT_NAME}-extras.cmake
@@ -93,10 +96,6 @@ install(DIRECTORY include/${PROJECT_NAME}/
     DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
-install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-
 install(TARGETS ${PROJECT_NAME}
     ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

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/error/motion_error.h

@@ -9,15 +9,23 @@ namespace march
 {
 namespace error
 {
+enum class ErrorRegisters
+{
+  MOTION_ERROR,
+  DETAILED_ERROR,
+  SECOND_DETAILED_ERROR
+};
+
 const size_t MOTION_ERRORS_SIZE = 16;
 extern const char* MOTION_ERRORS[MOTION_ERRORS_SIZE];
 
 const size_t DETAILED_MOTION_ERRORS_SIZE = 9;
 extern const char* DETAILED_MOTION_ERRORS[DETAILED_MOTION_ERRORS_SIZE];
 
-std::string parseMotionError(uint16_t motion_error);
+const size_t SECOND_DETAILED_MOTION_ERROR_SIZE = 7;
+extern const char* SECOND_DETAILED_MOTION_ERRORS[SECOND_DETAILED_MOTION_ERROR_SIZE];
 
-std::string parseDetailedError(uint16_t detailed_error);
+std::string parseError(uint16_t error, ErrorRegisters);
 
 }  // namespace error
 }  // namespace march

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/ethercat/pdo_map.h

@@ -47,6 +47,7 @@ enum class IMCObjectName
   ActualVelocity,
   MotionErrorRegister,
   DetailedErrorRegister,
+  SecondDetailedErrorRegister,
   DCLinkVoltage,
   DriveTemperature,
   ActualTorque,

march_hardware/include/march_hardware/imotioncube/imotioncube.h

@@ -57,6 +57,7 @@ class IMotionCube : public Slave
   uint16_t getStatusWord();
   uint16_t getMotionError();
   uint16_t getDetailedError();
+  uint16_t getSecondDetailedError();
 
   ActuationMode getActuationMode() const;
 
@@ -120,7 +121,7 @@ class IMotionCube : public Slave
    * @param end_address the found end address of the setup in the .sw file
    * @return the computed checksum in the .sw file
    */
-  uint32_t computeSWCheckSum(int& start_address, int& end_address);
+  uint16_t computeSWCheckSum(uint16_t& start_address, uint16_t& end_address);
   /**
    * Compares the checksum of the .sw file and the setup on the drive. If both are equal 1 is returned.
    * This method makes use of the computeSWCheckSum(int&, int&) method. The start and end addresses are used in

march_hardware/include/march_hardware/imotioncube/imotioncube_state.h

@@ -125,11 +125,14 @@ struct IMotionCubeState
   IMotionCubeState() = default;
 
   std::string statusWord;
-  std::string detailedError;
   std::string motionError;
+  std::string detailedError;
+  std::string secondDetailedError;
   IMCState state;
   std::string detailedErrorDescription;
   std::string motionErrorDescription;
+  std::string secondDetailedErrorDescription;
+
   float motorCurrent;
   float IMCVoltage;
   float motorVoltage;

march_hardware/include/march_hardware/joint.h

@@ -55,6 +55,7 @@ class Joint
 
   double getPosition() const;
   double getVelocity() const;
+  double getVoltageVelocity() const;
   double getIncrementalPosition() const;
   double getAbsolutePosition() const;
   int16_t getTorque();

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/package.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>march_hardware</name>
   <version>0.0.0</version>
   <description>

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/error/motion_error.cpp

@@ -36,29 +36,39 @@ const char* DETAILED_MOTION_ERRORS[DETAILED_MOTION_ERRORS_SIZE] = {
   "Invalid S-curve profile. ",
 };
 
-std::string parseMotionError(uint16_t motion_error)
-{
-  std::string description;
-  const std::bitset<16> bitset(motion_error);
-  for (size_t i = 0; i < MOTION_ERRORS_SIZE; i++)
-  {
-    if (bitset.test(i))
-    {
-      description += MOTION_ERRORS[i];
-    }
-  }
-  return description;
-}
+const char* SECOND_DETAILED_MOTION_ERRORS[SECOND_DETAILED_MOTION_ERROR_SIZE] = {
+  "BiSS data CRC error. ",
+  "BiSS data warning bit is set. ",
+  "BiSS data error bit is set. ",
+  "BiSS sensor missing. ",
+  "Absolute Encoder Interface (AEI) interface error. ",
+  "Hall sensor missing. ",
+  "Position wraparound. The position 2^31 was exceeded. ",
+};
 
-std::string parseDetailedError(uint16_t detailed_error)
+std::string parseError(uint16_t error, ErrorRegisters error_register)
 {
   std::string description;
-  const std::bitset<16> bitset(detailed_error);
-  for (size_t i = 0; i < DETAILED_MOTION_ERRORS_SIZE; i++)
+  const std::bitset<16> bitset(error);
+
+  for (size_t i = 0; i < 16; i++)
   {
     if (bitset.test(i))
     {
-      description += DETAILED_MOTION_ERRORS[i];
+      switch (error_register)
+      {
+        case ErrorRegisters::MOTION_ERROR:
+          description += MOTION_ERRORS[i];
+          break;
+        case ErrorRegisters::DETAILED_ERROR:
+          description += DETAILED_MOTION_ERRORS[i];
+          break;
+        case ErrorRegisters::SECOND_DETAILED_ERROR:
+          description += SECOND_DETAILED_MOTION_ERRORS[i];
+          break;
+        default:
+          break;
+      }
     }
   }
   return description;