Feature/pm 524 Generalize motor controller

march_hardware/CMakeLists.txt

@@ -53,10 +53,13 @@ add_library(${PROJECT_NAME}
     include/${PROJECT_NAME}/ethercat/pdo_types.h
     include/${PROJECT_NAME}/ethercat/sdo_interface.h
     include/${PROJECT_NAME}/ethercat/slave.h
-    include/${PROJECT_NAME}/imotioncube/actuation_mode.h
-    include/${PROJECT_NAME}/imotioncube/imotioncube.h
-    include/${PROJECT_NAME}/imotioncube/imotioncube_state.h
-    include/${PROJECT_NAME}/imotioncube/imotioncube_target_state.h
+    include/${PROJECT_NAME}/motor_controller/actuation_mode.h
+    include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube.h
+    include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_states.h
+    include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_state_of_operation.h
+    include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_target_state.h
+    include/${PROJECT_NAME}/motor_controller/motor_controller.h
+    include/${PROJECT_NAME}/motor_controller/motor_controller_states.h
     include/${PROJECT_NAME}/joint.h
     include/${PROJECT_NAME}/march_robot.h
     include/${PROJECT_NAME}/power/boot_shutdown_offsets.h
@@ -123,7 +126,7 @@ if(CATKIN_ENABLE_TESTING)
         test/joint_test.cpp
         test/mocks/mock_absolute_encoder.h
         test/mocks/mock_encoder.h
-        test/mocks/mock_imotioncube.h
+        test/mocks/mock_motor_controller.h
         test/mocks/mock_incremental_encoder.h
         test/mocks/mock_joint.h
         test/mocks/mock_pdo_interface.h

march_hardware/include/march_hardware/ethercat/ethercat_master.h

@@ -24,7 +24,7 @@ namespace march
 class EthercatMaster
 {
 public:
-  EthercatMaster(std::string ifname, int max_slave_index, int cycle_time, int slave_timeout);
+  EthercatMaster(std::string ifname, std::vector<std::shared_ptr<Slave>>, int cycle_time, int slave_timeout);
   ~EthercatMaster();
 
   /* Delete copy constructor/assignment since the member thread can not be copied */
@@ -45,12 +45,20 @@ class EthercatMaster
    */
   int getCycleTime() const;
 
+  /**
+   * Returns the largest slave index.
+   */
+  int getMaxSlaveIndex();
+
+  bool hasValidSlaves();
+
   /**
    * Initializes the ethercat train and starts a thread for the loop.
    * @throws HardwareException If not the configured amount of slaves was found
    *                           or they did not all reach operational state
+   * @returns true if one of the slaves requires resetting ethercat, false otherwise
    */
-  bool start(std::vector<Joint>& joints);
+  bool start();
 
   /**
    * Stops the ethercat loop and joins the thread.
@@ -67,8 +75,10 @@ class EthercatMaster
 
   /**
    * Configures the found slaves to operational state.
+   *
+   * @returns true if one of the slaves requires resetting ethercat, false otherwise
    */
-  bool ethercatSlaveInitiation(std::vector<Joint>& joints);
+  bool ethercatSlaveInitiation();
 
   /**
    * The ethercat train PDO loop. If the working counter is lower than
@@ -112,6 +122,7 @@ class EthercatMaster
   std::atomic<bool> is_operational_;
 
   const std::string ifname_;
+  std::vector<std::shared_ptr<Slave>> slave_list_;
   const int max_slave_index_;
   const int cycle_time_ms_;
 

march_hardware/include/march_hardware/ethercat/slave.h

@@ -48,6 +48,11 @@ class Slave : public PdoSlaveInterface
     this->reset(sdo_slave_interface);
   }
 
+  virtual bool hasWatchdog()
+  {
+    return false;
+  }
+
 protected:
   virtual bool initSdo(SdoSlaveInterface& /* sdo */, int /* cycle_time */)
   {

march_hardware/include/march_hardware/joint.h

@@ -7,10 +7,10 @@
 #include <utility>
 #include <vector>
 
-#include <march_hardware/imotioncube/imotioncube.h>
+#include <march_hardware/motor_controller/imotioncube/imotioncube.h>
 #include <march_hardware/power/power_distribution_board.h>
 #include <march_hardware/temperature/temperature_ges.h>
-#include <march_hardware/imotioncube/imotioncube_state.h>
+#include <march_hardware/motor_controller/motor_controller_states.h>
 
 namespace march
 {
@@ -26,107 +26,57 @@ class Joint
   /**
    * Initializes a Joint with motor controller and without temperature slave.
    */
-  Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr<IMotionCube> imc);
+  Joint(std::string name, int net_number, bool allow_actuation, std::shared_ptr<MotorController> controller);
 
   /**
    * Initializes a Joint with motor controller and temperature slave.
    */
-  Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr<IMotionCube> imc,
-        std::unique_ptr<TemperatureGES> temperature_ges);
+  Joint(std::string name, int net_number, bool allow_actuation, std::shared_ptr<MotorController> controller,
+        std::shared_ptr<TemperatureGES> temperature_ges);
 
   virtual ~Joint() noexcept = default;
 
-  /* Delete copy constructor/assignment since the unique_ptr cannot be copied */
+  /* Delete copy constructor/assignment since the shared_ptr cannot be copied */
   Joint(const Joint&) = delete;
   Joint& operator=(const Joint&) = delete;
 
-  void resetIMotionCube();
+  void resetMotorController();
 
   /* Delete move assignment since string cannot be move assigned */
   Joint(Joint&&) = default;
   Joint& operator=(Joint&&) = delete;
 
-  bool initialize(int cycle_time);
   void prepareActuation();
 
   void actuateRad(double target_position);
-  void actuateTorque(int16_t target_torque);
+  void actuateTorque(double target_torque);
   void readEncoders(const ros::Duration& elapsed_time);
 
   double getPosition() const;
   double getVelocity() const;
   double getVoltageVelocity() const;
   double getIncrementalPosition() const;
   double getAbsolutePosition() const;
-  int16_t getTorque();
-  int32_t getAngleIUAbsolute();
-  int32_t getAngleIUIncremental();
-  double getVelocityIUAbsolute();
-  double getVelocityIUIncremental();
+  double getTorque();
   float getTemperature();
-  IMotionCubeState getIMotionCubeState();
+  std::unique_ptr<MotorControllerStates> getMotorControllerStates();
 
   std::string getName() const;
-  int getTemperatureGESSlaveIndex() const;
-  int getIMotionCubeSlaveIndex() const;
   int getNetNumber() const;
 
   ActuationMode getActuationMode() const;
 
-  bool hasIMotionCube() const;
+  bool hasMotorController() const;
   bool hasTemperatureGES() const;
   bool canActuate() const;
   bool receivedDataUpdate();
   void setAllowActuation(bool allow_actuation);
 
-  /** @brief Override comparison operator */
-  friend bool operator==(const Joint& lhs, const Joint& rhs)
-  {
-    return lhs.name_ == rhs.name_ && ((lhs.imc_ && rhs.imc_ && *lhs.imc_ == *rhs.imc_) || (!lhs.imc_ && !rhs.imc_)) &&
-           ((lhs.temperature_ges_ && rhs.temperature_ges_ && *lhs.temperature_ges_ == *rhs.temperature_ges_) ||
-            (!lhs.temperature_ges_ && !rhs.temperature_ges_)) &&
-           lhs.allow_actuation_ == rhs.allow_actuation_ &&
-           lhs.getActuationMode().getValue() == rhs.getActuationMode().getValue();
-  }
-
-  friend bool operator!=(const Joint& lhs, const Joint& rhs)
-  {
-    return !(lhs == rhs);
-  }
-  /** @brief Override stream operator for clean printing */
-  friend ::std::ostream& operator<<(std::ostream& os, const Joint& joint)
-  {
-    os << "name: " << joint.name_ << ", "
-       << "ActuationMode: " << joint.getActuationMode().toString() << ", "
-       << "allowActuation: " << joint.allow_actuation_ << ", "
-       << "imotioncube: ";
-    if (joint.imc_)
-    {
-      os << *joint.imc_;
-    }
-    else
-    {
-      os << "none";
-    }
-
-    os << ", temperatureges: ";
-    if (joint.temperature_ges_)
-    {
-      os << *joint.temperature_ges_;
-    }
-    else
-    {
-      os << "none";
-    }
-
-    return os;
-  }
-
 private:
   const std::string name_;
   const int net_number_;
   bool allow_actuation_ = false;
-  float previous_imc_volt_ = 0.0;
+  float previous_controller_volt_ = 0.0;
   float previous_motor_current_ = 0.0;
   float previous_motor_volt_ = 0.0;
   double previous_absolute_position_ = 0.0;
@@ -139,8 +89,8 @@ class Joint
   double absolute_position_ = 0.0;
   double velocity_ = 0.0;
 
-  std::unique_ptr<IMotionCube> imc_ = nullptr;
-  std::unique_ptr<TemperatureGES> temperature_ges_ = nullptr;
+  std::shared_ptr<MotorController> controller_ = nullptr;
+  std::shared_ptr<TemperatureGES> temperature_ges_ = nullptr;
 };
 
 }  // namespace march

