diff --git a/source/RobotAPI/libraries/Controllers/RobotUnit.cpp b/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
index f612bf99b6088d9ea2edc071c572a7da37507239..7ea9bdb26a1ac4f8705d57c64663266f6a9a6eff 100644
--- a/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
@@ -31,6 +31,29 @@ armarx::PropertyDefinitionsPtr armarx::RobotUnit::createPropertyDefinitions()
return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
}
+void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::LVL0ControllerBase& lvl0Controller)
+{
+ GuardType guard {dataMutex};
+ if (hasLVL0Controller(jointName, lvl0Controller.getControlMode()))
+ {
+ ARMARX_ERROR << "A LVL0Controller for " + jointName + "(" + lvl0Controller.getControlMode() + ") does already exist!";
+ throw std::invalid_argument {"A LVL0Controller for " + jointName + "(" + lvl0Controller.getControlMode() + ") does already exist!"};
+ }
+ lvl0Controllers[jointName][lvl0Controller.getControlMode()] = &lvl0Controller;
+}
+
+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::LVL0ControllerBase& armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode)
+{
+ GuardType guard {dataMutex};
+ return *lvl0Controllers.at(jointName).at(controlMode);
+}
+
Ice::StringSeq armarx::RobotUnit::getControllersKnown(const Ice::Current&) const
{
return LVL1ControllerRegistry::getKeys();
@@ -41,7 +64,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesLoaded(const Ice::Current&)
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
- for (const auto& lvl1 : lvl1Controllers)
+ for (const auto & lvl1 : lvl1Controllers)
{
result.emplace_back(lvl1.first);
}
@@ -52,7 +75,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesRequested(const Ice::Current
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
- for (const auto& lvl1 : controllersRequested.getWriteBuffer().lvl1Controllers)
+ for (const auto & lvl1 : controllersRequested.getWriteBuffer().lvl1Controllers)
{
result.emplace_back(lvl1->getName());
}
@@ -63,7 +86,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesActivated(const Ice::Current
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
- for (const auto& lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ for (const auto & lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
{
result.emplace_back(lvl1->getName());
}
@@ -74,7 +97,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersLoade
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto& lvl1 : lvl1Controllers)
+ for (const auto & lvl1 : lvl1Controllers)
{
result[lvl1.first] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1.second->getProxy());
}
@@ -84,7 +107,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersReque
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto& lvl1 : controllersRequested.getWriteBuffer().lvl1Controllers)
+ for (const auto & lvl1 : controllersRequested.getWriteBuffer().lvl1Controllers)
{
result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
}
@@ -94,7 +117,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersActiv
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto& lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ for (const auto & lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
{
result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
}
@@ -105,9 +128,9 @@ armarx::JointNameToLVL1Dictionary armarx::RobotUnit::getControllerJointAssignmen
{
GuardType guard {dataMutex};
JointNameToLVL1Dictionary result;
- for (const auto& lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ for (const auto & lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
{
- for (const auto& jointMode : lvl1->jointControlModeMap)
+ for (const auto & jointMode : lvl1->jointControlModeMap)
{
result[jointMode.first] = lvl1->getName();
}
@@ -143,11 +166,11 @@ armarx::JointNameToControlModesDictionary armarx::RobotUnit::getJointControlMode
{
GuardType guard {dataMutex};
JointNameToControlModesDictionary result;
- for (const auto& joint : lvl0Controllers)
+ for (const auto & joint : lvl0Controllers)
{
std::vector<std::string> modes;
modes.reserve(joint.second.size());
- for (const auto& mode : joint.second)
+ for (const auto & mode : joint.second)
{
modes.emplace_back(mode.first);
}
@@ -160,7 +183,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
{
GuardType guard {dataMutex};
//check if all of these controllers exist
- for (const auto& lvl1 : controllerRequestedNames)
+ for (const auto & lvl1 : controllerRequestedNames)
{
if (!hasLVL1Controller(lvl1))
{
@@ -176,7 +199,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
std::set<std::string> controllersToActivate {currentActiveLVL1Controllers.begin(), currentActiveLVL1Controllers.end()}; //these controllers will be set as active controllers
- for (const auto& toActivate : controllerRequestedNames)
+ for (const auto & toActivate : controllerRequestedNames)
{
if (controllersToActivate.count(toActivate))
{
@@ -187,7 +210,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
ARMARX_INFO << "activate '" << toActivate << "'";
const auto& lvl1 = lvl1Controllers.at(toActivate);
//check and update the assignement map
- for (const auto& jointControlMode : lvl1->jointControlModeMap)
+ for (const auto & jointControlMode : lvl1->jointControlModeMap)
{
const auto& joint = jointControlMode.first;
const auto& currentAssignedLVL1 = lvl1ControllerAssignement.at(joint);
@@ -201,7 +224,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
continue;
}
//deactivate other controller
- for (auto& assignement : lvl1ControllerAssignement)
+ for (auto & assignement : lvl1ControllerAssignement)
{
if (assignement.second == currentAssignedLVL1)
{
@@ -214,7 +237,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
}
}
//verify (exception for collision of requested lvl1)
- for (const auto& toActivate : controllerRequestedNames)
+ for (const auto & toActivate : controllerRequestedNames)
{
if (!controllersToActivate.count(toActivate))
{
@@ -230,17 +253,17 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
const auto& joint = jointNames.at(i);
if (lvl1ControllerAssignement[joint].empty())
{
- controllersRequested.getWriteBuffer().lvl0Controllers.at(i) = lvl0Controllers.at(joint).at(ControlModes::EmergencyStopMode);
+ controllersRequested.getWriteBuffer().lvl0Controllers.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);
- controllersRequested.getWriteBuffer().lvl0Controllers.at(i) = lvl0Controllers.at(joint).at(lvl0Mode);
+ controllersRequested.getWriteBuffer().lvl0Controllers.at(i) = &getLVL0Controller(joint, lvl0Mode);
}
//populate controllersRequested.lvl1Controllers
controllersRequested.getWriteBuffer().lvl1Controllers.clear();
controllersRequested.getWriteBuffer().lvl1Controllers.reserve(controllersToActivate.size());
- for (const auto& lvl1 : controllersToActivate)
+ for (const auto & lvl1 : controllersToActivate)
{
controllersRequested.getWriteBuffer().lvl1Controllers.emplace_back(lvl1Controllers.at(lvl1));
}
@@ -293,14 +316,10 @@ armarx::JointTargetBase* armarx::RobotUnit::getJointTarget(const std::string& jo
{
return nullptr;
}
- const auto& lvl0 = lvl0Controllers.at(jointName).at(controlMode);
- if (!lvl0)
- {
- return nullptr;
- }
- 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();
}
bool armarx::RobotUnit::hasLVL1Controller(const std::string& name) const
@@ -312,8 +331,9 @@ bool armarx::RobotUnit::hasLVL1Controller(const std::string& name) const
bool armarx::RobotUnit::hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const
{
GuardType guard {dataMutex};
- return lvl0Controllers.end() != lvl0Controllers.find(jointName) &&
- lvl0Controllers.at(jointName).end() != lvl0Controllers.at(jointName).find(controlMode);
+ return lvl0Controllers.end() != lvl0Controllers.find(jointName) &&
+ lvl0Controllers.at(jointName).end() != lvl0Controllers.at(jointName).find(controlMode) &&
+ lvl0Controllers.at(jointName).at(controlMode);
}
diff --git a/source/RobotAPI/libraries/Controllers/RobotUnit.h b/source/RobotAPI/libraries/Controllers/RobotUnit.h
index 61e9239595261793c77656359a03eabb84e8888c..58fc29c028e1eba44eb4c751a43877056ec93e85 100644
--- a/source/RobotAPI/libraries/Controllers/RobotUnit.h
+++ b/source/RobotAPI/libraries/Controllers/RobotUnit.h
@@ -31,6 +31,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
@@ -74,7 +75,114 @@ namespace armarx
* @ingroup Component-RobotUnit
* @brief Brief description of class RobotUnit.
*
- * Detailed description of class RobotUnit.
+ * \section Datastructures you should have
+ * \li A thread for RT
+ * \li A thread for publishing + errorreporting
+ * \li A Tripplebuffer to publish the current robot state + current targets
+ * \li A WriteBufferedTripleBuffer containing an error and logging structure (to communicate such information to the publisher thread)
+ *
+ * \section RobotUnit-tasks-to-implement Tasks to implement when deriving from this class
+ * When implementing this class you have to handle the following tasks:
+ * \li Implement the real time thread.
+ * \li Implement the publisher thread.
+ * \li Implement the communication between both threads.
+ * \li Load all LVL0 controllers and add them to the unit.
+ *
+ * \subsection RobotUnit-tasks-to-implement-rt Implementing the RT thread
+ * The thread has to be RT.
+ * So you must not do anything of the following:
+ * \li Call blocking actions (e.g. mutex::lock)
+ * \li allocate ram using new (this calls lock on a mutex)
+ * \li print output (this may allocate ram or block)
+ * \li change the size of a datasructure (strings, vectors, etc) (this may allocate ram)
+ *
+ * The structure for a thread is:
+ * \code{.cpp}
+ * void rtThread()
+ * {
+ * try
+ * {
+ * initYourCommunication();
+ * IceUtil::Time currentStateTimestamp = TimeUtil::GetTime();
+ * IceUtil::Time lastStateTimestamp = TimeUtil::GetTime();
+ * getTheCurrentRobotState();
+ * while(isNotStopped())
+ * {
+ * const IceUtil::Time startIteration = TimeUtil::GetTime();
+ * if(controllersRequested.getUpToDateData())
+ * {
+ * //the controllers were switched
+ * //change the joint controll modes (they should use the new lvl0 controllers)
+ * //update controllersActivated to inform the user which controllers are in use
+ * controllersActivated.getWriteBuffer().lvl0Controllers = ...;
+ * controllersActivated.getWriteBuffer().lvl1Controllers = controllersRequested.getReadBuffer().lvl1Controllers;
+ * controllersActivated.commitWrite();
+ * }
+ * //run the lvl1 controllers
+ * for(LVL1ControllerPtr& lvl1: controllersActivated.getWriteBuffer().lvl1Controllers)
+ * {
+ * if(!lvl1)
+ * {
+ * continue;
+ * }
+ * lvl1->rtSwapBufferAndRun(currentStateTimestamp, currentStateTimestamp - lastStateTimestamp);
+ * }
+ * //validate targets
+ * for(LVL0ControllerBase* lvl0: controllersActivated.getWriteBuffer().lvl0Controllers)
+ * {
+ * if(!lvl0->isTargetVaild())
+ * {
+ * //handle this error!
+ * //you probably should log this error to some error struct
+ * //then identify the misbehaving lvl1 controller
+ * //set all lvl0 controllers controlled by it to ControlModes::EmergencyStopMode
+ * //update controllersActivated.getWriteBuffer().lvl0Controllers
+ * //remove the lvl controller from controllersActivated.getWriteBuffer().lvl1Controllers (set it to nullptr)
+ * controllersActivated.commitWrite()
+ * }
+ * }
+ * for(LVL0ControllerBase* lvl0: controllersActivated.getWriteBuffer().lvl0Controllers)
+ * {
+ * lvl0->run();
+ * }
+ *
+ * currentStateTimestamp = TimeUtil::GetTime();
+ * lastStateTimestamp = currentStateTimestamp;
+ * getTheCurrentRobotStateAndSendTheCurrentControllCommands();
+ * communicateWithYourPublisherThread();
+ * //maybe meassure how long a loop took and log this information to your error struct
+ * TimeUtil::Sleep(IceUtil::Time::microSeconds(1000)-(currentStateTimestamp-startIteration));
+ * }
+ * }
+ * catch(...)
+ * {
+ * emergencyStop();
+ * dumpDebuggingData();
+ * doSomeErrorHandling();
+ * shutEverythingGracefullyDown();
+ * }
+ * }
+ * \endcode
+ *
+ * \subsection RobotUnit-tasks-to-implement-pub Implementing the publisher thread
+ *
+ * \subsection RobotUnit-tasks-to-implement-com Implementing the communication between both threads
+ * The communication between both threads has to be RT.
+ * So you have to use atomics or TrippleBuffers (or any other non-blocking method)
+ * Data you should communicate is:
+ * \li Current state (joint modes/values)
+ * \li Current goals
+ * \li all error messages.
+ *
+ * \subsection RobotUnit-tasks-to-implement-lvl0 Adding LVL0 controllers
+ * Add LVL0Controllers by calling
+ * \code{.cpp}
+ * addLVL0Controller(const std::string& jointName, LVL0ControllerBase& lvl0Controller);
+ * \endcode
+ * If you add two controllers withe the same control mode for one joint, an std::invalid_argument exception is thrown.
+ *
+ * Each joint needs a LVL0Controller for the mode ControlModes::EmergencyStopMode.
+ * This controller should activate the motor brakes.
*/
class RobotUnit :
virtual public armarx::Component,
@@ -123,9 +231,32 @@ namespace armarx
*/
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- bool hasLVL1Controller(const std::string& name) const;
+ /**
+ * @brief Add a LVL0Controller for a specified joint.
+ * @param jointName The joint controlled by the controller
+ * @param lvl0Controller The controller to add.
+ * @throw If you add two controllers withe the same control mode for one joint, an std::invalid_argument exception is thrown.
+ */
+ 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;
+ /**
+ * @return The LVL0Controller for given joint and control mode.
+ */
+ LVL0ControllerBase& getLVL0Controller(const std::string& jointName, const std::string& controlMode);
+ /**
+ * @return Whether a LVL0Controller for given joint and control mode exists.
+ */
bool hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
+ /**
+ * @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)
{
lvl1->isActive = isActive;