42 GetNonNullNames(
const Cont&
c)
44 Ice::StringSeq result;
45 result.reserve(
c.size());
46 for (
const auto& e :
c)
50 result.emplace_back(e->getName());
65 SetRequested(
const NJointControllerBasePtr& nJointCtrl,
bool requested)
67 nJointCtrl->isRequested = requested;
79 return GetNonNullNames(_module<ControlThreadDataBuffer>().copyRequestedNJointControllers());
86 return GetNonNullNames(_module<ControlThreadDataBuffer>().getActivatedNJointControllers());
90 ControllerManagement::checkNJointControllerClassName(
const std::string& className)
const
96 ss <<
"Requested controller class '" << className
98 <<
" (If this class exists in a different lib then load it in the property "
99 "definitions of the RT-unit. DO NOT load it via "
100 "loadLibFromPath(path) or loadLibFromPackage(package, lib)) (see "
101 "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/documentation/-/"
104 throw InvalidArgumentException{ss.str()};
108 std::vector<NJointControllerBasePtr>
114 std::vector<NJointControllerBasePtr> ctrl;
115 ctrl.reserve(
names.size());
116 for (
const auto& name :
names)
123 const NJointControllerBasePtr&
129 auto it = nJointControllers.find(name);
130 if (it == nJointControllers.end())
132 std::stringstream ss;
133 ss <<
"RobotUnit: there is no NJointControllerBase with name '" << name
135 throw InvalidArgumentException{ss.str()};
139 std::stringstream ss;
140 ss <<
"RobotUnit: The NJointControllerBase with name '" << name
141 <<
"'. Is a nullptr! This should never be the case (invariant)! \nMap:\n"
142 << nJointControllers;
144 throw InvalidArgumentException{ss.str()};
156 StringNJointControllerPrxDictionary
160 std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
164 nJointControllersCopy = nJointControllers;
166 StringNJointControllerPrxDictionary result;
167 for (
const auto& pair : nJointControllersCopy)
170 NJointControllerInterfacePrx::uncheckedCast(pair.second->getProxy(-1,
true));
175 NJointControllerInterfacePrx
177 const std::string& instanceName,
178 const NJointControllerConfigPtr& config,
183 return NJointControllerInterfacePrx::uncheckedCast(
188 NJointControllerInterfacePrx
190 const std::string& className,
191 const std::string& instanceName,
197 checkNJointControllerClassName(className);
200 std::stringstream ss;
201 ss <<
"Requested controller class '" << className <<
"' allows no remote configuration"
203 <<
" (Implement 'static WidgetDescription::WidgetPtr " << className
204 <<
"::GenerateConfigDescription()'"
205 <<
" and 'static NJointControllerConfigPtr " << className
206 <<
"::GenerateConfigFromVariants(const StringVariantBaseMap&)' to allow remote "
209 throw InvalidArgumentException{ss.str()};
218 NJointControllerInterfacePrx
220 const std::string& instanceName,
221 const NJointControllerConfigPtr& config,
244 ARMARX_INFO <<
"wiating until controller '" << instanceName <<
"' is deleted";
254 ARMARX_INFO <<
"wiating until controller '" << instanceName <<
"' is removed from ice";
256 return NJointControllerInterfacePrx::uncheckedCast(
261 const NJointControllerBasePtr&
263 const std::string& instanceName,
264 const NJointControllerConfigPtr& config,
271 if (instanceName.empty())
273 ARMARX_ERROR <<
"The instance name is empty! (give a unique name)";
274 throw InvalidArgumentException{
"The instance name is empty! (give a unique name)"};
277 checkNJointControllerClassName(className);
281 if (nJointControllers.count(instanceName))
283 std::stringstream ss;
284 ss <<
"There already is a controller instance with the name '" << instanceName
285 <<
"'. Use a different instance name instead."
288 throw InvalidArgumentException{ss.str()};
293 NJointControllerBasePtr nJointCtrl =
294 factory->create(
this, config, controllerCreateRobot, deletable,
internal, instanceName);
296 <<
"The NJointControllerBase '" << nJointCtrl->getName()
297 <<
"' uses no ControlDevice! (It has to use at least one)";
300 nJointControllers[instanceName] = std::move(nJointCtrl);
301 _module<Publisher>().getRobotUnitListenerProxy()->nJointControllerCreated(instanceName);
302 return nJointControllers.at(instanceName);
309 ARMARX_WARNING <<
"Do not use this function as it has implications on the RT thread (see "
310 "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/"
311 "documentation/-/issues/85)";
313 ARMARX_INFO <<
"loadLibFromPath('" << path <<
"') -> " << result;
319 const std::string& lib,
323 ARMARX_WARNING <<
"Do not use this function as it has implications on the RT thread (see "
324 "https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/"
325 "documentation/-/issues/85)";
327 ARMARX_INFO <<
"loadLibFromPackage('" <<
package << "', '" << lib << "') -> " << result;
346 std::vector<std::string>
348 const std::vector<NJointControllerBasePtr>& ctrls)
const
351 std::vector<std::string> result;
352 result.reserve(ctrls.size());
353 for (
const auto& ctrl : ctrls)
357 result.emplace_back(ctrl->getInstanceName());
387 const std::vector<NJointControllerBasePtr>& ctrlsToActVec)
390 if (ctrlsToActVec.empty())
397 std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrlsToAct{
398 ctrlsToActVec.begin(), ctrlsToActVec.end()};
401 if (std::all_of(ctrlsToActVec.begin(),
403 [](
const NJointControllerBasePtr& ctrl)
404 { return ctrl->isControllerActive(); }))
409 const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
410 std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>>
411 ctrlsAlreadyRequested{ctrlVector.begin(), ctrlVector.end()};
412 ctrlsAlreadyRequested.erase(
nullptr);
414 std::vector<char> inuse;
420 std::stringstream ss;
421 ss <<
"activateNJointControllers: requested controllers are in "
422 "conflict!\ncontrollers:\n"
425 throw InvalidArgumentException{ss.str()};
427 inuse = std::move(*r);
432 for (
const auto c : inuse)
437 ARMARX_DEBUG <<
"inuse field (request)\n" << printInUse;
440 if (ctrlsAlreadyRequested.empty())
442 ARMARX_DEBUG <<
"no already requested NJointControllers";
444 for (
const NJointControllerBasePtr& nJointCtrl : ctrlsAlreadyRequested)
446 if (ctrlsToAct.count(nJointCtrl))
450 auto r = nJointCtrl->isNotInConflictWith(inuse);
453 ARMARX_DEBUG <<
"keeping already requested NJointControllerBase '"
454 << nJointCtrl->getInstanceName()
455 <<
"' in list of requested controllers";
456 ctrlsToAct.insert(nJointCtrl);
457 inuse = std::move(*r);
461 ARMARX_INFO <<
"removing already requested NJointControllerBase '"
462 << nJointCtrl->getInstanceName()
463 <<
"' from list of requested controllers";
468 _module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrlsToAct);
495 const std::vector<NJointControllerBasePtr>& ctrlsDeacVec)
500 if (ctrlsDeacVec.empty())
504 const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
505 std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls{
506 ctrlVector.begin(), ctrlVector.end()};
507 const std::size_t ctrlsNum = ctrls.size();
508 for (
const auto& nJointCtrlToDeactivate : ctrlsDeacVec)
510 ctrls.erase(nJointCtrlToDeactivate);
512 if (ctrls.size() == ctrlsNum)
516 _module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrls);
542 _module<ControlThreadDataBuffer>().setActivateControllersRequest(
543 {ctrlsToActVec.begin(), ctrlsToActVec.end()});
548 const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
553 if (ctrlsToDelVec.empty())
558 for (
const auto& nJointCtrl : ctrlsToDelVec)
560 if (!nJointCtrl->isDeletable())
562 throw LogicError{
"The NJointControllerBase '" + nJointCtrl->getInstanceName() +
563 "' can't be deleted since this operation is not allowed for this "
564 "controller! (no NJointControllerBase was deleted)"};
566 if (nJointCtrl->isControllerActive() || nJointCtrl->isControllerRequested())
568 throw LogicError{
"The NJointControllerBase '" + nJointCtrl->getInstanceName() +
569 "' can't be deleted since it is active or requested! (no "
570 "NJointControllerBase was deleted)"};
573 for (
const auto& nJointCtrl : ctrlsToDelVec)
575 const auto name = nJointCtrl->getInstanceName();
577 nJointControllersToBeDeleted[name] = std::move(nJointCtrl);
578 nJointControllers.erase(name);
579 ARMARX_VERBOSE <<
"added NJointControllerBase '" << name <<
"' to be deleted";
608 const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
613 if (ctrlsToDelVec.empty())
618 while (std::any_of(ctrlsToDelVec.begin(),
620 [](
const NJointControllerBasePtr& ctrl)
621 { return ctrl->isControllerActive(); }))
627 std::this_thread::sleep_for(std::chrono::microseconds{100});
632 NJointControllerClassDescription
634 const Ice::Current&)
const
641 std::this_thread::sleep_for(std::chrono::milliseconds(50));
645 checkNJointControllerClassName(className);
646 NJointControllerClassDescription
data;
647 data.className = className;
650 data.configDescription =
652 controllerCreateRobot,
653 _module<Devices>().getControlDevicesConstPtr(),
654 _module<Devices>().getSensorDevicesConstPtr());
659 NJointControllerClassDescriptionSeq
663 std::size_t tries = 200;
668 std::this_thread::sleep_for(std::chrono::milliseconds(50));
671 throw RuntimeError{
"RobotUnit::getNJointControllerClassDescriptions: it took too "
672 "long to for the unit to get in a valid state"};
676 NJointControllerClassDescriptionSeq r;
685 NJointControllerInterfacePrx
689 NJointControllerBasePtr ctrl;
692 auto it = nJointControllers.find(name);
693 if (it == nJointControllers.end())
699 return NJointControllerInterfacePrx::uncheckedCast(ctrl->getProxy(-1,
true));
702 NJointControllerStatus
704 const Ice::Current&)
const
712 NJointControllerStatusSeq
721 NJointControllerStatusSeq r;
722 r.reserve(nJointControllers.size());
723 for (
const auto& nJointCtrl : nJointControllers)
725 r.emplace_back(nJointCtrl.second->getControllerStatus());
730 NJointControllerDescription
732 const Ice::Current&)
const
735 NJointControllerBasePtr nJointCtrl;
741 return nJointCtrl->getControllerDescription();
744 NJointControllerDescriptionSeq
748 std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
755 nJointControllersCopy = nJointControllers;
757 NJointControllerDescriptionSeq r;
758 r.reserve(nJointControllersCopy.size());
759 for (
const auto& nJointCtrl : nJointControllersCopy)
761 r.emplace_back(nJointCtrl.second->getControllerDescription());
766 NJointControllerDescriptionWithStatus
768 const Ice::Current&)
const
771 NJointControllerBasePtr nJointCtrl;
777 return nJointCtrl->getControllerDescriptionWithStatus();
780 NJointControllerDescriptionWithStatusSeq
784 std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
791 nJointControllersCopy = nJointControllers;
793 NJointControllerDescriptionWithStatusSeq r;
794 r.reserve(nJointControllersCopy.size());
795 for (
const auto& nJointCtrl : nJointControllersCopy)
797 r.emplace_back(nJointCtrl.second->getControllerDescriptionWithStatus());
803 ControllerManagement::removeNJointControllers(
804 std::map<std::string, NJointControllerBasePtr>& ctrls,
806 RobotUnitListenerPrx l)
809 for (
auto& n2NJointCtrl : ctrls)
811 NJointControllerBasePtr& nJointCtrl = n2NJointCtrl.second;
814 ARMARX_VERBOSE <<
"deleted NJointControllerBase " << n2NJointCtrl.first;
819 ARMARX_VERBOSE <<
"deleted NJointControllerBase " << n2NJointCtrl.first;
824 l->nJointControllerDeleted(n2NJointCtrl.first);
831 ControllerManagement::removeNJointControllersToBeDeleted(
bool blocking, RobotUnitListenerPrx l)
834 removeNJointControllers(nJointControllersToBeDeleted, blocking, l);
838 ControllerManagement::_preFinishRunning()
842 ARMARX_DEBUG <<
"remove NJointControllers queued for deletion";
843 removeNJointControllersToBeDeleted();
844 ARMARX_DEBUG <<
"remove NJointControllers queued for deletion...done";
847 removeNJointControllers(nJointControllers);
852 ControllerManagement::_postFinishRunning()
855 nJointControllers.clear();
863 ControllerManagement::_preOnInitRobotUnit()
866 controllerCreateRobot = _module<RobotData>().cloneRobot();
870 ControllerManagement::updateNJointControllerRequestedState(
871 const std::set<NJointControllerBasePtr>& request)
874 ARMARX_DEBUG <<
"set requested state for NJoint controllers";
875 for (
const auto& name2NJoint : nJointControllers)
877 NJointControllerAttorneyForControllerManagement::SetRequested(
878 name2NJoint.second, request.count(name2NJoint.second));