march_hardware/include/march_hardware/march_robot.h

@@ -21,17 +21,17 @@ class MarchRobot
   ::std::vector<Joint> jointList;
   urdf::Model urdf_;
   EthercatMaster ethercatMaster;
-  std::unique_ptr<PowerDistributionBoard> pdb_;
+  std::shared_ptr<PowerDistributionBoard> pdb_;
 
 public:
   using iterator = std::vector<Joint>::iterator;
 
-  MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime,
-             int ecatSlaveTimeout);
+  MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf, ::std::string ifName,
+             std::vector<std::shared_ptr<Slave>> slave_list, int ecatCycleTime, int ecatSlaveTimeout);
 
   MarchRobot(::std::vector<Joint> jointList, urdf::Model urdf,
-             std::unique_ptr<PowerDistributionBoard> powerDistributionBoard, ::std::string ifName, int ecatCycleTime,
-             int ecatSlaveTimeout);
+             std::shared_ptr<PowerDistributionBoard> powerDistributionBoard, ::std::string ifName,
+             std::vector<std::shared_ptr<Slave>> slave_list, int ecatCycleTime, int ecatSlaveTimeout);
 
   ~MarchRobot();
 
@@ -43,16 +43,14 @@ class MarchRobot
   MarchRobot(MarchRobot&&) = delete;
   MarchRobot& operator=(MarchRobot&&) = delete;
 
