Skip to content
Snippets Groups Projects
Commit 8e9b1b6e authored by Raphael's avatar Raphael
Browse files

added doxygen and an addLVL0Controller function

parent e7b654c9
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/Controllers/RobotUnit.cpp
+ 47
− 27
View file @ 8e9b1b6e
......@@ -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);
}
......
source/RobotAPI/libraries/Controllers/RobotUnit.h
+ 133
− 2
View file @ 8e9b1b6e
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment