diff --git a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
index 05c70890cbb232fe8911e2ac73c7f120c1a5b825..38e8ccac55917f9eef81a3c27c601f089ce444c0 100644
--- a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
+++ b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
@@ -40,18 +40,18 @@ namespace armarx
template <typename T>
struct AtomicWrapper
{
- std::atomic<T> val;
-
- AtomicWrapper( ):val{ }{}
- AtomicWrapper(const T& v ):val{v }{}
- AtomicWrapper(const std::atomic<T> &val ):val{val.load() }{}
- AtomicWrapper(const AtomicWrapper &other):val{other.val.load()}{}
-
- AtomicWrapper &operator=(const AtomicWrapper &other)
- {
- val.store(other.val.load());
- return *this;
- }
+ std::atomic<T> val;
+
+ AtomicWrapper(): val { } {}
+ AtomicWrapper(const T& v): val {v } {}
+ AtomicWrapper(const std::atomic<T>& val): val {val.load() } {}
+ AtomicWrapper(const AtomicWrapper& other): val {other.val.load()} {}
+
+ AtomicWrapper& operator=(const AtomicWrapper& other)
+ {
+ val.store(other.val.load());
+ return *this;
+ }
};
template <typename TargetType>
@@ -65,7 +65,7 @@ namespace armarx
public:
using Traits = PassThroughControllerTargetTypeTraits<TargetType>;
- inline PassThroughController(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config);
+ inline PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
inline virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
@@ -73,8 +73,8 @@ namespace armarx
//ice interface
inline virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override;
- inline virtual void setJoint(const std::string& name, Ice::Float value, const Ice::Current & = GlobalIceCurrent) override;
- inline virtual void setJoints(const StringFloatDictionary& values, const Ice::Current & = GlobalIceCurrent) override;
+ inline virtual void setJoint(const std::string& name, Ice::Float value, const Ice::Current& = GlobalIceCurrent) override;
+ inline virtual void setJoints(const StringFloatDictionary& values, const Ice::Current& = GlobalIceCurrent) override;
protected:
std::vector<float*> lvl0Targets;
std::vector<const float*> jointStates;
@@ -136,7 +136,7 @@ namespace armarx
LVL1ControllerRegistration<PassThroughController<JointPositionTarget>> registrationSomeControllerPositionPassThroughController("PositionPassThroughController");
LVL1ControllerRegistration<PassThroughController<JointVelocityTarget>> registrationSomeControllerVelocityPassThroughController("VelocityPassThroughController");
- LVL1ControllerRegistration<PassThroughController<JointTorqueTarget >> registrationSomeControllerJointPassThroughController ("JointPassThroughController");
+ LVL1ControllerRegistration<PassThroughController<JointTorqueTarget >> registrationSomeControllerJointPassThroughController("JointPassThroughController");
template <typename TargetType>
std::string PassThroughController<TargetType>::getClassName(const Ice::Current&) const
@@ -145,7 +145,7 @@ namespace armarx
}
template <typename TargetType>
- PassThroughController<TargetType>::PassThroughController(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)
+ PassThroughController<TargetType>::PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
{
PassThroughControllerConfigPtr cfg = PassThroughControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
@@ -153,19 +153,19 @@ namespace armarx
<< " instead of " << PassThroughControllerConfig::ice_staticId());
//make sure the used units are supported
- auto kinunit = robotUnit->getRTKinematicDataUnit();
- ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
+ auto kinunit = prov->getRTKinematicDataUnit();
+ ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The DataProvider " << prov->getName() << " has no kinematic data unit");
//get pointers to sensor values from units
jointStates = Traits::getSensorValues(kinunit, cfg->jointNames) ;
//not initialized, this is done in rtPreActivateController
- iceTargets.resize(cfg->jointNames.size(),0);
+ iceTargets.resize(cfg->jointNames.size(), 0);
//get pointers for the results of this controller
lvl0Targets.reserve(cfg->jointNames.size());
- for (const auto & j : cfg->jointNames)
+ for (const auto& j : cfg->jointNames)
{
- auto target = dynamic_cast<TargetType*>(robotUnit->getJointTarget(j, Traits::getControlMode()));
+ auto target = dynamic_cast<TargetType*>(prov->getJointTarget(j, Traits::getControlMode()));
ARMARX_CHECK_EXPRESSION_W_HINT(target, "The joint " << j << " has no controll mode " << Traits::getControlMode());
lvl0Targets.emplace_back(Traits::getTargetDatamember(*target));
}
@@ -197,20 +197,20 @@ namespace armarx
}
template <typename TargetType>
- void PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current &)
+ void PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current&)
{
auto targetIt = indices.find(name);
- if(targetIt == indices.end())
+ if (targetIt == indices.end())
{
- throw InvalidArgumentException{"The joint " + name + " is not controlled by this (" + getName() + ") controller"};
+ throw InvalidArgumentException {"The joint " + name + " is not controlled by this (" + getName() + ") controller"};
}
iceTargets.at(targetIt->second).val.store(value);
}
template <typename TargetType>
- void PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current &)
+ void PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current&)
{
- for(const auto& value:values)
+ for (const auto& value : values)
{
setJoint(value.first, value.second);
}
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Constants.h b/source/RobotAPI/libraries/RobotRTControllers/Constants.h
index 74e538838dd8f045ed93982ec0a894d7de3f894d..a56fce762581992a0e915d79f2a5a47efd29c3a7 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/Constants.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Constants.h
@@ -30,7 +30,10 @@ namespace armarx
{
namespace ControllerConstants
{
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
static const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
+#pragma GCC diagnostic pop
}
}
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
index 8dbd97b1ef17102767bd325e95cbe05ccb430e59..aa19370a1e71bc7cdd6c218ea264bebcc482b3eb 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
@@ -31,13 +31,36 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/util/Registrar.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.h>
#include "Targets/JointTargetBase.h"
+//units
+#include "DataUnits/ForceTorqueDataUnit.h"
+#include "DataUnits/HapticDataUnit.h"
+#include "DataUnits/IMUDataUnit.h"
+#include "DataUnits/KinematicDataUnit.h"
+#include "DataUnits/PlatformDataUnit.h"
+
namespace armarx
{
+ class LVL1ControllerDataProviderInterface: virtual public Ice::Object
+ {
+ public:
+ virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
+ virtual const HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
+ virtual const IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
+ virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
+ virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
+
+ virtual JointTargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) = 0;
+
+ virtual std::string getName() const = 0;
+ };
+ using LVL1ControllerDataProviderInterfacePtr = IceInternal::Handle<LVL1ControllerDataProviderInterface>;
+
/**
* @defgroup Library-RobotRTControllers RobotRTControllers
* @ingroup RobotAPI
@@ -82,7 +105,7 @@ namespace armarx
*
* \section rtnrtcomm Communication between RT and NonRT
* All communication between RT and NonRT has to be lockfree.
- * So you have to use constructs like atomics or TripleBuffers (See armarx::LVL1ControllerTemplate).
+ * So you have to use constructs like atomics or TripleBuffers (See armarx::LVL1ControllerWithTripleBuffer).
*
* \image html LVL1ControllerGeneralDataFlow.svg "General Dataflow in a LVL1Controller"
*
@@ -172,17 +195,16 @@ namespace armarx
};
using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
- class RobotUnit;
-
- using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)>>;
+ using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(LVL1ControllerDataProviderInterfacePtr robotUnit, LVL1ControllerConfigPtr config)>>;
template<class ControllerType>
struct LVL1ControllerRegistration
{
LVL1ControllerRegistration(const std::string& name)
{
- LVL1ControllerRegistry::registerElement(name, [](IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)
+ LVL1ControllerRegistry::registerElement(name, [](LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
{
- return new ControllerType(robotUnit, config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider is NULL!");
+ return new ControllerType(prov, config);
});
}
};
@@ -273,10 +295,10 @@ namespace armarx
* \endcode
*/
template <typename ControlDataStruct>
- class LVL1ControllerTemplate: virtual public LVL1Controller
+ class LVL1ControllerWithTripleBuffer: virtual public LVL1Controller
{
public:
- LVL1ControllerTemplate(const ControlDataStruct& initialCommands = ControlDataStruct()):
+ LVL1ControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
controlDataTripleBuffer {initialCommands}
{
}
@@ -313,6 +335,6 @@ namespace armarx
WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
};
template <typename ControlDataStruct>
- using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerTemplate<ControlDataStruct>>;
+ using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
}
#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
index 095e2ef7e464918f755b1c584a8a0dcc73c49785..97ab989622bd3089e6960d7d69291a745b0811ff 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
@@ -35,6 +35,7 @@ void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::
{
std::string controlMode = lvl0Controller.getControlMode();
GuardType guard {dataMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
if (hasLVL0Controller(jointName, controlMode))
{
ARMARX_ERROR << "A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!";
@@ -47,26 +48,20 @@ void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::
}
}
-const armarx::LVL0ControllerBase& armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode) const
+const armarx::LVL0ControllerBase* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode) const
{
GuardType guard {dataMutex};
- return *lvl0Controllers.at(jointName).at(controlMode);
+ ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+ return lvl0Controllers.at(jointName).at(controlMode);
}
-armarx::LVL0ControllerBase& armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode)
+armarx::LVL0ControllerBase* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode)
{
GuardType guard {dataMutex};
- return *lvl0Controllers.at(jointName).at(controlMode);
+ ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+ return lvl0Controllers.at(jointName).at(controlMode);
}
-armarx::RobotUnit::RobotUnit(const std::vector<std::string>& jointNames):
- jointNames {jointNames},
- jointNameIndices {toIndexMap(jointNames)},
- controllersRequested {jointNames.size()},
- controllersActivated {jointNames.size()},
- lvl0ControllersEmergencyStop {jointNames.size()}
-{}
-
Ice::StringSeq armarx::RobotUnit::getControllersKnown(const Ice::Current&) const
{
return LVL1ControllerRegistry::getKeys();
@@ -201,6 +196,7 @@ armarx::JointNameToControlModesDictionary armarx::RobotUnit::getJointControlMode
void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current&)
{
GuardType guard {dataMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
//check if all of these controllers exist
for (const auto& lvl1 : controllerRequestedNames)
{
@@ -272,12 +268,12 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
const auto& joint = jointNames.at(i);
if (lvl1ControllerAssignement[joint].empty())
{
- getRequestedLVL0Controllers().at(i) = &getLVL0Controller(joint, ControlModes::EmergencyStopMode);
+ getRequestedLVL0Controllers().at(i) = getLVL0Controller(joint, ControlModes::EmergencyStopMode);
ARMARX_WARNING << "Joint '" << joint << "' has no lvl1 controller assigned!";
continue;
}
const auto& lvl0Mode = lvl1Controllers.at(lvl1ControllerAssignement[joint])->jointControlModeMap.at(joint);
- getRequestedLVL0Controllers().at(i) = &getLVL0Controller(joint, lvl0Mode);
+ getRequestedLVL0Controllers().at(i) = getLVL0Controller(joint, lvl0Mode);
}
//populate controllersRequested.lvl1Controllers
getRequestedLVL1Controllers().clear();
@@ -320,7 +316,9 @@ armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::
}
//create the controller
jointsUsedByLVL1Controler.clear();
- LVL1ControllerPtr lvl1 = factory(this, config);
+ RobotUnitInterfacePtr::dynamicCast(RobotUnitPtr {this});
+ LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this});
+ LVL1ControllerPtr lvl1 = factory(LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config);
lvl1->jointControlModeMap = jointsUsedByLVL1Controler;
lvl1->jointIndices.clear();
lvl1->jointIndices.reserve(jointsUsedByLVL1Controler.size());
@@ -342,19 +340,31 @@ armarx::JointTargetBase* armarx::RobotUnit::getJointTarget(const std::string& jo
{
return nullptr;
}
- const auto& lvl0 = getLVL0Controller(jointName, controlMode);
- ARMARX_CHECK_EXPRESSION(lvl0.getControlMode() == controlMode);
+ const auto lvl0 = getLVL0Controller(jointName, controlMode);
+ ARMARX_CHECK_EXPRESSION(lvl0->getControlMode() == controlMode);
jointsUsedByLVL1Controler[jointName] = controlMode;
- return lvl0.getTarget();
+ return lvl0->getTarget();
+}
+
+void armarx::RobotUnit::setJointNames(const std::vector<std::string>& names)
+{
+ GuardType guard {dataMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(!areJointNamesSet(), "JointNames were already set!");
+ jointNames = names;
+ jointNameIndices = toIndexMap(jointNames);
+ lvl0ControllersEmergencyStop.resize(jointNames.size());
+ controllersRequested.reinitAllBuffers(LVL0AndLVL1ControllerList(jointNames.size()));
+ controllersActivated.reinitAllBuffers(LVL0AndLVL1ControllerList(jointNames.size()));
}
bool armarx::RobotUnit::hasLVL1Controller(const std::string& name) const
{
GuardType guard {dataMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
return lvl1Controllers.end() != lvl1Controllers.find(name);
}
-void armarx::RobotUnit::setLVL1ControllerActive(const armarx::LVL1ControllerPtr& lvl1, bool isActive)
+void armarx::RobotUnit::rtSetLVL1ControllerActive(const armarx::LVL1ControllerPtr& lvl1, bool isActive)
{
if (isActive)
{
@@ -366,9 +376,16 @@ void armarx::RobotUnit::setLVL1ControllerActive(const armarx::LVL1ControllerPtr&
}
}
+bool armarx::RobotUnit::areJointNamesSet() const
+{
+ GuardType guard {dataMutex};
+ return !jointNames.empty();
+}
+
bool armarx::RobotUnit::hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const
{
GuardType guard {dataMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
return lvl0Controllers.end() != lvl0Controllers.find(jointName) &&
lvl0Controllers.at(jointName).end() != lvl0Controllers.at(jointName).find(controlMode) &&
lvl0Controllers.at(jointName).at(controlMode);
@@ -452,76 +469,6 @@ bool armarx::RobotUnit::loadLibFromPackage(const std::string& package, const std
return false;
}
-bool armarx::RobotUnit::rtControllersWereSwitched() const
-{
- return controllersRequested.updateReadBuffer();
-}
-
-std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getRequestedLVL0Controllers()
-{
- return controllersRequested.getWriteBuffer().lvl0Controllers;
-}
-
-std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getRequestedLVL1Controllers()
-{
- return controllersRequested.getWriteBuffer().lvl1Controllers;
-}
-
-const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getRequestedLVL0Controllers() const
-{
- return controllersRequested.getWriteBuffer().lvl0Controllers;
-}
-
-const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getRequestedLVL1Controllers() const
-{
- return controllersRequested.getWriteBuffer().lvl1Controllers;
-}
-
-const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getActivatedLVL0Controllers() const
-{
- return controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
-}
-
-const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getActivatedLVL1Controllers() const
-{
- return controllersActivated.getUpToDateReadBuffer().lvl1Controllers;
-}
-
-const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetRequestedLVL0Controllers() const
-{
- return controllersRequested.getReadBuffer().lvl0Controllers;
-}
-
-const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetRequestedLVL1Controllers() const
-{
- return controllersRequested.getReadBuffer().lvl1Controllers;
-}
-
-std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetActivatedLVL0Controllers()
-{
- return controllersActivated.getWriteBuffer().lvl0Controllers;
-}
-
-std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetActivatedLVL1Controllers()
-{
- return controllersActivated.getWriteBuffer().lvl1Controllers;
-}
-
-const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetActivatedLVL0Controllers() const
-{
- return controllersActivated.getWriteBuffer().lvl0Controllers;
-}
-
-const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetActivatedLVL1Controllers() const
-{
- return controllersActivated.getWriteBuffer().lvl1Controllers;
-}
-
-void armarx::RobotUnit::rtCommitActivatedLVL1Controllers()
-{
- controllersActivated.commitWrite();
-}
-
bool armarx::RobotUnit::rtSwitchSetup()
{
if (!rtControllersWereSwitched())
@@ -532,25 +479,31 @@ bool armarx::RobotUnit::rtSwitchSetup()
for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
{
//deactivate old
- if (rtGetActivatedLVL1Controllers().at(i))
+ if (rtGetActivatedLVL1Controller(i))
{
- rtGetActivatedLVL1Controllers().at(i)->rtDeactivateController();
+ rtGetActivatedLVL1Controller(i)->rtDeactivateController();
}
//activate new
- if (rtGetRequestedLVL1Controllers().at(i))
+ if (rtGetRequestedLVL1Controller(i))
{
- rtGetRequestedLVL1Controllers().at(i)->rtActivateController();
+ rtGetRequestedLVL1Controller(i)->rtActivateController();
}
//update activated
- rtGetActivatedLVL1Controllers().at(i) = rtGetRequestedLVL1Controllers().at(i);
+ rtGetActivatedLVL1Controller(i) = rtGetRequestedLVL1Controller(i);
}
//handle lvl0
for (std::size_t i = 0; i < rtGetRequestedLVL0Controllers().size(); ++i)
{
- if (rtGetRequestedLVL0Controllers().at(i) != rtGetActivatedLVL0Controllers().at(i))
+ if (rtGetRequestedLVL0Controller(i) != rtGetActivatedLVL0Controller(i))
{
- rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controllers().at(i), rtGetRequestedLVL0Controllers().at(i));
- rtGetActivatedLVL0Controllers().at(i) = rtGetRequestedLVL0Controllers().at(i);
+ rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetRequestedLVL0Controller(i));
+ rtGetActivatedLVL0Controller(i) = rtGetRequestedLVL0Controller(i);
+ }
+ if (!rtGetRequestedLVL0Controller(i))
+ {
+ //no lvl0 controller is set!
+ rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetEmergencyStopLVL0Controller(i));
+ rtGetActivatedLVL0Controller(i) = rtGetActivatedLVL0Controller(i);
}
}
return true;
@@ -572,19 +525,19 @@ void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
{
for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
{
- if (!rtGetRequestedLVL1Controllers().at(i))
+ if (!rtGetRequestedLVL1Controller(i))
{
continue;
}
- if (rtGetRequestedLVL1Controllers().at(i)->rtUsesJoint(index))
+ if (rtGetRequestedLVL1Controller(i)->rtUsesJoint(index))
{
- rtGetRequestedLVL1Controllers().at(i)->rtDeactivateController();
- for (auto used : rtGetRequestedLVL1Controllers().at(i)->rtGetJointIndices())
+ rtGetRequestedLVL1Controller(i)->rtDeactivateController();
+ for (auto used : rtGetRequestedLVL1Controller(i)->rtGetJointIndices())
{
- rtSwitchLVL0Controller(used, rtGetActivatedLVL0Controllers().at(i), lvl0ControllersEmergencyStop.at(i));
- rtGetActivatedLVL0Controllers().at(i) = lvl0ControllersEmergencyStop.at(i);
+ rtSwitchLVL0Controller(used, rtGetActivatedLVL0Controller(i), rtGetEmergencyStopLVL0Controller(i));
+ rtGetActivatedLVL0Controller(i) = rtGetEmergencyStopLVL0Controller(i);
}
- rtGetActivatedLVL1Controllers().at(i) = nullptr;
+ rtGetActivatedLVL1Controller(i) = nullptr;
return;
}
}
@@ -598,8 +551,10 @@ void armarx::RobotUnit::rtRunLVL0Controllers()
}
}
-bool armarx::RobotUnit::rtValidateLVL0ControllerSetup() const
+bool armarx::RobotUnit::validateAddedLVL0Controllers() const
{
+ GuardType guard {dataMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
for (const auto& lvl0 : lvl0ControllersEmergencyStop)
{
if (!lvl0)
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
index f98dbc8e5d3400df4b15a0c812a7016b28777bdd..734be3dfb30840bd9cc51a0937e5549aa7c799d2 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
@@ -40,12 +40,6 @@
#include "LVL0Controller.h"
#include "LVL1Controller.h"
-#include "DataUnits/ForceTorqueDataUnit.h"
-#include "DataUnits/HapticDataUnit.h"
-#include "DataUnits/IMUDataUnit.h"
-#include "DataUnits/KinematicDataUnit.h"
-#include "DataUnits/PlatformDataUnit.h"
-
namespace armarx
{
class DynamicLibrary;
@@ -104,11 +98,13 @@ namespace armarx
* {
* initYourCommunication();//implement this
* passYourLVL0ControllersToTheRobotUnit();//implement this
- * if(!rtValidateLVL0ControllerSetup())
+ * if(!validateAddedLVL0Controllers())
* {
* //report errors
* throw std::logic_error{"lvl0 controller setup invalid"};
* }
+ *
+ * //at this point the RT thread becomes RT (calls before this may not be rt)
* IceUtil::Time currentStateTimestamp = TimeUtil::GetTime();
* IceUtil::Time lastStateTimestamp = TimeUtil::GetTime();
* getTheCurrentRobotState();//implement this
@@ -124,19 +120,19 @@ namespace armarx
* throw std::logic_error{"switching the controller setup failed"};
* }
* //validate the setup if you need to
- * //if you had to change something, call rtCommitActivatedLVL1Controllers();
+ * //if you had to change something, call rtCommitActivatedControllers();
* }
* //run the lvl1 controllers
* rtRunLVL1Controllers(currentStateTimestamp, currentStateTimestamp - lastStateTimestamp);
* //validate targets
* for(std::size_t i = 0; i<rtGetActivatedLVL0Controllers().size(); ++i)
* {
- * if(!rtGetActivatedLVL0Controllers().at(i)->isTargetVaild())
+ * if(!rtGetActivatedLVL0Controller(i)->isTargetVaild())
* {
* //handle this error!
* //you probably should log this error to some error struct
* rtDeactivateAssignedLVL1Controller(i);
- * rtCommitActivatedLVL1Controllers();
+ * rtCommitActivatedControllers();
* }
* }
* //run lvl0
@@ -182,15 +178,24 @@ namespace armarx
* This controller should activate the motor brakes.
*/
class RobotUnit :
- virtual public armarx::Component,
- virtual public armarx::RobotUnitInterface
+ virtual public Component,
+ virtual public RobotUnitInterface,
+ virtual public LVL1ControllerDataProviderInterface
{
public:
using MutexType = std::recursive_mutex;
using GuardType = std::lock_guard<MutexType>;
- RobotUnit(const std::vector<std::string>& jointNames);
-
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ //ice LVL1ControllerDataProviderInterface
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ virtual std::string getName() const override
+ {
+ return Component::getName();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ //ice interface
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
//controllers
virtual Ice::StringSeq getControllersKnown(const Ice::Current& = GlobalIceCurrent) const override;
virtual Ice::StringSeq getControllerNamesLoaded(const Ice::Current& = GlobalIceCurrent) const override;
@@ -205,73 +210,74 @@ namespace armarx
virtual JointNameToControlModeDictionary getJointControlModesRequested(const Ice::Current& = GlobalIceCurrent) const override;
virtual JointNameToControlModeDictionary getJointControlModesActivated(const Ice::Current& = GlobalIceCurrent) const override;
virtual JointNameToControlModesDictionary getJointControlModesSupported(const Ice::Current& = GlobalIceCurrent) const override;
- //loading and switching (ice)
+ //loading and switching
virtual void switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current& = GlobalIceCurrent) override;
virtual LVL1ControllerInterfacePrx loadController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current& = GlobalIceCurrent) override;
virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) override;
virtual bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) override;
- //get buffers (ice)ELMO_MODE_OF_OPERATION_CURRENT_CONTROL
- /// @return The requested lvl0 controllers (none is null)
- std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers();
- /// @return The requested lvl1 controllers (some may be null)
- std::vector<LVL1ControllerPtr >& getRequestedLVL1Controllers();
- /// @return The requested lvl0 controllers (none is null)
- const std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers() const;
- /// @return The requested lvl1 controllers (some may be null)
- const std::vector<LVL1ControllerPtr >& getRequestedLVL1Controllers() const;
- /// @return The activated lvl0 controllers (none is null)
- const std::vector<LVL0ControllerBase*>& getActivatedLVL0Controllers() const;
- /// @return The activated lvl1 controllers (some may be null)
- const std::vector<LVL1ControllerPtr >& getActivatedLVL1Controllers() const;
- //buffers (rt)
- bool rtControllersWereSwitched() const;
- /// @return The requested lvl0 controllers (none is null)
- const std::vector<LVL0ControllerBase*>& rtGetRequestedLVL0Controllers() const;
- /// @return The requested lvl1 controllers (some may be null)
- const std::vector<LVL1ControllerPtr >& rtGetRequestedLVL1Controllers() const;
- /// @return The activated lvl0 controllers (none is null)
- std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers();
- /// @return The activated lvl1 controllers (some may be null)
- std::vector<LVL1ControllerPtr >& rtGetActivatedLVL1Controllers();
- /// @return The activated lvl0 controllers (none is null)
- const std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers() const;
- /// @return The activated lvl1 controllers (some may be null)
- const std::vector<LVL1ControllerPtr >& rtGetActivatedLVL1Controllers() const;
- void rtCommitActivatedLVL1Controllers();
- //switching (rt)
- bool rtSwitchSetup();
- void rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- void rtDeactivateAssignedLVL1Controller(std::size_t index);
- void rtRunLVL0Controllers();
-
- /**
- * @brief Hook for switching the lvl0 controller (this changes the controll mode)
- * @param index The index of the lvl0 controller
- * @param oldLVL0 The old lvl0 controller (will be deactivated)
- * @param newLVL0 The new lvl0 controller (will be activated)
- */
- virtual void rtSwitchLVL0Controller(std::size_t index, LVL0ControllerBase* oldLVL0, LVL0ControllerBase* newLVL0) = 0;
- //checks (rt)
- virtual bool rtValidateLVL0ControllerSetup() const;
-
- //units (ice)
+ //units
virtual KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
virtual ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
virtual PlatformUnitInterfacePrx getPlatformUnitInterface(const Ice::Current& = GlobalIceCurrent) const = 0;
- //units (ice but results are used in rt)
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // used by lvl1 controllers
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ //units (nonrt but results are used in rt)
virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
virtual const HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
virtual const IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
- //targets (ice)
- JointTargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode);
+ //targets (nonrt but results are used in rt)
+ virtual JointTargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) override;
protected:
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // NonRT
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // setup the data structures
+ void setJointNames(const std::vector<std::string>& names);
+
+ // Component
/// @see PropertyUser::createPropertyDefinitions()
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ //get buffers
+ /// @return The requested lvl0 controllers (none is null)
+ std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers()
+ {
+ return controllersRequested.getWriteBuffer().lvl0Controllers;
+ }
+ /// @return The requested lvl1 controllers (some may be null)
+ std::vector<LVL1ControllerPtr >& getRequestedLVL1Controllers()
+ {
+ return controllersRequested.getWriteBuffer().lvl1Controllers;
+ }
+ /// @return The requested lvl0 controllers (none is null)
+ const std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers() const
+ {
+ return controllersRequested.getWriteBuffer().lvl0Controllers;
+ }
+ /// @return The requested lvl1 controllers (some may be null)
+ const std::vector<LVL1ControllerPtr >& getRequestedLVL1Controllers() const
+ {
+ return controllersRequested.getWriteBuffer().lvl1Controllers;
+ }
+ /// @return The activated lvl0 controllers (none is null)
+ const std::vector<LVL0ControllerBase*>& getActivatedLVL0Controllers() const
+ {
+ return controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
+ }
+ /// @return The activated lvl1 controllers (some may be null)
+ const std::vector<LVL1ControllerPtr >& getActivatedLVL1Controllers() const
+ {
+ return controllersActivated.getUpToDateReadBuffer().lvl1Controllers;
+ }
+
+ //checks
+ /// @return Return whether the set of added LVL0Controllers is ok (e.g. if each joint has one EmergencyStop controller)
+ virtual bool validateAddedLVL0Controllers() const;
/**
* @brief Add a LVL0Controller for a specified joint.
@@ -282,10 +288,15 @@ namespace armarx
void addLVL0Controller(const std::string& jointName, LVL0ControllerBase& lvl0Controller);
/// @return The LVL0Controller for given joint and control mode.
- const LVL0ControllerBase& getLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
+ const LVL0ControllerBase* getLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
/// @return The LVL0Controller for given joint and control mode.
- LVL0ControllerBase& getLVL0Controller(const std::string& jointName, const std::string& controlMode);
+ LVL0ControllerBase* getLVL0Controller(const std::string& jointName, const std::string& controlMode);
+
+ LVL0ControllerBase* rtGetEmergencyStopLVL0Controller(std::size_t index)
+ {
+ return lvl0ControllersEmergencyStop.at(index);
+ }
/// @return Whether a LVL0Controller for given joint and control mode exists.
bool hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
@@ -293,13 +304,118 @@ namespace armarx
/// @return Whether a LVL1Controller for this name is loaded.
bool hasLVL1Controller(const std::string& name) const;
- static void setLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool isActive = 1);
+ static void rtSetLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool isActive = 1);
+
+ //other
+ private:
+ bool areJointNamesSet() const;
+ bool loadLibFromAbsolutePath(const std::string& path);
+
+ protected:
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // RT
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // get buffers (rt)
+ bool rtControllersWereSwitched() const
+ {
+ return controllersRequested.updateReadBuffer();
+ }
+ /// @return The requested lvl0 controllers (none is null)
+ const std::vector<LVL0ControllerBase*>& rtGetRequestedLVL0Controllers() const
+ {
+ return controllersRequested.getReadBuffer().lvl0Controllers;
+ }
+ /// @return The requested lvl1 controllers (some may be null)
+ const std::vector<LVL1ControllerPtr >& rtGetRequestedLVL1Controllers() const
+ {
+ return controllersRequested.getReadBuffer().lvl1Controllers;
+ }
+ /// @return The activated lvl0 controllers (none is null)
+ std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers()
+ {
+ return controllersActivated.getWriteBuffer().lvl0Controllers;
+ }
+ /// @return The activated lvl1 controllers (some may be null)
+ std::vector<LVL1ControllerPtr >& rtGetActivatedLVL1Controllers()
+ {
+ return controllersActivated.getWriteBuffer().lvl1Controllers;
+ }
+ /// @return The activated lvl0 controllers (none is null)
+ const std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers() const
+ {
+ return controllersActivated.getWriteBuffer().lvl0Controllers;
+ }
+ /// @return The activated lvl1 controllers (some may be null)
+ const std::vector<LVL1ControllerPtr >& rtGetActivatedLVL1Controllers() const
+ {
+ return controllersActivated.getWriteBuffer().lvl1Controllers;
+ }
+ //get controllers (rt)
+ LVL0ControllerBase* const& rtGetActivatedLVL0Controller(std::size_t index) const
+ {
+ return rtGetActivatedLVL0Controllers().at(index);
+ }
+ const LVL1ControllerPtr& rtGetActivatedLVL1Controller(std::size_t index) const
+ {
+ return rtGetActivatedLVL1Controllers().at(index);
+ }
+ LVL0ControllerBase*& rtGetActivatedLVL0Controller(std::size_t index)
+ {
+ return rtGetActivatedLVL0Controllers().at(index);
+ }
+ LVL1ControllerPtr& rtGetActivatedLVL1Controller(std::size_t index)
+ {
+ return rtGetActivatedLVL1Controllers().at(index);
+ }
+
+
+ LVL0ControllerBase* rtGetRequestedLVL0Controller(std::size_t index) const
+ {
+ return rtGetRequestedLVL0Controllers().at(index);
+ }
+ const LVL1ControllerPtr& rtGetRequestedLVL1Controller(std::size_t index) const
+ {
+ return getRequestedLVL1Controllers().at(index);
+ }
+
+ void rtCommitActivatedControllers()
+ {
+ controllersActivated.commitWrite();
+ }
+
+ //switching (rt)
+ bool rtSwitchSetup();
+ void rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtDeactivateAssignedLVL1Controller(std::size_t index);
+ void rtRunLVL0Controllers();
+
+ /**
+ * @brief Hook for switching the lvl0 controller (this changes the controll mode)
+ * @param index The index of the lvl0 controller
+ * @param oldLVL0 The old lvl0 controller (will be deactivated)
+ * @param newLVL0 The new lvl0 controller (will be activated)
+ */
+ virtual void rtSwitchLVL0Controller(std::size_t index, LVL0ControllerBase* oldLVL0, LVL0ControllerBase* newLVL0) = 0;
+
+ protected:
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // data
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ mutable MutexType dataMutex;
+ //data accessible in RT and nonRT
+ std::vector<std::string> jointNames;
+ std::unordered_map<std::string, std::size_t> jointNameIndices;
+ /// @brief True if some error occured in switch LVL1Controllers
+ std::atomic_bool switchSetupError {false};
+
+ private:
+ //controllers
struct LVL0AndLVL1ControllerList
{
LVL0AndLVL1ControllerList(std::size_t size):
- lvl0Controllers{size,nullptr},
- lvl1Controllers{size,nullptr}
+ lvl0Controllers {size, nullptr},
+ lvl1Controllers {size, nullptr}
{}
LVL0AndLVL1ControllerList() = default;
@@ -308,14 +424,6 @@ namespace armarx
std::vector<LVL0ControllerBase*> lvl0Controllers;
std::vector<LVL1ControllerPtr > lvl1Controllers;
};
- mutable MutexType dataMutex;
- //data accessible in RT and nonRT
- const std::vector<std::string> jointNames;
- const std::unordered_map<std::string, std::size_t> jointNameIndices;
- /// @brief
- std::atomic_bool switchSetupError {false};
- //used to communicate with rt
- //non rt data
/**
* @brief Holds pointer to all lvl0 controllers. (index: [jointname][controlmode])
* Is initialized by derived classes.
@@ -327,18 +435,19 @@ namespace armarx
* May not be accessed in rt.
*/
std::map<std::string, LVL1ControllerPtr> lvl1Controllers;
- std::map<std::string, DynamicLibraryPtr> loadedLibs;
- private:
- bool loadLibFromAbsolutePath(const std::string& path);
+ /// @brief LVL0Controllers for Emergency stop
+ std::vector<LVL0ControllerBase*> lvl0ControllersEmergencyStop;
- /// @brief Stores joints accessed via getJointTarget
- JointNameToControlModeDictionary jointsUsedByLVL1Controler;
+ //used to communicate with rt
/// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some lvl1 controllers may be null)
TripleBuffer<LVL0AndLVL1ControllerList> controllersRequested;
/// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some lvl1 controllers may be null)
TripleBuffer<LVL0AndLVL1ControllerList> controllersActivated;
- /// @brief LVL0Controllers for Emergency stop
- std::vector<LVL0ControllerBase*> lvl0ControllersEmergencyStop;
+
+ //other
+ /// @brief Stores joints accessed via getJointTarget
+ JointNameToControlModeDictionary jointsUsedByLVL1Controler;
+ std::map<std::string, DynamicLibraryPtr> loadedLibs;
};
using RobotUnitPtr = IceInternal::Handle<RobotUnit>;