diff --git a/hardware_interface/include/hardware_interface/actuator_state_interface.h b/hardware_interface/include/hardware_interface/actuator_state_interface.h index 9c23808..722f2b1 100644 --- a/hardware_interface/include/hardware_interface/actuator_state_interface.h +++ b/hardware_interface/include/hardware_interface/actuator_state_interface.h @@ -36,50 +36,126 @@ namespace hardware_interface { +class PositionActuatorStateHandle +{ + public: + PositionActuatorStateHandle() = default; + + PositionActuatorStateHandle(const double* pos) : pos_(pos) + { + if (!pos) + { + throw HardwareInterfaceException("Position data pointer is null."); + } + } + + virtual double getPosition() const + { + assert(pos_); + return *pos_; + } + + virtual const double* getPositionPtr() const + { + return pos_; + } + + virtual std::string getName() const = 0; + + protected: + const double* pos_ = nullptr; +}; + +class VelocityActuatorStateHandle +{ + public: + VelocityActuatorStateHandle() = default; + + VelocityActuatorStateHandle(const double* vel) : vel_(vel) + { + if (!vel) + { + throw HardwareInterfaceException("Velocity data pointer is null."); + } + } + + virtual double getVelocity() const + { + assert(vel_); + return *vel_; + } + + virtual const double* getVelocityPtr() const + { + return vel_; + } + + virtual std::string getName() const = 0; + + protected: + const double* vel_ = nullptr; +}; + +class EffortActuatorStateHandle +{ + public: + EffortActuatorStateHandle() = default; + + EffortActuatorStateHandle(const double* eff) : eff_(eff) + { + if (!eff) + { + throw HardwareInterfaceException("Effort data pointer is null."); + } + } + + virtual double getEffort() const + { + assert(eff_); + return *eff_; + } + + virtual const double* getEffortPtr() const + { + return eff_; + } + + virtual std::string getName() const = 0; + + protected: + const double* eff_ = nullptr; +}; /** A handle used to read the state of a single actuator. */ -class ActuatorStateHandle +class ActuatorStateHandle : public PositionActuatorStateHandle, + public VelocityActuatorStateHandle, + public EffortActuatorStateHandle { -public: - ActuatorStateHandle() : name_(), pos_(0), vel_(0), eff_(0) {} - - /** - * \param name The name of the actuator - * \param pos A pointer to the storage for this actuator's position - * \param vel A pointer to the storage for this actuator's velocity - * \param eff A pointer to the storage for this actuator's effort (force or torque) - */ - ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) - : name_(name), pos_(pos), vel_(vel), eff_(eff) - { - if (!pos) + public: + ActuatorStateHandle() = default; + + /** + * \param name The name of the actuator + * \param pos A pointer to the storage for this actuator's position + * \param vel A pointer to the storage for this actuator's velocity + * \param eff A pointer to the storage for this actuator's effort (force or torque) + */ + ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) + try : PositionActuatorStateHandle(pos), VelocityActuatorStateHandle(vel), EffortActuatorStateHandle(eff), name_(name) { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); } - if (!vel) + catch(const HardwareInterfaceException& ex) { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + throw HardwareInterfaceException("Cannot create handle '" + name + "'. " + ex.what()); } - if (!eff) + + std::string getName() const { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + return name_; } - } - - std::string getName() const {return name_;} - double getPosition() const {assert(pos_); return *pos_;} - double getVelocity() const {assert(vel_); return *vel_;} - double getEffort() const {assert(eff_); return *eff_;} - - const double* getPositionPtr() const {return pos_;} - const double* getVelocityPtr() const {return vel_;} - const double* getEffortPtr() const {return eff_;} - -private: - std::string name_; - const double* pos_; - const double* vel_; - const double* eff_; + + private: + std::string name_ = ""; }; /** \brief Hardware interface to support reading the state of an array of actuators @@ -89,8 +165,9 @@ class ActuatorStateHandle * torque). * */ -class ActuatorStateInterface : public HardwareResourceManager {}; - +class ActuatorStateInterface : public HardwareResourceManager +{ +}; } #endif