-  void resetIMotionCubes();
+  void resetMotorControllers();
 
-  void startEtherCAT(bool reset_imc);
+  void startEtherCAT(bool reset_motor_controllers);
 
   void stopEtherCAT();
 
   int getMaxSlaveIndex();
 
-  bool hasValidSlaves();
-
   bool isEthercatOperational();
 
   std::exception_ptr getLastEthercatException() const noexcept;
@@ -74,35 +72,6 @@ class MarchRobot
   PowerDistributionBoard* getPowerDistributionBoard() const;
 
   const urdf::Model& getUrdf() const;
-
-  /** @brief Override comparison operator */
-  friend bool operator==(const MarchRobot& lhs, const MarchRobot& rhs)
-  {
-    if (lhs.jointList.size() != rhs.jointList.size())
-    {
-      return false;
-    }
-    for (unsigned int i = 0; i < lhs.jointList.size(); i++)
-    {
-      const march::Joint& lhsJoint = lhs.jointList.at(i);
-      const march::Joint& rhsJoint = rhs.jointList.at(i);
-      if (lhsJoint != rhsJoint)
-      {
-        return false;
-      }
-    }
-    return true;
-  }
-
-  /** @brief Override stream operator for clean printing */
-  friend ::std::ostream& operator<<(std::ostream& os, const MarchRobot& marchRobot)
-  {
-    for (unsigned int i = 0; i < marchRobot.jointList.size(); i++)
-    {
-      os << marchRobot.jointList.at(i) << "\n";
-    }
-    return os;
-  }
 };
 }  // namespace march
 #endif  // MARCH_HARDWARE_MARCH_ROBOT_H