Forked from
Software / ArmarX / RobotAPI
924 commits behind the upstream repository.
RobotUnitModuleControllerManagement.cpp 33.66 KiB
/*
* 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(BOOST_CURRENT_FUNCTION);
return GetNonNullNames(_module<ControlThreadDataBuffer>().copyRequestedNJointControllers());
}
Ice::StringSeq
ControllerManagement::getActivatedNJointControllerNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return GetNonNullNames(_module<ControlThreadDataBuffer>().getActivatedNJointControllers());
}
void
ControllerManagement::checkNJointControllerClassName(const std::string& className) const
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
}
StringNJointControllerPrxDictionary
ControllerManagement::getAllNJointControllers(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_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()};
}
//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);
}
bool
ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_WARNING << "Do not use this function as it has implications on the RT thread (see "
"https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/"
"documentation/-/issues/85)";
const bool result = getArmarXManager()->loadLibFromPath(path);
ARMARX_INFO << "loadLibFromPath('" << path << "') -> " << result;
return result;
}
bool
ControllerManagement::loadLibFromPackage(const std::string& package,
const std::string& lib,
const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_WARNING << "Do not use this function as it has implications on the RT thread (see "
"https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/"
"documentation/-/issues/85)";
const bool result = getArmarXManager()->loadLibFromPackage(package, lib);
ARMARX_INFO << "loadLibFromPackage('" << package << "', '" << lib << "') -> " << result;
return result;
}
Ice::StringSeq
ControllerManagement::getNJointControllerClassNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return NJointControllerRegistry::getKeys();
}
Ice::StringSeq
ControllerManagement::getNJointControllerNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
return getMapKeys(nJointControllers);
}
std::vector<std::string>
ControllerManagement::getNJointControllerNames(
const std::vector<NJointControllerBasePtr>& ctrls) const
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION);
activateNJointControllers(getNJointControllersNotNull({name}));
}
void
ControllerManagement::activateNJointControllers(const Ice::StringSeq& names,
const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers(getNJointControllersNotNull(names));
}
void
ControllerManagement::activateNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
}
void
ControllerManagement::activateNJointControllers(
const std::vector<NJointControllerBasePtr>& ctrlsToActVec)
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(getNJointControllersNotNull({name}));
}
void
ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names,
const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(getNJointControllersNotNull(names));
}
void
ControllerManagement::deactivateNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(std::vector<NJointControllerBasePtr>{ctrl}); }
void
ControllerManagement::deactivateNJointControllers(
const std::vector<NJointControllerBasePtr>& ctrlsDeacVec)
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(getNJointControllersNotNull({name}));
}
void
ControllerManagement::deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(getNJointControllersNotNull(names));
}
void
ControllerManagement::switchNJointControllerSetup(const Ice::StringSeq& newSetup,
const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_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)
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(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers(getNJointControllersNotNull({name}));
}
void
ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names,
const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers(getNJointControllersNotNull(names));
}
void
ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers({ctrl});
}
void
ControllerManagement::deactivateAndDeleteNJointControllers(
const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
return getNJointControllerNotNull(name)->getControllerStatus();
}
NJointControllerStatusSeq
ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION);
NJointControllerBasePtr nJointCtrl;
{
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
nJointCtrl = getNJointControllerNotNull(name);
}
return nJointCtrl->getControllerDescription();
}
NJointControllerDescriptionSeq
ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION); NJointControllerBasePtr nJointCtrl;
{
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
nJointCtrl = getNJointControllerNotNull(name);
}
return nJointCtrl->getControllerDescriptionWithStatus();
}
NJointControllerDescriptionWithStatusSeq
ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION);
removeNJointControllers(nJointControllersToBeDeleted, blocking, l);
}
void
ControllerManagement::_preFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_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(BOOST_CURRENT_FUNCTION);
nJointControllers.clear();
}
ControllerManagement::~ControllerManagement()
{
}
void
ControllerManagement::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controllerCreateRobot = _module<RobotData>().cloneRobot();
}
void
ControllerManagement::updateNJointControllerRequestedState(
const std::set<NJointControllerBasePtr>& request)
{
throwIfInControlThread(BOOST_CURRENT_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