diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h index 82c21709b90071a112ee8b86c43a0f4fb1ff31b1..ffb694ab40b87caf32c05d0f761f0bc1e50dd7fe 100644 --- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h +++ b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h @@ -3,6 +3,7 @@ #include <stdint.h> #include <memory> #include <ArmarXCore/core/logging/Logging.h> +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> #include "SlaveIdentifier.h" @@ -103,5 +104,59 @@ namespace armarx }; + template<class InputT, class OutputT> + class AbstractSlaveWithInputOutput : public AbstractSlave + { + public: + using AbstractSlave::AbstractSlave; + + void setInputPDO(void *ptr) override + { + const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr); + ARMARX_CHECK_EXPRESSION_W_HINT( + (ptrAsInt % alignof(InputT)) == 0, + "\nThe alignment is wrong!\nIt has to be " + << alignof(InputT) << ", but the data is aligned with " + << ptrAsInt % alignof(std::max_align_t) + << "!\nThis is an offset of " << (ptrAsInt % alignof(InputT)) + << " bytes!\nThe datatype is " << GetTypeString<InputT>() + << "\nIts size is " << sizeof(InputT) + << " bytes" + ); + inputs = static_cast<InputT*>(ptr); + } + void setOutputPDO(void *ptr) override + { + const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr); + ARMARX_CHECK_EXPRESSION_W_HINT( + (ptrAsInt % alignof(OutputT)) == 0, + "\nThe alignment is wrong!\nIt has to be " + << alignof(OutputT) << ", but the data is aligned with " + << ptrAsInt % alignof(std::max_align_t) + << "!\nThis is an offset of " << (ptrAsInt % alignof(OutputT)) + << " bytes!\nThe datatype is " << GetTypeString<OutputT>() + << "\nIts size is " << sizeof(OutputT) + << " bytes" + ); + outputs = static_cast<OutputT*>(ptr); + } + bool hasPDOMapping() const final override + { + return true; + } + OutputT* getOutputsPtr() + { + return outputs; + } + InputT* getInputsPtr() + { + return inputs; + } + protected: + InputT* inputs{nullptr}; + OutputT* outputs; + + }; + } diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp index 2387c60e07bbf8d54dded3d7c072cb57fc16ca09..1eb574a8fa6c83d7d12039d9aca2030a5bf35aec 100644 --- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp +++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoard.cpp @@ -84,7 +84,7 @@ namespace armarx void KITGripperBasisBoard::initData() { dataPtr = std::make_shared<KITGripperBasisBoardData>(configNode, defaultConfigurationNode, - slave->getOutputs(), slave->getInputs(), robot); + slave->getOutputsPtr(), slave->getInputsPtr(), robot); for (auto motorNode : configNode.nodes("Actor")) { diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp index 4a3eec9b1a7a199284e38a93998d54bdb4812da2..fa31a5a3c225a0356f9ee3852584e4b1bf551c8d 100644 --- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp +++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.cpp @@ -34,7 +34,7 @@ namespace armarx KITGripperBasisBoardSlave::KITGripperBasisBoardSlave(const SlaveIdentifier slaveIdentifier, uint16_t slaveNumber) : - AbstractSlave(slaveIdentifier, slaveNumber) + AbstractSlaveWithInputOutput(slaveIdentifier, slaveNumber) { setTag("KITGripperBasisBoardSlave_" + slaveIdentifier.humanName); } @@ -63,16 +63,6 @@ namespace armarx return true; } - void KITGripperBasisBoardSlave::setInputPDO(void* ptr) - { - inputs = static_cast<KITGripperBasisBoardIN_t*>(ptr); - } - - void KITGripperBasisBoardSlave::setOutputPDO(void* ptr) - { - outputs = static_cast<KITGripperBasisBoardOUT_t*>(ptr); - } - void KITGripperBasisBoardSlave::prepareForOp() { } @@ -82,22 +72,6 @@ namespace armarx return false; } - bool KITGripperBasisBoardSlave::hasPDOMapping() const - { - return true; - } - - KITGripperBasisBoardIN_t* KITGripperBasisBoardSlave::getInputs() const - { - return inputs; - } - - KITGripperBasisBoardOUT_t* KITGripperBasisBoardSlave::getOutputs() const - { - return outputs; - } - - /** * register this class in the super class factory */ diff --git a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h index 54b92e0faf49f7a5c2d8b9408b5a4028c6bf46c1..7966df46e1af9cc7a3c1c16037483c4cf27ab24c 100644 --- a/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h +++ b/source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/KITGripperBasisBoardSlave.h @@ -95,7 +95,8 @@ namespace armarx typedef std::shared_ptr<KITGripperBasisBoardSlave> KITGripperBasisBoardSlavePtr; - class KITGripperBasisBoardSlave : public AbstractSlave + class KITGripperBasisBoardSlave : + public AbstractSlaveWithInputOutput<KITGripperBasisBoardIN_t, KITGripperBasisBoardOUT_t> { public: KITGripperBasisBoardSlave(const armarx::SlaveIdentifier slaveIdentifier, uint16_t slaveNumber); @@ -106,18 +107,8 @@ namespace armarx bool prepare() override; void execute() override; bool shutdown() override; - void setInputPDO(void* ptr) override; - void setOutputPDO(void* ptr) override; void prepareForOp() override; bool hasError() override; - bool hasPDOMapping() const override; - KITGripperBasisBoardIN_t* getInputs() const; - - KITGripperBasisBoardOUT_t* getOutputs() const; - - protected: - KITGripperBasisBoardIN_t* inputs; - KITGripperBasisBoardOUT_t* outputs; }; class KITGripperBasisBoardFactory : public EtherCATDeviceFactory