From 95a48216b16c4ca8c02f9b7c9a1e09e426c543e0 Mon Sep 17 00:00:00 2001
From: Raphael <ufdrv@student.kit.edu>
Date: Wed, 15 Feb 2017 19:22:22 +0100
Subject: [PATCH] add RobotUnit::setJoint method (instead of setting the joints
in the ctor) add some other access utiliti functions order the members of
RobotUnit add the LVL1ControllerDataProviderInterface (this is used by
LVL1Controllers to get their data)
---
.../PassThroughController.h | 54 ++--
.../libraries/RobotRTControllers/Constants.h | 3 +
.../RobotRTControllers/LVL1Controller.h | 40 ++-
.../RobotRTControllers/RobotUnit.cpp | 165 ++++-------
.../libraries/RobotRTControllers/RobotUnit.h | 275 ++++++++++++------
5 files changed, 313 insertions(+), 224 deletions(-)
diff --git a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
index 05c70890c..38e8ccac5 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 74e538838..a56fce762 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 8dbd97b1e..aa19370a1 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 095e2ef7e..97ab98962 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 f98dbc8e5..734be3dfb 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>;
--
GitLab