added doxygen and an addLVL0Controller function

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);
 }
 
 

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;