Merge branch 'develop'

Author: Olavhaasie

.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;