diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 722c1b20e5ea2029afba614c5a887f0a4abaefa4..d16b645a263a7f24f53ee47acaa23dc175ec1a98 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -56,6 +56,7 @@ set(LIB_HEADERS
Constants.h
ControlModes.h
RobotUnit.h
+ RobotUnit.ipp
RobotUnitObserver.h
BasicControllers.h
DefaultWidgetDescriptions.h
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index f64cca350770319a942e66d151c01927aff97c6c..5617cdd7a6b53aa8666270b9a0ba66a90aa532c0 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -698,376 +698,5 @@ namespace armarx
}
-//inline functions
-namespace armarx
-{
- //RobotUnit
- inline const KeyValueVector<std::string, SensorDevicePtr>& RobotUnit::getSensorDevices() const
- {
- return sensorDevices;
- }
-
- inline const KeyValueVector<std::string, ControlDevicePtr>& RobotUnit::getControlDevices() const
- {
- return controlDevices;
- }
-
- inline RobotUnit::TimePointT RobotUnit::Now()
- {
- return ClockT::now();
- }
-
- inline std::chrono::microseconds RobotUnit::MicroToNow(RobotUnit::TimePointT last)
- {
- return std::chrono::duration_cast<std::chrono::microseconds>(Now() - last);
- }
-
- inline std::chrono::microseconds RobotUnit::MicroNow()
- {
- return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
- }
-
- inline PropertyDefinitionsPtr RobotUnit::createPropertyDefinitions()
- {
- return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
- }
-
- inline std::string RobotUnit::getDefaultName() const
- {
- return "RobotUnit";
- }
-
- inline constexpr std::size_t RobotUnit::IndexSentinel()
- {
- return JointAndNJointControllers::Sentinel();
- }
-
- inline void RobotUnit::onInitRobotUnit() {}
-
- inline void RobotUnit::onConnectRobotUnit() {}
-
- inline void RobotUnit::onDisconnectRobotUnit() {}
-
- inline void RobotUnit::onExitRobotUnit() {}
-
- inline bool RobotUnit::rtRequestedControllersChanged() const
- {
- return controllersRequested.updateReadBuffer();
- }
-
- inline const JointAndNJointControllers& RobotUnit::rtGetRequestedControllers() const
- {
- return controllersRequested.getReadBuffer();
- }
-
- inline const std::vector<JointController*>& RobotUnit::rtGetRequestedJointControllers() const
- {
- return rtGetRequestedControllers().jointControllers;
- }
-
- inline const std::vector<NJointController*>& RobotUnit::rtGetRequestedNJointControllers() const
- {
- return rtGetRequestedControllers().nJointControllers;
- }
-
- inline void RobotUnit::rtWriteActivatedControllers()
- {
- controllersActivated.commitWrite();
- }
-
- inline JointAndNJointControllers& RobotUnit::rtGetActivatedControllers()
- {
- return controllersActivated.getWriteBuffer();
- }
-
- inline std::vector<JointController*>& RobotUnit::rtGetActivatedJointControllers()
- {
- return rtGetActivatedControllers().jointControllers;
- }
-
- inline std::vector<NJointController*>& RobotUnit::rtGetActivatedNJointControllers()
- {
- return rtGetActivatedControllers().nJointControllers;
- }
-
- inline std::vector<std::size_t>& RobotUnit::rtGetActivatedJointToNJointControllerAssignement()
- {
- return rtGetActivatedControllers().jointToNJointControllerAssignement;
- }
-
- inline RTThreadTimingsSensorDevicePtr RobotUnit::createRTThreadTimingSensorDevice() const
- {
- return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
- }
-
- inline void RobotUnit::rtDeactivatedNJointControllerBecauseOfError(const NJointControllerPtr&) {}
-
- inline RTThreadTimingsSensorDevice& RobotUnit::rtGetRTThreadTimingsSensorDevice()
- {
- return *rtThreadTimingsSensorDevice;
- }
-
- inline NJointControllerInterfacePrx RobotUnit::createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current&
- )
- {
- //no lock required
- return NJointControllerInterfacePrx::uncheckedCast(
- createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
- }
-
- inline void RobotUnit::deleteNJointController(const std::string& name, const Ice::Current&)
- {
- deleteNJointControllers({name});
- }
-
- inline void RobotUnit::activateNJointController(const std::string& name, const Ice::Current&)
- {
- activateNJointControllers({name});
- }
-
- inline void RobotUnit::deactivateNJointController(const std::string& name, const Ice::Current&)
- {
- deactivateNJointControllers(Ice::StringSeq {name});
- }
-
- inline void RobotUnit::checkSetOfControllersToActivate(const JointAndNJointControllers&) const {}
-
- inline Ice::StringSeq RobotUnit::getNJointControllerNames(const Ice::Current&) const
- {
- auto guard = getGuard();
- return getMapKeys(nJointControllers);
- }
-
- inline Ice::StringSeq RobotUnit::getRequestedNJointControllerNames(const Ice::Current&) const
- {
- auto guard = getGuard();
- return GetNonNullNames(getRequestedNJointControllers());
- }
-
- inline Ice::StringSeq RobotUnit::getActivatedNJointControllerNames(const Ice::Current&) const
- {
- auto guard = getGuard();
- return GetNonNullNames(getActivatedNJointControllers());
- }
-
- inline Ice::StringSeq RobotUnit::getNJointControllerClassNames(const Ice::Current&) const
- {
- return NJointControllerRegistry::getKeys();
- }
-
- inline KinematicUnitInterfacePrx RobotUnit::getKinematicUnit(const Ice::Current&) const
- {
- //no lock required
- return KinematicUnitInterfacePrx::uncheckedCast(getUnit(KinematicUnitInterface::ice_staticId(), GlobalIceCurrent));
- }
-
- inline ForceTorqueUnitInterfacePrx RobotUnit::getForceTorqueUnit(const Ice::Current&) const
- {
- //no lock required
- return ForceTorqueUnitInterfacePrx::uncheckedCast(getUnit(ForceTorqueUnitInterface::ice_staticId(), GlobalIceCurrent));
- }
-
- inline InertialMeasurementUnitInterfacePrx RobotUnit::getInertialMeasurementUnit(const Ice::Current&) const
- {
- //no lock required
- return InertialMeasurementUnitInterfacePrx::uncheckedCast(
- getUnit(InertialMeasurementUnitInterface::ice_staticId(), GlobalIceCurrent));
- }
-
- inline PlatformUnitInterfacePrx RobotUnit::getPlatformUnit(const Ice::Current&) const
- {
- //no lock required
- return PlatformUnitInterfacePrx::uncheckedCast(getUnit(PlatformUnitInterface::ice_staticId(), GlobalIceCurrent));
- }
-
- inline std::string RobotUnit::getListenerTopicName(const Ice::Current&) const
- {
- auto guard = getGuard();
- return robotUnitListenerTopicName;
- }
-
- inline const std::string& RobotUnit::getRobotNodetSeName() const
- {
- return robotNodeSetName;
- }
-
- inline const std::string& RobotUnit::getRobotProjectName() const
- {
- return robotProjectName;
- }
-
- inline const std::string& RobotUnit::getRobotFileName() const
- {
- return robotFileName;
- }
-
- inline const std::string& RobotUnit::getRobotPlatformName() const
- {
- return robotPlatformName;
- }
-
- inline std::size_t RobotUnit::rtGetNumberOfControlDevices() const
- {
- return controlDevices.size();
- }
-
- inline std::size_t RobotUnit::rtGetNumberOfSensorDevices() const
- {
- return sensorDevices.size();
- }
-
- inline bool RobotUnit::areDevicesReady() const
- {
- return devicesReadyStates.count(state);
- }
-
- inline RobotUnitState RobotUnit::getRobotUnitState() const
- {
- return state;
- }
-
- inline std::string RobotUnit::getName() const
- {
- return Component::getName();
- }
-
- inline void RobotUnit::setEmergencyStopState(EmergencyStopState state)
- {
- emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
- }
-
- inline EmergencyStopState RobotUnit::getEmergencyStopState() const
- {
- return emergencyStop ? EmergencyStopState::eEmergencyStopActive :
- EmergencyStopState::eEmergencyStopInactive;
- }
-
- template<class ExceptT>
- inline void RobotUnit::throwIfStateIsNot(const std::set<RobotUnitState>& sinit, const std::string& fnc, bool onlyWarn) const
- {
- std::set<RobotUnitState> sset {sinit};
- if (! sset.count(state))
- {
- std::stringstream ss;
- ss << fnc << ": Can't be called if state is not in {";
- bool fst = true;
- for (RobotUnitState st : sset)
- {
- if (!fst)
- {
- ss << ", ";
- }
- ss << st;
- fst = false;
- }
- ss << "} (current state " << getRobotUnitState() << ")";
- ARMARX_ERROR << ss.str();
- if (!onlyWarn)
- {
- throw ExceptT {ss.str()};
- }
- }
- }
-
- template<class ExceptT>
- inline void RobotUnit::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
- {
- throwIfStateIsNot<ExceptT>(std::set<RobotUnitState> {s}, fnc, onlyWarn);
- }
-
- template<class ExceptT>
- inline void RobotUnit::throwIfDevicesNotReady(const std::string& fnc) const
- {
- throwIfStateIsNot<ExceptT>(devicesReadyStates, fnc);
- }
-
- template<class Cont>
- inline Ice::StringSeq RobotUnit::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;
- }
-
- //RobotUnitNJointControllerAccess
- inline void RobotUnitNJointControllerAccess::RtSetNJointControllerActivatedStatus(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtActivateController();
- }
-
- inline void RobotUnitNJointControllerAccess::RtSetNJointControllerDeactivatedStatus(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtDeactivateController();
- }
-
- inline void RobotUnitNJointControllerAccess::RtSetNJointControllerDeactivatedBecauseOfErrorStatus(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtDeactivateControllerBecauseOfError();
- }
-
- inline void RobotUnitNJointControllerAccess::InitNJointControler(
- const NJointControllerPtr& nJointCtrl,
- RobotUnit* ru,
- std::size_t ctrlId,
- std::map<std::string, const JointController*> ctrlDeviceUsedJointController,
- std::vector<char> ctrlDeviceUsedBitmap,
- std::vector<std::size_t> ctrlDeviceUsedIndices,
- bool deletable, bool internal
- ) noexcept
- {
- nJointCtrl->robotUnitInit(
- ru, ctrlId, std::move(ctrlDeviceUsedJointController), std::move(ctrlDeviceUsedBitmap),
- std::move(ctrlDeviceUsedIndices), deletable, internal
- );
- }
-
- inline void RobotUnitNJointControllerAccess::PublishNJointController(
- const NJointControllerPtr& nJointCtrl,
- const SensorAndControl& sac,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
- {
- nJointCtrl ->publish(sac, draw, observer);
- }
-
- inline bool RobotUnitNJointControllerAccess::GetNJointControllerStatusReportedActive(const NJointControllerPtr& nJointCtrl)
- {
- return nJointCtrl->statusReportedActive;
- }
-
- inline bool RobotUnitNJointControllerAccess::GetNJointControllerStatusReportedRequested(const NJointControllerPtr& nJointCtrl)
- {
- return nJointCtrl->statusReportedRequested;
- }
-
- inline void RobotUnitNJointControllerAccess::UpdateNJointControllerStatusReported(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
- nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
- }
-
- inline void RobotUnitNJointControllerAccess::SetNJointControllerRequested(const NJointControllerPtr& nJointCtrl, bool requested)
- {
- nJointCtrl->isRequested = requested;
- }
-
- inline void RobotUnitNJointControllerAccess::DeactivateNJointControllerPublishing(
- const NJointControllerPtr& nJointCtrl,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
- {
- nJointCtrl->deactivatePublish(draw, observer);
- }
-}
+#include "RobotUnit.ipp"
#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.ipp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.ipp
new file mode 100644
index 0000000000000000000000000000000000000000..9e462648eded545d67df2f99eced29761b03ab33
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.ipp
@@ -0,0 +1,400 @@
+/*
+ * 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::RobotUnit
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_UNIT_RobotAPI_RobotUnit_IPP
+#define _ARMARX_UNIT_RobotAPI_RobotUnit_IPP
+
+#include "RobotUnit.h"
+
+//inline functions
+namespace armarx
+{
+ //RobotUnit
+ inline const KeyValueVector<std::string, SensorDevicePtr>& RobotUnit::getSensorDevices() const
+ {
+ return sensorDevices;
+ }
+
+ inline const KeyValueVector<std::string, ControlDevicePtr>& RobotUnit::getControlDevices() const
+ {
+ return controlDevices;
+ }
+
+ inline RobotUnit::TimePointT RobotUnit::Now()
+ {
+ return ClockT::now();
+ }
+
+ inline std::chrono::microseconds RobotUnit::MicroToNow(RobotUnit::TimePointT last)
+ {
+ return std::chrono::duration_cast<std::chrono::microseconds>(Now() - last);
+ }
+
+ inline std::chrono::microseconds RobotUnit::MicroNow()
+ {
+ return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
+ }
+
+ inline PropertyDefinitionsPtr RobotUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
+ }
+
+ inline std::string RobotUnit::getDefaultName() const
+ {
+ return "RobotUnit";
+ }
+
+ inline constexpr std::size_t RobotUnit::IndexSentinel()
+ {
+ return JointAndNJointControllers::Sentinel();
+ }
+
+ inline void RobotUnit::onInitRobotUnit() {}
+
+ inline void RobotUnit::onConnectRobotUnit() {}
+
+ inline void RobotUnit::onDisconnectRobotUnit() {}
+
+ inline void RobotUnit::onExitRobotUnit() {}
+
+ inline bool RobotUnit::rtRequestedControllersChanged() const
+ {
+ return controllersRequested.updateReadBuffer();
+ }
+
+ inline const JointAndNJointControllers& RobotUnit::rtGetRequestedControllers() const
+ {
+ return controllersRequested.getReadBuffer();
+ }
+
+ inline const std::vector<JointController*>& RobotUnit::rtGetRequestedJointControllers() const
+ {
+ return rtGetRequestedControllers().jointControllers;
+ }
+
+ inline const std::vector<NJointController*>& RobotUnit::rtGetRequestedNJointControllers() const
+ {
+ return rtGetRequestedControllers().nJointControllers;
+ }
+
+ inline void RobotUnit::rtWriteActivatedControllers()
+ {
+ controllersActivated.commitWrite();
+ }
+
+ inline JointAndNJointControllers& RobotUnit::rtGetActivatedControllers()
+ {
+ return controllersActivated.getWriteBuffer();
+ }
+
+ inline std::vector<JointController*>& RobotUnit::rtGetActivatedJointControllers()
+ {
+ return rtGetActivatedControllers().jointControllers;
+ }
+
+ inline std::vector<NJointController*>& RobotUnit::rtGetActivatedNJointControllers()
+ {
+ return rtGetActivatedControllers().nJointControllers;
+ }
+
+ inline std::vector<std::size_t>& RobotUnit::rtGetActivatedJointToNJointControllerAssignement()
+ {
+ return rtGetActivatedControllers().jointToNJointControllerAssignement;
+ }
+
+ inline RTThreadTimingsSensorDevicePtr RobotUnit::createRTThreadTimingSensorDevice() const
+ {
+ return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
+ }
+
+ inline void RobotUnit::rtDeactivatedNJointControllerBecauseOfError(const NJointControllerPtr&) {}
+
+ inline RTThreadTimingsSensorDevice& RobotUnit::rtGetRTThreadTimingsSensorDevice()
+ {
+ return *rtThreadTimingsSensorDevice;
+ }
+
+ inline NJointControllerInterfacePrx RobotUnit::createNJointController(
+ const std::string& className, const std::string& instanceName,
+ const NJointControllerConfigPtr& config, const Ice::Current&
+ )
+ {
+ //no lock required
+ return NJointControllerInterfacePrx::uncheckedCast(
+ createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
+ }
+
+ inline void RobotUnit::deleteNJointController(const std::string& name, const Ice::Current&)
+ {
+ deleteNJointControllers({name});
+ }
+
+ inline void RobotUnit::activateNJointController(const std::string& name, const Ice::Current&)
+ {
+ activateNJointControllers({name});
+ }
+
+ inline void RobotUnit::deactivateNJointController(const std::string& name, const Ice::Current&)
+ {
+ deactivateNJointControllers(Ice::StringSeq {name});
+ }
+
+ inline void RobotUnit::checkSetOfControllersToActivate(const JointAndNJointControllers&) const {}
+
+ inline Ice::StringSeq RobotUnit::getNJointControllerNames(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ return getMapKeys(nJointControllers);
+ }
+
+ inline Ice::StringSeq RobotUnit::getRequestedNJointControllerNames(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ return GetNonNullNames(getRequestedNJointControllers());
+ }
+
+ inline Ice::StringSeq RobotUnit::getActivatedNJointControllerNames(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ return GetNonNullNames(getActivatedNJointControllers());
+ }
+
+ inline Ice::StringSeq RobotUnit::getNJointControllerClassNames(const Ice::Current&) const
+ {
+ return NJointControllerRegistry::getKeys();
+ }
+
+ inline KinematicUnitInterfacePrx RobotUnit::getKinematicUnit(const Ice::Current&) const
+ {
+ //no lock required
+ return KinematicUnitInterfacePrx::uncheckedCast(getUnit(KinematicUnitInterface::ice_staticId(), GlobalIceCurrent));
+ }
+
+ inline ForceTorqueUnitInterfacePrx RobotUnit::getForceTorqueUnit(const Ice::Current&) const
+ {
+ //no lock required
+ return ForceTorqueUnitInterfacePrx::uncheckedCast(getUnit(ForceTorqueUnitInterface::ice_staticId(), GlobalIceCurrent));
+ }
+
+ inline InertialMeasurementUnitInterfacePrx RobotUnit::getInertialMeasurementUnit(const Ice::Current&) const
+ {
+ //no lock required
+ return InertialMeasurementUnitInterfacePrx::uncheckedCast(
+ getUnit(InertialMeasurementUnitInterface::ice_staticId(), GlobalIceCurrent));
+ }
+
+ inline PlatformUnitInterfacePrx RobotUnit::getPlatformUnit(const Ice::Current&) const
+ {
+ //no lock required
+ return PlatformUnitInterfacePrx::uncheckedCast(getUnit(PlatformUnitInterface::ice_staticId(), GlobalIceCurrent));
+ }
+
+ inline std::string RobotUnit::getListenerTopicName(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ return robotUnitListenerTopicName;
+ }
+
+ inline const std::string& RobotUnit::getRobotNodetSeName() const
+ {
+ return robotNodeSetName;
+ }
+
+ inline const std::string& RobotUnit::getRobotProjectName() const
+ {
+ return robotProjectName;
+ }
+
+ inline const std::string& RobotUnit::getRobotFileName() const
+ {
+ return robotFileName;
+ }
+
+ inline const std::string& RobotUnit::getRobotPlatformName() const
+ {
+ return robotPlatformName;
+ }
+
+ inline std::size_t RobotUnit::rtGetNumberOfControlDevices() const
+ {
+ return controlDevices.size();
+ }
+
+ inline std::size_t RobotUnit::rtGetNumberOfSensorDevices() const
+ {
+ return sensorDevices.size();
+ }
+
+ inline bool RobotUnit::areDevicesReady() const
+ {
+ return devicesReadyStates.count(state);
+ }
+
+ inline RobotUnitState RobotUnit::getRobotUnitState() const
+ {
+ return state;
+ }
+
+ inline std::string RobotUnit::getName() const
+ {
+ return Component::getName();
+ }
+
+ inline void RobotUnit::setEmergencyStopState(EmergencyStopState state)
+ {
+ emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
+ }
+
+ inline EmergencyStopState RobotUnit::getEmergencyStopState() const
+ {
+ return emergencyStop ? EmergencyStopState::eEmergencyStopActive :
+ EmergencyStopState::eEmergencyStopInactive;
+ }
+
+ template<class ExceptT>
+ inline void RobotUnit::throwIfStateIsNot(const std::set<RobotUnitState>& sinit, const std::string& fnc, bool onlyWarn) const
+ {
+ std::set<RobotUnitState> sset {sinit};
+ if (! sset.count(state))
+ {
+ std::stringstream ss;
+ ss << fnc << ": Can't be called if state is not in {";
+ bool fst = true;
+ for (RobotUnitState st : sset)
+ {
+ if (!fst)
+ {
+ ss << ", ";
+ }
+ ss << st;
+ fst = false;
+ }
+ ss << "} (current state " << getRobotUnitState() << ")";
+ ARMARX_ERROR << ss.str();
+ if (!onlyWarn)
+ {
+ throw ExceptT {ss.str()};
+ }
+ }
+ }
+
+ template<class ExceptT>
+ inline void RobotUnit::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
+ {
+ throwIfStateIsNot<ExceptT>(std::set<RobotUnitState> {s}, fnc, onlyWarn);
+ }
+
+ template<class ExceptT>
+ inline void RobotUnit::throwIfDevicesNotReady(const std::string& fnc) const
+ {
+ throwIfStateIsNot<ExceptT>(devicesReadyStates, fnc);
+ }
+
+ template<class Cont>
+ inline Ice::StringSeq RobotUnit::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;
+ }
+
+ //RobotUnitNJointControllerAccess
+ inline void RobotUnitNJointControllerAccess::RtSetNJointControllerActivatedStatus(const NJointControllerPtr& nJointCtrl)
+ {
+ nJointCtrl->rtActivateController();
+ }
+
+ inline void RobotUnitNJointControllerAccess::RtSetNJointControllerDeactivatedStatus(const NJointControllerPtr& nJointCtrl)
+ {
+ nJointCtrl->rtDeactivateController();
+ }
+
+ inline void RobotUnitNJointControllerAccess::RtSetNJointControllerDeactivatedBecauseOfErrorStatus(const NJointControllerPtr& nJointCtrl)
+ {
+ nJointCtrl->rtDeactivateControllerBecauseOfError();
+ }
+
+ inline void RobotUnitNJointControllerAccess::InitNJointControler(
+ const NJointControllerPtr& nJointCtrl,
+ RobotUnit* ru,
+ std::size_t ctrlId,
+ std::map<std::string, const JointController*> ctrlDeviceUsedJointController,
+ std::vector<char> ctrlDeviceUsedBitmap,
+ std::vector<std::size_t> ctrlDeviceUsedIndices,
+ bool deletable, bool internal
+ ) noexcept
+ {
+ nJointCtrl->robotUnitInit(
+ ru, ctrlId, std::move(ctrlDeviceUsedJointController), std::move(ctrlDeviceUsedBitmap),
+ std::move(ctrlDeviceUsedIndices), deletable, internal
+ );
+ }
+
+ inline void RobotUnitNJointControllerAccess::PublishNJointController(
+ const NJointControllerPtr& nJointCtrl,
+ const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer
+ )
+ {
+ nJointCtrl ->publish(sac, draw, observer);
+ }
+
+ inline bool RobotUnitNJointControllerAccess::GetNJointControllerStatusReportedActive(const NJointControllerPtr& nJointCtrl)
+ {
+ return nJointCtrl->statusReportedActive;
+ }
+
+ inline bool RobotUnitNJointControllerAccess::GetNJointControllerStatusReportedRequested(const NJointControllerPtr& nJointCtrl)
+ {
+ return nJointCtrl->statusReportedRequested;
+ }
+
+ inline void RobotUnitNJointControllerAccess::UpdateNJointControllerStatusReported(const NJointControllerPtr& nJointCtrl)
+ {
+ nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
+ nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
+ }
+
+ inline void RobotUnitNJointControllerAccess::SetNJointControllerRequested(const NJointControllerPtr& nJointCtrl, bool requested)
+ {
+ nJointCtrl->isRequested = requested;
+ }
+
+ inline void RobotUnitNJointControllerAccess::DeactivateNJointControllerPublishing(
+ const NJointControllerPtr& nJointCtrl,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer
+ )
+ {
+ nJointCtrl->deactivatePublish(draw, observer);
+ }
+}
+#endif