RobotUnitModuleControllerManagement.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "RobotUnitModuleControllerManagement.h"
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
 
namespace armarx::RobotUnitModule
{
    template <class Cont>
    static Ice::StringSeq
    GetNonNullNames(const Cont& c)
    {
        Ice::StringSeq result;
        result.reserve(c.size());
        for (const auto& e : c)
        {
            if (e)
            {
                result.emplace_back(e->getName());
            }
        }
        return result;
    }
 
    /**
     * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref ControllerManagement.
     * \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
     */
    class NJointControllerAttorneyForControllerManagement
    {
        friend class ControllerManagement;
 
        static void
        SetRequested(const NJointControllerBasePtr& nJointCtrl, bool requested)
        {
            nJointCtrl->isRequested = requested;
        }
    };
} // namespace armarx::RobotUnitModule
 
namespace armarx::RobotUnitModule
{
    Ice::StringSeq
    ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(__PRETTY_FUNCTION__);
        return GetNonNullNames(_module<ControlThreadDataBuffer>().copyRequestedNJointControllers());
    }
 
    Ice::StringSeq
    ControllerManagement::getActivatedNJointControllerNames(const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        return GetNonNullNames(_module<ControlThreadDataBuffer>().getActivatedNJointControllers());
    }
 
    void
    ControllerManagement::checkNJointControllerClassName(const std::string& className) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        if (!NJointControllerRegistry::has(className))
        {
            std::stringstream ss;
            ss << "Requested controller class '" << className
               << "' unknown! Known classes:" << NJointControllerRegistry::getKeys()
               << " (If this class exists in a different lib then load it in the property "
                  "definitions of the RT-unit. DO NOT load it via "
                  "loadLibFromPath(path) or loadLibFromPackage(package, lib)) (see "
                  "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/documentation/-/"
                  "issues/85)";
            ARMARX_ERROR << ss.str();
            throw InvalidArgumentException{ss.str()};
        }
    }
 
    std::vector<NJointControllerBasePtr>
    ControllerManagement::getNJointControllersNotNull(const std::vector<std::string>& names) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        std::vector<NJointControllerBasePtr> ctrl;
        ctrl.reserve(names.size());
        for (const auto& name : names)
        {
            ctrl.emplace_back(getNJointControllerNotNull(name));
        }
        return ctrl;
    }
 
    const NJointControllerBasePtr&
    ControllerManagement::getNJointControllerNotNull(const std::string& name) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        auto it = nJointControllers.find(name);
        if (it == nJointControllers.end())
        {
            std::stringstream ss;
            ss << "RobotUnit: there is no NJointControllerBase with name '" << name
               << "'. Existing NJointControllers are: " << getNJointControllerNames();
            throw InvalidArgumentException{ss.str()};
        }
        if (!it->second)
        {
            std::stringstream ss;
            ss << "RobotUnit: The NJointControllerBase with name '" << name
               << "'. Is a nullptr! This should never be the case (invariant)!  \nMap:\n"
               << nJointControllers;
            ARMARX_FATAL << ss.str();
            throw InvalidArgumentException{ss.str()};
        }
        return it->second;
    }
 
    void
    ControllerManagement::deleteNJointController(const NJointControllerBasePtr& ctrl)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deleteNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
    }
 
    StringNJointControllerPrxDictionary
    ControllerManagement::getAllNJointControllers(const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
        {
            auto guard = getGuard();
            //copy to keep lock retention time low
            nJointControllersCopy = nJointControllers;
        }
        StringNJointControllerPrxDictionary result;
        for (const auto& pair : nJointControllersCopy)
        {
            result[pair.first] =
                NJointControllerInterfacePrx::uncheckedCast(pair.second->getProxy(-1, true));
        }
        return result;
    }
 
    NJointControllerInterfacePrx
    ControllerManagement::createNJointController(const std::string& className,
                                                 const std::string& instanceName,
                                                 const NJointControllerConfigPtr& config,
                                                 const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        //no lock required
        return NJointControllerInterfacePrx::uncheckedCast(
            createNJointController(className, instanceName, config, true, false)
                ->getProxy(-1, true));
    }
 
    NJointControllerInterfacePrx
    ControllerManagement::createNJointControllerFromVariantConfig(
        const std::string& className,
        const std::string& instanceName,
        const StringVariantBaseMap& variants,
        const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        //no lock required
        checkNJointControllerClassName(className);
        if (!NJointControllerRegistry::get(className)->hasRemoteConfiguration())
        {
            std::stringstream ss;
            ss << "Requested controller class '" << className << "' allows no remote configuration"
               << NJointControllerRegistry::getKeys()
               << " (Implement 'static WidgetDescription::WidgetPtr " << className
               << "::GenerateConfigDescription()'"
               << " and 'static NJointControllerConfigPtr " << className
               << "::GenerateConfigFromVariants(const StringVariantBaseMap&)' to allow remote "
                  "configuration";
            ARMARX_ERROR << ss.str();
            throw InvalidArgumentException{ss.str()};
        }
        return createNJointController(
            className,
            instanceName,
            NJointControllerRegistry::get(className)->GenerateConfigFromVariants(variants),
            Ice::emptyCurrent /*to select ice overload*/);
    }
 
    NJointControllerInterfacePrx
    ControllerManagement::createOrReplaceNJointController(const std::string& className,
                                                          const std::string& instanceName,
                                                          const NJointControllerConfigPtr& config,
                                                          const Ice::Current&)
    {
        ARMARX_TRACE;
        throwIfInControlThread(__PRETTY_FUNCTION__);
        {
            ARMARX_TRACE;
            auto guard = getGuard();
            if (getNJointController(instanceName))
            {
                ARMARX_TRACE;
                deactivateAndDeleteNJointController(instanceName);
            }
        }
        ARMARX_TRACE;
        while (getNJointController(instanceName))
        {
            ARMARX_TRACE;
            if (isShuttingDown())
            {
                ARMARX_TRACE;
                return nullptr;
            }
            ARMARX_INFO << "wiating until controller '" << instanceName << "' is deleted";
        }
        while (getArmarXManager()->getIceManager()->isObjectReachable(instanceName))
        {
            ARMARX_TRACE;
            if (isShuttingDown())
            {
                ARMARX_TRACE;
                return nullptr;
            }
            ARMARX_INFO << "wiating until controller '" << instanceName << "' is removed from ice";
        }
        return NJointControllerInterfacePrx::uncheckedCast(
            createNJointController(className, instanceName, config, true, false)
                ->getProxy(-1, true));
    }
 
    const NJointControllerBasePtr&
    ControllerManagement::createNJointController(const std::string& className,
                                                 const std::string& instanceName,
                                                 const NJointControllerConfigPtr& config,
                                                 bool deletable,
                                                 bool internal)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        if (instanceName.empty())
        {
            ARMARX_ERROR << "The instance name is empty! (give a unique name)";
            throw InvalidArgumentException{"The instance name is empty! (give a unique name)"};
        }
        //check if we would be able to create the class
        checkNJointControllerClassName(className);
        auto& factory = NJointControllerRegistry::get(className);
 
        //check if the instance name is already in use
        if (nJointControllers.count(instanceName))
        {
            std::stringstream ss;
            ss << "There already is a controller instance with the name '" << instanceName
               << "'. Use a different instance name instead."
               << " Other used instance names are " << getNJointControllerNames();
            ARMARX_ERROR << ss.str();
            throw InvalidArgumentException{ss.str()};
        }
        if (nJointControllersToBeDeleted.count(instanceName))
        {
            std::stringstream ss;
            ss << "The controller instance '" << instanceName
               << "' is still pending deletion. Retry later or use a different name.";
            ARMARX_ERROR << ss.str();
            throw InvalidArgumentException{ss.str()};
        }
 
        //create the controller
        ARMARX_CHECK_EXPRESSION(factory);
        NJointControllerBasePtr nJointCtrl =
            factory->create(this, config, controllerCreateRobot, deletable, internal, instanceName);
        ARMARX_CHECK_NOT_EQUAL(nJointCtrl->getControlDeviceUsedIndices().size(), 0)
            << "The NJointControllerBase '" << nJointCtrl->getName()
            << "' uses no ControlDevice! (It has to use at least one)";
 
        getArmarXManager()->addObject(nJointCtrl, instanceName, false, false);
        nJointControllers[instanceName] = std::move(nJointCtrl);
        _module<Publisher>().getRobotUnitListenerProxy()->nJointControllerCreated(instanceName);
        return nJointControllers.at(instanceName);
    }
 
    Ice::StringSeq
    ControllerManagement::getNJointControllerClassNames(const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        return NJointControllerRegistry::getKeys();
    }
 
    Ice::StringSeq
    ControllerManagement::getNJointControllerNames(const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        return getMapKeys(nJointControllers);
    }
 
    std::vector<std::string>
    ControllerManagement::getNJointControllerNames(
        const std::vector<NJointControllerBasePtr>& ctrls) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        std::vector<std::string> result;
        result.reserve(ctrls.size());
        for (const auto& ctrl : ctrls)
        {
            if (ctrl)
            {
                result.emplace_back(ctrl->getInstanceName());
            }
        }
        return result;
    }
 
    void
    ControllerManagement::activateNJointController(const std::string& name, const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        activateNJointControllers(getNJointControllersNotNull({name}));
    }
 
    void
    ControllerManagement::activateNJointControllers(const Ice::StringSeq& names,
                                                    const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        activateNJointControllers(getNJointControllersNotNull(names));
    }
 
    void
    ControllerManagement::activateNJointController(const NJointControllerBasePtr& ctrl)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        activateNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
    }
 
    void
    ControllerManagement::activateNJointControllers(
        const std::vector<NJointControllerBasePtr>& ctrlsToActVec)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        if (ctrlsToActVec.empty())
        {
            return;
        }
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        //if not activate them
        std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrlsToAct{
            ctrlsToActVec.begin(), ctrlsToActVec.end()};
        ARMARX_CHECK_EXPRESSION(!ctrlsToAct.count(nullptr));
        //check if all already active
        if (std::all_of(ctrlsToActVec.begin(),
                        ctrlsToActVec.end(),
                        [](const NJointControllerBasePtr& ctrl)
                        { return ctrl->isControllerActive(); }))
        {
            return;
        }
        //get already requested
        const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
        std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>>
            ctrlsAlreadyRequested{ctrlVector.begin(), ctrlVector.end()};
        ctrlsAlreadyRequested.erase(nullptr);
        //check for conflict
        std::vector<char> inuse;
        //check requested controllers
        {
            auto r = NJointControllerBase::AreNotInConflict(ctrlsToAct.begin(), ctrlsToAct.end());
            if (!r)
            {
                std::stringstream ss;
                ss << "activateNJointControllers: requested controllers are in "
                      "conflict!\ncontrollers:\n"
                   << getNJointControllerNames(ctrlsToActVec);
                ARMARX_ERROR << ss.str();
                throw InvalidArgumentException{ss.str()};
            }
            inuse = std::move(*r);
        }
        ARMARX_DEBUG << "all requested controllers are conflict free" << std::flush;
        auto printInUse = ARMARX_STREAM_PRINTER
        {
            for (const auto c : inuse)
            {
                out << (c ? 1 : 0);
            }
        };
        ARMARX_DEBUG << "inuse field (request)\n" << printInUse;
        //add already active controllers if they are conflict free
        {
            if (ctrlsAlreadyRequested.empty())
            {
                ARMARX_DEBUG << "no already requested NJointControllers";
            }
            for (const NJointControllerBasePtr& nJointCtrl : ctrlsAlreadyRequested)
            {
                if (ctrlsToAct.count(nJointCtrl))
                {
                    continue;
                }
                auto r = nJointCtrl->isNotInConflictWith(inuse);
                if (r)
                {
                    ARMARX_DEBUG << "keeping  already requested NJointControllerBase '"
                                 << nJointCtrl->getInstanceName()
                                 << "' in   list of requested controllers";
                    ctrlsToAct.insert(nJointCtrl);
                    inuse = std::move(*r);
                }
                else
                {
                    ARMARX_INFO << "removing already requested NJointControllerBase '"
                                << nJointCtrl->getInstanceName()
                                << "' from list of requested controllers";
                }
            }
            ARMARX_DEBUG << "inuse field (all)\n" << printInUse;
        }
        _module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrlsToAct);
    }
 
    void
    ControllerManagement::deactivateNJointController(const std::string& name, const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deactivateNJointControllers(getNJointControllersNotNull({name}));
    }
 
    void
    ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names,
                                                      const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deactivateNJointControllers(getNJointControllersNotNull(names));
    }
 
    void
    ControllerManagement::deactivateNJointController(const NJointControllerBasePtr& ctrl)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deactivateNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
    }
 
    void
    ControllerManagement::deactivateNJointControllers(
        const std::vector<NJointControllerBasePtr>& ctrlsDeacVec)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        if (ctrlsDeacVec.empty())
        {
            return;
        }
        const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
        std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls{
            ctrlVector.begin(), ctrlVector.end()};
        const std::size_t ctrlsNum = ctrls.size();
        for (const auto& nJointCtrlToDeactivate : ctrlsDeacVec)
        {
            ctrls.erase(nJointCtrlToDeactivate);
        }
        if (ctrls.size() == ctrlsNum)
        {
            return;
        }
        _module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrls);
    }
 
    void
    ControllerManagement::deleteNJointController(const std::string& name, const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deleteNJointControllers(getNJointControllersNotNull({name}));
    }
 
    void
    ControllerManagement::deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deleteNJointControllers(getNJointControllersNotNull(names));
    }
 
    void
    ControllerManagement::switchNJointControllerSetup(const Ice::StringSeq& newSetup,
                                                      const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        auto ctrlsToActVec =
            getNJointControllersNotNull(newSetup); //also checks if these controllers exist
        _module<ControlThreadDataBuffer>().setActivateControllersRequest(
            {ctrlsToActVec.begin(), ctrlsToActVec.end()});
    }
 
    void
    ControllerManagement::deleteNJointControllers(
        const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        if (ctrlsToDelVec.empty())
        {
            return;
        }
        //check if all can be deleted
        for (const auto& nJointCtrl : ctrlsToDelVec)
        {
            if (!nJointCtrl->isDeletable())
            {
                throw LogicError{"The NJointControllerBase '" + nJointCtrl->getInstanceName() +
                                 "' can't be deleted since this operation is not allowed for this "
                                 "controller! (no NJointControllerBase was deleted)"};
            }
            if (nJointCtrl->isControllerActive() || nJointCtrl->isControllerRequested())
            {
                throw LogicError{"The NJointControllerBase '" + nJointCtrl->getInstanceName() +
                                 "' can't be deleted since it is active or requested! (no "
                                 "NJointControllerBase was deleted)"};
            }
        }
        for (const auto& nJointCtrl : ctrlsToDelVec)
        {
            const auto name = nJointCtrl->getInstanceName();
            //deletion is done in a different thread since this call may be done by the controller (protection against use after free)
            ARMARX_CHECK_EQUAL(nJointControllersToBeDeleted.count(name), 0)
                << "The controller '" << name << "' is already pending deletion";
            nJointControllersToBeDeleted[name] = std::move(nJointCtrl);
            nJointControllers.erase(name);
            ARMARX_VERBOSE << "added NJointControllerBase '" << name << "' to be deleted";
        }
    }
 
    void
    ControllerManagement::deactivateAndDeleteNJointController(const std::string& name,
                                                              const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deactivateAndDeleteNJointControllers(getNJointControllersNotNull({name}));
    }
 
    void
    ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names,
                                                               const Ice::Current&)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deactivateAndDeleteNJointControllers(getNJointControllersNotNull(names));
    }
 
    void
    ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerBasePtr& ctrl)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        deactivateAndDeleteNJointControllers({ctrl});
    }
 
    void
    ControllerManagement::deactivateAndDeleteNJointControllers(
        const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        if (ctrlsToDelVec.empty())
        {
            return;
        }
        deactivateNJointControllers(ctrlsToDelVec);
        while (std::any_of(ctrlsToDelVec.begin(),
                           ctrlsToDelVec.end(),
                           [](const NJointControllerBasePtr& ctrl)
                           { return ctrl->isControllerActive(); }))
        {
            if (isShuttingDown())
            {
                return;
            }
            std::this_thread::sleep_for(std::chrono::microseconds{100});
        }
        deleteNJointControllers(ctrlsToDelVec);
    }
 
    NJointControllerClassDescription
    ControllerManagement::getNJointControllerClassDescription(const std::string& className,
                                                              const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        while (getRobotUnitState() == RobotUnitState::InitializingComponent ||
               getRobotUnitState() == RobotUnitState::InitializingDevices)
        {
            //this phase should only last short so busy waiting is ok
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        checkNJointControllerClassName(className);
        NJointControllerClassDescription data;
        data.className = className;
        if (NJointControllerRegistry::get(className)->hasRemoteConfiguration())
        {
            data.configDescription =
                NJointControllerRegistry::get(className)->GenerateConfigDescription(
                    controllerCreateRobot,
                    _module<Devices>().getControlDevicesConstPtr(),
                    _module<Devices>().getSensorDevicesConstPtr());
        }
        return data;
    }
 
    NJointControllerClassDescriptionSeq
    ControllerManagement::getNJointControllerClassDescriptions(const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        std::size_t tries = 200;
        while (getRobotUnitState() == RobotUnitState::InitializingComponent ||
               getRobotUnitState() == RobotUnitState::InitializingDevices)
        {
            //this phase should only last short so busy waiting is ok
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            if (!--tries)
            {
                throw RuntimeError{"RobotUnit::getNJointControllerClassDescriptions: it took too "
                                   "long to for the unit to get in a valid state"};
            }
        }
        auto guard = getGuard();
        NJointControllerClassDescriptionSeq r;
        r.reserve(NJointControllerRegistry::getKeys().size());
        for (const auto& key : NJointControllerRegistry::getKeys())
        {
            r.emplace_back(getNJointControllerClassDescription(key));
        }
        return r;
    }
 
    NJointControllerInterfacePrx
    ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        NJointControllerBasePtr ctrl;
        {
            auto guard = getGuard();
            auto it = nJointControllers.find(name);
            if (it == nJointControllers.end())
            {
                return nullptr;
            }
            ctrl = it->second;
        }
        return NJointControllerInterfacePrx::uncheckedCast(ctrl->getProxy(-1, true));
    }
 
    NJointControllerStatus
    ControllerManagement::getNJointControllerStatus(const std::string& name,
                                                    const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        throwIfDevicesNotReady(__FUNCTION__);
        return getNJointControllerNotNull(name)->getControllerStatus();
    }
 
    NJointControllerStatusSeq
    ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        auto guard = getGuard();
        if (!areDevicesReady())
        {
            return {};
        }
        NJointControllerStatusSeq r;
        r.reserve(nJointControllers.size());
        for (const auto& nJointCtrl : nJointControllers)
        {
            r.emplace_back(nJointCtrl.second->getControllerStatus());
        }
        return r;
    }
 
    NJointControllerDescription
    ControllerManagement::getNJointControllerDescription(const std::string& name,
                                                         const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        NJointControllerBasePtr nJointCtrl;
        {
            auto guard = getGuard();
            throwIfDevicesNotReady(__FUNCTION__);
            nJointCtrl = getNJointControllerNotNull(name);
        }
        return nJointCtrl->getControllerDescription();
    }
 
    NJointControllerDescriptionSeq
    ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
        {
            auto guard = getGuard();
            if (!areDevicesReady())
            {
                return {};
            }
            nJointControllersCopy = nJointControllers;
        }
        NJointControllerDescriptionSeq r;
        r.reserve(nJointControllersCopy.size());
        for (const auto& nJointCtrl : nJointControllersCopy)
        {
            r.emplace_back(nJointCtrl.second->getControllerDescription());
        }
        return r;
    }
 
    NJointControllerDescriptionWithStatus
    ControllerManagement::getNJointControllerDescriptionWithStatus(const std::string& name,
                                                                   const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        NJointControllerBasePtr nJointCtrl;
        {
            auto guard = getGuard();
            throwIfDevicesNotReady(__FUNCTION__);
            nJointCtrl = getNJointControllerNotNull(name);
        }
        return nJointCtrl->getControllerDescriptionWithStatus();
    }
 
    NJointControllerDescriptionWithStatusSeq
    ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
        {
            auto guard = getGuard();
            if (!areDevicesReady())
            {
                return {};
            }
            nJointControllersCopy = nJointControllers;
        }
        NJointControllerDescriptionWithStatusSeq r;
        r.reserve(nJointControllersCopy.size());
        for (const auto& nJointCtrl : nJointControllersCopy)
        {
            r.emplace_back(nJointCtrl.second->getControllerDescriptionWithStatus());
        }
        return r;
    }
 
    void
    ControllerManagement::removeNJointControllers(
        std::map<std::string, NJointControllerBasePtr>& ctrls,
        bool blocking,
        RobotUnitListenerPrx l)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        for (auto& n2NJointCtrl : ctrls)
        {
            NJointControllerBasePtr& nJointCtrl = n2NJointCtrl.second;
            if (blocking)
            {
                ARMARX_VERBOSE << "deleted NJointControllerBase " << n2NJointCtrl.first;
                getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
            }
            else
            {
                ARMARX_VERBOSE << "deleted NJointControllerBase " << n2NJointCtrl.first;
                getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
            }
            if (l)
            {
                l->nJointControllerDeleted(n2NJointCtrl.first);
            }
        }
        ctrls.clear();
    }
 
    void
    ControllerManagement::removeNJointControllersToBeDeleted(bool blocking, RobotUnitListenerPrx l)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        removeNJointControllers(nJointControllersToBeDeleted, blocking, l);
    }
 
    void
    ControllerManagement::_preFinishRunning()
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        //NJoint queued for deletion (some could still be in the queue)
        ARMARX_DEBUG << "remove NJointControllers queued for deletion";
        removeNJointControllersToBeDeleted();
        ARMARX_DEBUG << "remove NJointControllers queued for deletion...done";
        //NJoint
        ARMARX_DEBUG << "remove NJointControllers";
        removeNJointControllers(nJointControllers);
        ARMARX_DEBUG << "remove NJointControllers...done";
    }
 
    void
    ControllerManagement::_postFinishRunning()
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        nJointControllers.clear();
    }
 
    ControllerManagement::~ControllerManagement()
    {
    }
 
    void
    ControllerManagement::_preOnInitRobotUnit()
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        controllerCreateRobot = _module<RobotData>().cloneRobot();
    }
 
    void
    ControllerManagement::updateNJointControllerRequestedState(
        const std::set<NJointControllerBasePtr>& request)
    {
        throwIfInControlThread(__PRETTY_FUNCTION__);
        ARMARX_DEBUG << "set requested state for NJoint controllers";
        for (const auto& name2NJoint : nJointControllers)
        {
            NJointControllerAttorneyForControllerManagement::SetRequested(
                name2NJoint.second, request.count(name2NJoint.second));
        }
    }
} // namespace armarx::RobotUnitModule