diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
index 3a9c5915df3610764ea2d4bbecfc0a253e335383..666f08ff11b7ef1936a0c06eb199d5e029bfde9b 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
@@ -31,23 +31,22 @@
#include "../ControlModes.h"
#include "../JointControllers/JointController.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
- {
- TYPEDEF_PTRS_HANDLE(Devices);
- }
- TYPEDEF_PTRS_SHARED(ControlDevice);
-
+ TYPEDEF_PTRS_HANDLE(Devices);
+}
- namespace ControlDeviceTags
- {
- using namespace DeviceTags;
- const static std::string PeriodicPosition = "ControlDeviceTag_PeriodicPosition";
- const static std::string CreateNoDefaultController = "ControlDeviceTag_CreateNoDefaultController";
- const static std::string ForcePositionThroughVelocity = "ControlDeviceTag_ForcePositionThroughVelocity";
- }
+namespace armarx::ControlDeviceTags
+{
+ using namespace DeviceTags;
+ const static std::string PeriodicPosition = "ControlDeviceTag_PeriodicPosition";
+ const static std::string CreateNoDefaultController = "ControlDeviceTag_CreateNoDefaultController";
+ const static std::string ForcePositionThroughVelocity = "ControlDeviceTag_ForcePositionThroughVelocity";
+}
+namespace armarx
+{
+ TYPEDEF_PTRS_SHARED(ControlDevice);
/**
* @brief The ControlDevice class represents a logical unit accepting commands via its JointControllers.
*
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
index 9e38d7d35ea6a93847b9ebefe2ebe6b01d48ac98..89c0fab1ba6d9f042a7351233a14a86d7641edc0 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
@@ -25,12 +25,13 @@
#include"SensorDevice.h"
#include "../SensorValues/SensorValueRTThreadTimings.h"
+namespace armarx::RobotUnitModule
+{
+ TYPEDEF_PTRS_HANDLE(ControlThread);
+}
+
namespace armarx
{
- namespace RobotUnitModule
- {
- TYPEDEF_PTRS_HANDLE(ControlThread);
- }
TYPEDEF_PTRS_SHARED(RTThreadTimingsSensorDevice);
class RTThreadTimingsSensorDevice: virtual public SensorDevice
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
index 66760b4be0405bd3c092aec23bb9e6da4c7e4e05..a35ae8b8d359fffc4c963b1a0a426b68902eea51 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
@@ -27,19 +27,19 @@
#include "../util.h"
#include <IceUtil/Time.h>
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
- {
- TYPEDEF_PTRS_HANDLE(Devices);
- }
- TYPEDEF_PTRS_SHARED(SensorDevice);
+ TYPEDEF_PTRS_HANDLE(Devices);
+}
- namespace SensorDeviceTags
- {
- using namespace DeviceTags;
- }
+namespace armarx::SensorDeviceTags
+{
+ using namespace DeviceTags;
+}
+namespace armarx
+{
+ TYPEDEF_PTRS_SHARED(SensorDevice);
/**
* @brief This class represents some part of the robot providing sensor values.
*
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index 86911d7e44b317bf7d04ea226a516853967947c4..7f73e77b62de92a209368a3f379e0c9e1d3da81e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -29,47 +29,45 @@
#include <atomic>
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
- {
- /**
- * \brief This class allows minimal access to private members of \ref armarx::RobotUnitModule::Devices in a sane fashion for \ref armarx::NJointController.
- * \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 DevicesAttorneyForNJointController
+ /**
+ * \brief This class allows minimal access to private members of \ref armarx::RobotUnitModule::Devices in a sane fashion for \ref armarx::NJointControllerBase.
+ * \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 DevicesAttorneyForNJointController
+ {
+ friend class ::armarx::NJointControllerBase;
+ static JointController* GetJointController(Devices& d, const std::string& deviceName, const std::string& controlMode)
{
- friend class ::armarx::NJointController;
- static JointController* GetJointController(Devices& d, const std::string& deviceName, const std::string& controlMode)
+ if (d.controlDevices.has(deviceName))
{
- if (d.controlDevices.has(deviceName))
+ auto& dev = d.controlDevices.at(deviceName);
+ if (dev->hasJointController(controlMode))
{
- auto& dev = d.controlDevices.at(deviceName);
- if (dev->hasJointController(controlMode))
- {
- ARMARX_CHECK_NOT_NULL(dev->getJointController(controlMode));
- ARMARX_CHECK_NOT_NULL(dev->getJointController(controlMode)->getControlTarget());
- return dev->getJointController(controlMode);
- }
+ ARMARX_CHECK_NOT_NULL(dev->getJointController(controlMode));
+ ARMARX_CHECK_NOT_NULL(dev->getJointController(controlMode)->getControlTarget());
+ return dev->getJointController(controlMode);
}
- return nullptr;
}
- };
- }
+ return nullptr;
+ }
+ };
}
+
thread_local bool armarx::NJointControllerRegistryEntry::ConstructorIsRunning_ = false;
namespace armarx
{
- const NJointControllerPtr NJointController::NullPtr
+ const NJointControllerBasePtr NJointControllerBase::NullPtr
{
nullptr
};
- NJointControllerDescription NJointController::getControllerDescription(const Ice::Current&) const
+ NJointControllerDescription NJointControllerBase::getControllerDescription(const Ice::Current&) const
{
NJointControllerDescription d;
d.className = getClassName();
@@ -82,7 +80,7 @@ namespace armarx
return d;
}
- boost::optional<std::vector<char> > NJointController::isNotInConflictWith(const std::vector<char>& used) const
+ boost::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
{
ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
auto result = used;
@@ -101,7 +99,7 @@ namespace armarx
return {true, std::move(result)};
}
- NJointControllerStatus NJointController::getControllerStatus(const Ice::Current&) const
+ NJointControllerStatus NJointControllerBase::getControllerStatus(const Ice::Current&) const
{
NJointControllerStatus s;
s.active = isActive;
@@ -112,7 +110,7 @@ namespace armarx
return s;
}
- NJointControllerDescriptionWithStatus NJointController::getControllerDescriptionWithStatus(const Ice::Current&) const
+ NJointControllerDescriptionWithStatus NJointControllerBase::getControllerDescriptionWithStatus(const Ice::Current&) const
{
NJointControllerDescriptionWithStatus ds;
ds.description = getControllerDescription();
@@ -120,31 +118,31 @@ namespace armarx
return ds;
}
- RobotUnitInterfacePrx NJointController::getRobotUnit(const Ice::Current&) const
+ RobotUnitInterfacePrx NJointControllerBase::getRobotUnit(const Ice::Current&) const
{
return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
}
- void NJointController::activateController(const Ice::Current&)
+ void NJointControllerBase::activateController(const Ice::Current&)
{
robotUnit.activateNJointController(this);
}
- void NJointController::deactivateController(const Ice::Current&)
+ void NJointControllerBase::deactivateController(const Ice::Current&)
{
robotUnit.deactivateNJointController(this);
}
- void NJointController::deleteController(const Ice::Current&)
+ void NJointControllerBase::deleteController(const Ice::Current&)
{
robotUnit.deleteNJointController(this);
}
- void NJointController::deactivateAndDeleteController(const Ice::Current&)
+ void NJointControllerBase::deactivateAndDeleteController(const Ice::Current&)
{
robotUnit.deactivateAndDeleteNJointController(this);
}
- void NJointController::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
+ void NJointControllerBase::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
{
const bool active = isActive;
if (publishActive.exchange(active) != active)
@@ -199,7 +197,7 @@ namespace armarx
}
}
- void NJointController::deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
+ void NJointControllerBase::deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
{
if (publishActive.exchange(false))
{
@@ -208,7 +206,7 @@ namespace armarx
}
}
- void NJointController::rtActivateController()
+ void NJointControllerBase::rtActivateController()
{
if (!isActive)
{
@@ -219,7 +217,7 @@ namespace armarx
}
}
- void NJointController::rtDeactivateController()
+ void NJointControllerBase::rtDeactivateController()
{
if (isActive)
{
@@ -228,13 +226,13 @@ namespace armarx
}
}
- void NJointController::rtDeactivateControllerBecauseOfError()
+ void NJointControllerBase::rtDeactivateControllerBecauseOfError()
{
deactivatedBecauseOfError = true;
rtDeactivateController();
}
- armarx::ConstSensorDevicePtr armarx::NJointController::peekSensorDevice(const std::string& deviceName) const
+ armarx::ConstSensorDevicePtr armarx::NJointControllerBase::peekSensorDevice(const std::string& deviceName) const
{
ARMARX_CHECK_EXPRESSION_W_HINT(
NJointControllerRegistryEntry::ConstructorIsRunning(),
@@ -243,7 +241,7 @@ namespace armarx
return RobotUnitModule::Devices::Instance().getSensorDevice(deviceName);
}
- ConstControlDevicePtr NJointController::peekControlDevice(const std::string& deviceName) const
+ ConstControlDevicePtr NJointControllerBase::peekControlDevice(const std::string& deviceName) const
{
ARMARX_CHECK_EXPRESSION_W_HINT(
NJointControllerRegistryEntry::ConstructorIsRunning(),
@@ -252,7 +250,7 @@ namespace armarx
return RobotUnitModule::Devices::Instance().getControlDevice(deviceName);
}
- const VirtualRobot::RobotPtr& NJointController::useSynchronizedRtRobot(bool updateCollisionModel)
+ const VirtualRobot::RobotPtr& NJointControllerBase::useSynchronizedRtRobot(bool updateCollisionModel)
{
ARMARX_CHECK_EXPRESSION_W_HINT(
NJointControllerRegistryEntry::ConstructorIsRunning(),
@@ -264,22 +262,22 @@ namespace armarx
return rtRobot;
}
- void NJointController::onInitComponent()
+ void NJointControllerBase::onInitComponent()
{
onInitNJointController();
}
- void NJointController::onConnectComponent()
+ void NJointControllerBase::onConnectComponent()
{
onConnectNJointController();
}
- void NJointController::onDisconnectComponent()
+ void NJointControllerBase::onDisconnectComponent()
{
onDisconnectNJointController();
}
- void NJointController::onExitComponent()
+ void NJointControllerBase::onExitComponent()
{
onExitNJointController();
// make sure the destructor of the handles does not throw an exception and triggers a termination of the process
@@ -327,14 +325,14 @@ namespace armarx
threadHandles.clear();
}
- ThreadPoolPtr NJointController::getThreadPool() const
+ ThreadPoolPtr NJointControllerBase::getThreadPool() const
{
ARMARX_CHECK_EXPRESSION(Application::getInstance());
return Application::getInstance()->getThreadPool();
}
- const SensorValueBase* NJointController::useSensorValue(const std::string& deviceName) const
+ const SensorValueBase* NJointControllerBase::useSensorValue(const std::string& deviceName) const
{
ARMARX_CHECK_EXPRESSION_W_HINT(
NJointControllerRegistryEntry::ConstructorIsRunning(),
@@ -349,18 +347,18 @@ namespace armarx
return dev->getSensorValue();
}
- NJointController::NJointController() :
+ NJointControllerBase::NJointControllerBase() :
robotUnit(RobotUnitModule::ModuleBase::Instance< ::armarx::RobotUnit>()),
controlDeviceUsedBitmap(robotUnit.getNumberOfControlDevices(), 0)
{
controlDeviceUsedIndices.reserve(robotUnit.getNumberOfControlDevices());
}
- NJointController::~NJointController()
+ NJointControllerBase::~NJointControllerBase()
{
}
- ControlTargetBase* NJointController::useControlTarget(const std::string& deviceName, const std::string& controlMode)
+ ControlTargetBase* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode)
{
ARMARX_CHECK_EXPRESSION_W_HINT(
NJointControllerRegistryEntry::ConstructorIsRunning(),
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index 3b2deaf46191bc48c46bdaba02d4707cc25e4452..7bb3d462007d4b2af909fad70df17139466b8386 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -60,22 +60,25 @@ namespace VirtualRobot
using RobotPtr = boost::shared_ptr<Robot>;
}
+namespace armarx::RobotUnitModule
+{
+ class NJointControllerAttorneyForPublisher;
+ class NJointControllerAttorneyForControlThread;
+ class NJointControllerAttorneyForControllerManagement;
+}
+
+namespace armarx::detail
+{
+ template<class> class NJointControllerRegistryEntryHelper;
+}
+
namespace armarx
{
using ThreadPoolPtr = std::shared_ptr<class ThreadPool>;
- namespace RobotUnitModule
- {
- class NJointControllerAttorneyForPublisher;
- class NJointControllerAttorneyForControlThread;
- class NJointControllerAttorneyForControllerManagement;
- }
- namespace detail
- {
- template<class> class NJointControllerRegistryEntryHelperBase;
- }
-
- TYPEDEF_PTRS_HANDLE(NJointController);
+ TYPEDEF_PTRS_HANDLE(NJointControllerBase);
+ TYPEDEF_PTRS_HANDLE(SynchronousNJointController);
+ TYPEDEF_PTRS_HANDLE(AsynchronousNJointController);
TYPEDEF_PTRS_HANDLE(RobotUnit);
@@ -90,40 +93,40 @@ namespace armarx
*
* This is the abstract base class for all NJointControllers.
* It implements basic management routines required by the RobotUnit and some basic ice calls.
- * \ref NJointController "NJointControllers" are instantiated and managed by the \ref RobotUnit.
+ * \ref NJointControllerBase "NJointControllers" are instantiated and managed by the \ref RobotUnit.
*
- * A \ref NJointController calculates \ref ControlTargetBase "ControlTargets" for a set of
+ * A \ref NJointControllerBase calculates \ref ControlTargetBase "ControlTargets" for a set of
* \ref ControlDevice "ControlDevices" in a specific ControlMode.
* This ControlMode is defined for each \ref ControlDevice during construction.
*
* \section nj-state Requested and Active
- * A \ref NJointController can is requested / not requested and active / inactive.
+ * A \ref NJointControllerBase can is requested / not requested and active / inactive.
* All four combinations are possible.
*
* \subsection nj-state-req Requested / Not Requested
- * If the user wants this \ref NJointController to be executed it is in a requested state (see \ref isControllerRequested).
+ * If the user wants this \ref NJointControllerBase to be executed it is in a requested state (see \ref isControllerRequested).
* Otherwise the controler is not requested.
*
* Calling \ref activateController sets the state to requested.
* Calling \ref deactivateController sets the state to not requested.
- * If the \ref NJointController causes an error or a different \ref NJointController using one or more of the same
- * \ref ControlDevice's is requested, this \ref NJointController is deactivated.
+ * If the \ref NJointControllerBase causes an error or a different \ref NJointControllerBase using one or more of the same
+ * \ref ControlDevice's is requested, this \ref NJointControllerBase is deactivated.
*
*
* \subsection nj-state-act Active / Inactive
- * Is the \ref NJointController executed by the \ref RobotUnitModules::ControlThread "ControlThread" it is active
+ * Is the \ref NJointControllerBase executed by the \ref RobotUnitModules::ControlThread "ControlThread" it is active
* (see \ref isControllerActive).
*
- * To be executed by the \ref RobotUnitModules::ControlThread "ControlThread", the \ref NJointController has to be
+ * To be executed by the \ref RobotUnitModules::ControlThread "ControlThread", the \ref NJointControllerBase has to be
* requested at some point in the past.
*
* If a controller is active, it has to write a valid \ref ControlTargetBase "ControlTarget"
* for each of its \ref ControlDevice "ControlDevicea" (if it fails, it will be deactivated).
*
* \section Constructor
- * In the Constructor, a \ref NJointController has to declare all \ref ControlDevice "ControlDevices" it uses.
+ * In the Constructor, a \ref NJointControllerBase has to declare all \ref ControlDevice "ControlDevices" it uses.
*
- * The constructor takes a pointer to a configuration structure of the type \ref NJointController::ConfigPtrT.
+ * The constructor takes a pointer to a configuration structure of the type \ref NJointControllerBase::ConfigPtrT.
* If an implementation requires a special configuration structure (e.g.: SomeOtherCfg), it has to override this type by calling adding
* \code{.cpp}
* using ConfigPtrT = SomeOtherCfgPtr;
@@ -134,19 +137,19 @@ namespace armarx
* There is a way to generate a small gui widget for controller construction (see this \ref nj-ctor-gui "section").
*
* \subsection nj-ctor-req-ctrl Using ControlTargets
- * A \ref NJointController can use \ref peekControlDevice to examine a \ref ControlDevice before using it
+ * A \ref NJointControllerBase can use \ref peekControlDevice to examine a \ref ControlDevice before using it
* (e.g.: checking the supported \ref ControlTargetBase "ControlTargets").
*
- * If a \ref ControlDevice should be used by this \ref NJointController, it has to call \ref useControlDevice.
+ * If a \ref ControlDevice should be used by this \ref NJointControllerBase, it has to call \ref useControlDevice.
* This sets the ControlMode for the \ref ControlDevice and returns a pointer to the \ref ControlTargetBase "ControlTarget".
* This pointer has to be used to send commands in each iteration of \ref rtRun.
- * The ControlMode can't be changed afterwards (A new \ref NJointController has to be created).
+ * The ControlMode can't be changed afterwards (A new \ref NJointControllerBase has to be created).
*
*
* \subsection nj-ctor-req-sens Using SensorValues
- * A \ref NJointController can use \ref peekSensorDevice to examine a \ref SensorDevice before using it.
+ * A \ref NJointControllerBase can use \ref peekSensorDevice to examine a \ref SensorDevice before using it.
*
- * If a \ref SensorDevice should be used by this \ref NJointController, it has to call \ref useSensorDevice.
+ * If a \ref SensorDevice should be used by this \ref NJointControllerBase, it has to call \ref useSensorDevice.
* This returns a pointer to the \ref SensorValueBase "SensorValue".
*
* \subsection nj-ctor-rob A synchronized Virtualrobot
@@ -155,15 +158,15 @@ namespace armarx
* This robot is synchronized with the real robots state before calling \ref rtRun
*
* \section nj-parts Rt and non Rt
- * Each \ref NJointController has two parts:
+ * Each \ref NJointControllerBase has two parts:
* \li The RT controll loop
* \li The NonRT ice communication
*
* \subsection rtcontrollloop The Rt controll loop (\ref rtRun)
* \warning This part has to satisfy realtime conditions!
- * All realtime functions of \ref NJointController have the 'rt' prefix.
+ * All realtime functions of \ref NJointControllerBase have the 'rt' prefix.
*
- * Here the \ref NJointController has access to the robot's current state
+ * Here the \ref NJointControllerBase has access to the robot's current state
* and has to write results into \ref ControlTargetBase "ControlTargets".
*
* It must not:
@@ -177,21 +180,21 @@ namespace armarx
*
* \subsection nonrtpart The NonRT ice communication
* This part consits of any ice communication.
- * Here the \ref NJointController can get new controll parameters or targets from other components.
+ * Here the \ref NJointControllerBase can get new controll parameters or targets from other components.
*
* \section rtnrtcomm Communication between RT and NonRT
* All communication between RT and NonRT has to be lockfree.
- * The \ref NJointController has to use constructs like atomics or
+ * The \ref NJointControllerBase has to use constructs like atomics or
* \ref TripleBuffer "TripleBuffers" (See \ref armarx::NJointControllerWithTripleBuffer).
*
- * \image html NJointControllerGeneralDataFlow.svg "General Dataflow in a NJointController"
+ * \image html NJointControllerGeneralDataFlow.svg "General Dataflow in a NJointControllerBase"
*
- * \image html NJointControllerAtomicDataFlow.svg "Dataflow in a NJointController using atomics for communication between RT and NonRT"
+ * \image html NJointControllerAtomicDataFlow.svg "Dataflow in a NJointControllerBase using atomics for communication between RT and NonRT"
*
- * \image html NJointControllerTripleBufferDataFlow.svg "Dataflow in a NJointController using a triple buffer for communication between RT and NonRT"
+ * \image html NJointControllerTripleBufferDataFlow.svg "Dataflow in a NJointControllerBase using a triple buffer for communication between RT and NonRT"
*
*
- * \image html NJointControllerDataFlow_Graph.svg "Dataflow in a NJointController as a Graph of the two participating domains"
+ * \image html NJointControllerDataFlow_Graph.svg "Dataflow in a NJointControllerBase as a Graph of the two participating domains"
* The above image shows the two participating domains: one RT thread and multiple ICE threads.
* If data has to flow along an arrow, you need some construct for non blocking message passing.
*
@@ -209,9 +212,9 @@ namespace armarx
*
* If you do some additional calculation in rtRun, you maybe need to pass config parameters from NonRT to RT using a nonblocking method.
*
- * \image html NJointControllerWorkerThreadDataFlow.svg "Dataflow in a NJointController using a worker thread"
+ * \image html NJointControllerWorkerThreadDataFlow.svg "Dataflow in a NJointControllerBase using a worker thread"
*
- * \image html NJointControllerWorkerThreadDataFlow_Graph.svg "Dataflow in a NJointController using a worker thread as a Graph of the three participating domains"
+ * \image html NJointControllerWorkerThreadDataFlow_Graph.svg "Dataflow in a NJointControllerBase using a worker thread as a Graph of the three participating domains"
* The above image shows the three participating domains: one RT thread, one worker trhead and multiple ICE threads.
* If data has to flow along an arrow, you need some construct for non blocking message passing.
*
@@ -229,7 +232,7 @@ namespace armarx
static ConfigPtrT GenerateConfigFromVariants(const StringVariantBaseMap&); // turns the resulting variants into a config
* \endcode
*
- * The \ref RobotUnitGui will provide a widget to configure and construct a \ref NJointController of this type.
+ * The \ref RobotUnitGui will provide a widget to configure and construct a \ref NJointControllerBase of this type.
*
* \section Examples
*
@@ -268,14 +271,14 @@ namespace armarx
};
* \endcode
*
- * The controller class has to inherit \ref NJointController
+ * The controller class has to inherit \ref NJointControllerBase
* \code{.cpp}
- class NJointPositionPassThroughController: public NJointController
+ class NJointPositionPassThroughController: public NJointControllerBase
{
public:
* \endcode
*
- * The \ref NJointController provides a config widget for the \ref RobotUnitGuiPlugin
+ * The \ref NJointControllerBase provides a config widget for the \ref RobotUnitGuiPlugin
* \code{.cpp}
static WidgetDescription::WidgetPtr GenerateConfigDescription
(
@@ -311,14 +314,14 @@ namespace armarx
void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
* \endcode
*
- * Hooks for this \ref NJointController to execute code during publishing.
+ * Hooks for this \ref NJointControllerBase to execute code during publishing.
* \code{.cpp}
void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
void onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
* \endcode
*
- * This \ref NJointController uses one position \ref SensorValue1DoFActuatorPosition "SensorValue", calculates one
+ * This \ref NJointControllerBase uses one position \ref SensorValue1DoFActuatorPosition "SensorValue", calculates one
* position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and stores the current position target and position value in atomics.
* \code{.cpp}
const SensorValue1DoFActuatorPosition* sensor;
@@ -386,7 +389,7 @@ namespace armarx
}
* \endcode
*
- * This turns a map of variants into the config required by this \ref NJointController.
+ * This turns a map of variants into the config required by this \ref NJointControllerBase.
* The \ref Variant for the key 'name' defines the \ref ControlDevice.
* \code{.cpp}
NJointControllerConfigPtr NJointPositionPassThroughController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
@@ -434,7 +437,7 @@ namespace armarx
* \endcode
*
- * This \ref NJointController provides two widgets for function calls.
+ * This \ref NJointControllerBase provides two widgets for function calls.
* \code{.cpp}
WidgetDescription::StringWidgetDictionary NJointPositionPassThroughController::getFunctionDescriptions(const Ice::Current&) const
{
@@ -546,8 +549,8 @@ namespace armarx
}
* \endcode
*
- * This registers a factory for this \ref NJointController.
- * This factory is used by the \ref RobotUnit to create the \ref NJointController.
+ * This registers a factory for this \ref NJointControllerBase.
+ * This factory is used by the \ref RobotUnit to create the \ref NJointControllerBase.
* The passed name has to match the name returned by \ref getClassName.
* \code{.cpp}
NJointControllerRegistration<NJointPositionPassThroughController> registrationControllerNJointPositionPassThroughController("NJointPositionPassThroughController");
@@ -562,7 +565,7 @@ namespace armarx
}
* \endcode
*/
- class NJointController:
+ class NJointControllerBase:
virtual public NJointControllerInterface,
virtual public ManagedIceObject
{
@@ -585,14 +588,14 @@ namespace armarx
public:
/**
* @brief Get a const ptr to the given \ref SensorDevice
- * \warning This function can only be called in a \ref NJointController's ctor (or functions called in it)
+ * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
* @param deviceName The \ref SensorDevice's name
* @return A const ptr to the given \ref SensorDevice
*/
ConstSensorDevicePtr peekSensorDevice(const std::string& deviceName) const;
/**
* @brief Get a const ptr to the given \ref ControlDevice
- * \warning This function can only be called in a \ref NJointController's ctor (or functions called in it)
+ * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
* @param deviceName The \ref ControlDevice's name
* @return A const ptr to the given \ref ControlDevice
*/
@@ -601,7 +604,7 @@ namespace armarx
/**
* @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
- * \warning This function can only be called in a \ref NJointController's ctor (or functions called in it)
+ * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
* @param deviceName The \ref ControlDevice's name
* @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
* @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
@@ -610,7 +613,7 @@ namespace armarx
/**
* @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
- * \warning This function can only be called in a \ref NJointController's ctor (or functions called in it)
+ * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
* @param deviceName The \ref ControlDevice's name
* @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
* @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
@@ -620,14 +623,14 @@ namespace armarx
/**
* @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
- * \warning This function can only be called in a \ref NJointController's ctor (or functions called in it)
+ * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
* @param deviceName The \ref SensorDevice's name
* @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
*/
const SensorValueBase* useSensorValue(const std::string& sensorDeviceName) const;
/**
* @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
- * \warning This function can only be called in a \ref NJointController's ctor (or functions called in it)
+ * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
* @param deviceName The \ref SensorDevice's name
* @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
*/
@@ -637,7 +640,7 @@ namespace armarx
/**
* @brief Requests a VirtualRobot for use in \ref rtRun
* *
- * \warning This function can only be called in a \ref NJointController's ctor (or functions called in it)
+ * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
*
* The robot is updated before \ref rtRun is called and can be accessed via \rtGetRobot
* @param updateCollisionModel Whether the robot's collision model should be updated
@@ -676,7 +679,7 @@ namespace armarx
* @brief Executes a given task in a separate thread from the Application ThreadPool.
* @param taskName Descriptive name of this task to identify it on errors.
* @param task std::function object (or lambda) that is to be executed.
- * @note This task will be joined in onExitComponent of the NJointController. So make sure it terminates, when the
+ * @note This task will be joined in onExitComponent of the NJointControllerBase. So make sure it terminates, when the
* controller is deactivated or removed!
*
* @code{.cpp}
@@ -707,7 +710,7 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(!taskName.empty());
ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
- ARMARX_VERBOSE << "Adding NJointController task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount();
+ ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount();
auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
ARMARX_CHECK_EXPRESSION_W_HINT(handlePtr->isValid(), "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount());
threadHandles[taskName] = handlePtr;
@@ -743,12 +746,10 @@ namespace armarx
// ///////////////////////////////////// rt interface ///////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
public: ///TODO make protected and use attorneys
- virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
/**
- * @brief Returns the virtual robot used by this \ref NJointController in the \ref rtRun
- * @return The virtual robot used by this \ref NJointController in the \ref rtRun
+ * @brief Returns the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
+ * @return The virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @see useSynchronizedRtRobot
* @see rtGetRobotNodes
*/
@@ -757,8 +758,8 @@ namespace armarx
return rtRobot;
}
/**
- * @brief Returns the nodes of the virtual robot used by this \ref NJointController in the \ref rtRun
- * @return The nodes of the virtual robot used by this \ref NJointController in the \ref rtRun
+ * @brief Returns the nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
+ * @return The nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @see useSynchronizedRtRobot
* @see rtGetRobot
*/
@@ -767,17 +768,17 @@ namespace armarx
return rtRobotNodes;
}
/**
- * @brief Returns whether this \ref NJointController calculates a \ref ControlTargetBase "ControlTarget"
+ * @brief Returns whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice
* @param deviceIndex The \ref ControlDevice's index
- * @return Whether this \ref NJointController calculates a \ref ControlTargetBase "ControlTarget"
+ * @return Whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice
*/
bool rtUsesControlDevice(std::size_t deviceIndex) const;
/**
- * @brief Returns the indices of all \ref ControlDevice's this \ref NJointController
+ * @brief Returns the indices of all \ref ControlDevice's this \ref NJointControllerBase
* calculates a \ref ControlTargetBase "ControlTarget" for.
- * @return The indices of all \ref ControlDevice's this \ref NJointController
+ * @return The indices of all \ref ControlDevice's this \ref NJointControllerBase
* calculates a \ref ControlTargetBase "ControlTarget" for.
*/
const std::vector<std::size_t>& rtGetControlDeviceUsedIndices() const;
@@ -845,7 +846,7 @@ namespace armarx
* and sets the error flag.
*
* Calls \ref rtPostDeactivateController
- * This function is called when this \ref NJointController produces an error
+ * This function is called when this \ref NJointControllerBase produces an error
* (e.g.: calculates an invalid \ref ControlTargetBase "ControlTarget", throws an exception)
* @see rtActivateController
* @see rtDeactivateController
@@ -855,11 +856,11 @@ namespace armarx
public:
- static const NJointControllerPtr NullPtr;
+ static const NJointControllerBasePtr NullPtr;
- NJointController();
+ NJointControllerBase();
- ~NJointController() override;
+ ~NJointControllerBase() override;
//ice interface (must not be called in the rt thread)
//c++ interface (local calls) (must be called in the rt thread)
@@ -875,7 +876,7 @@ namespace armarx
const std::map<std::string, const JointController*>& getControlDevicesUsedJointController();
//check for conflict
- boost::optional<std::vector<char>> isNotInConflictWith(const NJointControllerPtr& other) const;
+ boost::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const;
boost::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
template<class ItT>
@@ -929,17 +930,17 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
/**
- * \brief This class allows minimal access to private members of \ref NJointController in a sane fashion for \ref RobotUnitModule::ControllerManagement.
+ * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::ControllerManagement.
* \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
*/
friend class RobotUnitModule::NJointControllerAttorneyForControllerManagement;
/**
- * \brief This class allows minimal access to private members of \ref NJointController in a sane fashion for \ref RobotUnitModule::ControlThread.
+ * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::ControlThread.
* \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
*/
friend class RobotUnitModule::NJointControllerAttorneyForControlThread;
/**
- * \brief This class allows minimal access to private members of \ref NJointController in a sane fashion for \ref RobotUnitModule::Publisher.
+ * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::Publisher.
* \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
*/
friend class RobotUnitModule::NJointControllerAttorneyForPublisher;
@@ -947,8 +948,23 @@ namespace armarx
* \brief This is required for the factory
* \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
*/
- template<class> friend class detail::NJointControllerRegistryEntryHelperBase;
+ template<class> friend class detail::NJointControllerRegistryEntryHelper;
+ };
+
+ class SynchronousNJointController : public NJointControllerBase
+ {
+ public: ///TODO make protected and use attorneys
+ virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ };
+ class AsynchronousNJointController : public NJointControllerBase
+ {
+ public: ///TODO make protected and use attorneys
+ virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
};
+ using NJointController = SynchronousNJointController;
+ using NJointControllerPtr = SynchronousNJointControllerPtr;
}
#include "NJointController.ipp"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
index c00425e6aff91380633a72a32eae2758e11346d9..bcefd5a83350721c67aff181034f5f0ef80fe8d6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
@@ -23,26 +23,23 @@
#ifndef _ARMARX_LIB_RobotAPI_NJointController_HPP
#define _ARMARX_LIB_RobotAPI_NJointController_HPP
-#include <thread>
-
#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include "NJointController.h"
#include "../RobotUnit.h"
+namespace armarx::RobotUnitModule
+{
+ class ControllerManagement;
+}
namespace armarx
{
- namespace RobotUnitModule
- {
- class ControllerManagement;
- }
-
class NJointControllerRegistryEntry
{
private:
friend class RobotUnitModule::ControllerManagement;
- virtual NJointControllerPtr create(
+ virtual NJointControllerBasePtr create(
RobotUnitModule::ControllerManagement* cmngr,
const NJointControllerConfigPtr&,
const VirtualRobot::RobotPtr&,
@@ -66,7 +63,7 @@ namespace armarx
struct NJointControllerRegistration;
template <typename ControlDataStruct>
- class NJointControllerWithTripleBuffer: virtual public NJointController
+ class NJointControllerWithTripleBuffer: public SynchronousNJointController
{
public:
using MutexType = std::recursive_mutex;
@@ -103,119 +100,119 @@ namespace armarx
namespace armarx
{
template<class T>
- inline T* NJointController::useControlTarget(const std::string& deviceName, const std::string& controlMode)
+ inline T* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode)
{
static_assert(std::is_base_of<ControlTargetBase, T>::value, "The given type does not derive ControlTargetBase");
ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode);
return ptr ? ptr->asA<T>(): nullptr;
}
template<class T>
- inline const T* NJointController::useSensorValue(const std::string& deviceName) const
+ inline const T* NJointControllerBase::useSensorValue(const std::string& deviceName) const
{
static_assert(std::is_base_of<SensorValueBase, T>::value, "The given type does not derive SensorValueBase");
const SensorValueBase* const ptr = useSensorValue(deviceName);
return ptr ? ptr->asA<T>(): nullptr;
}
- inline void NJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ inline void SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
- inline bool NJointController::rtUsesControlDevice(std::size_t deviceIndex) const
+ inline bool NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
{
return controlDeviceUsedBitmap.at(deviceIndex);
}
- inline StringStringDictionary NJointController::getControlDeviceUsedControlModeMap(const Ice::Current&) const
+ inline StringStringDictionary NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
{
return controlDeviceControlModeMap;
}
- inline const std::vector<char>& NJointController::getControlDeviceUsedBitmap() const
+ inline const std::vector<char>& NJointControllerBase::getControlDeviceUsedBitmap() const
{
return controlDeviceUsedBitmap;
}
- inline const std::vector<std::size_t>& NJointController::rtGetControlDeviceUsedIndices() const
+ inline const std::vector<std::size_t>& NJointControllerBase::rtGetControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
- inline const std::vector<std::size_t>& NJointController::getControlDeviceUsedIndices() const
+ inline const std::vector<std::size_t>& NJointControllerBase::getControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
- inline const std::map<std::string, const JointController*>& NJointController::getControlDevicesUsedJointController()
+ inline const std::map<std::string, const JointController*>& NJointControllerBase::getControlDevicesUsedJointController()
{
return controlDeviceUsedJointController;
}
- inline boost::optional<std::vector<char> > NJointController::isNotInConflictWith(const NJointControllerPtr& other) const
+ inline boost::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
{
return isNotInConflictWith(other->getControlDeviceUsedBitmap());
}
- inline std::string NJointController::getDefaultName() const
+ inline std::string NJointControllerBase::getDefaultName() const
{
return getClassName();
}
- inline bool NJointController::isControllerActive(const Ice::Current&) const
+ inline bool NJointControllerBase::isControllerActive(const Ice::Current&) const
{
return isActive;
}
- inline bool NJointController::isControllerRequested(const Ice::Current&) const
+ inline bool NJointControllerBase::isControllerRequested(const Ice::Current&) const
{
return isRequested;
}
- inline bool NJointController::isDeletable(const Ice::Current&) const
+ inline bool NJointControllerBase::isDeletable(const Ice::Current&) const
{
return deletable;
}
- inline bool NJointController::hasControllerError(const Ice::Current&) const
+ inline bool NJointControllerBase::hasControllerError(const Ice::Current&) const
{
return deactivatedBecauseOfError;
}
- inline std::string NJointController::getInstanceName(const Ice::Current&) const
+ inline std::string NJointControllerBase::getInstanceName(const Ice::Current&) const
{
return instanceName_;
}
- inline WidgetDescription::StringWidgetDictionary NJointController::getFunctionDescriptions(const Ice::Current&) const
+ inline WidgetDescription::StringWidgetDictionary NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
{
return {};
}
- inline void NJointController::callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current&)
+ inline void NJointControllerBase::callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current&)
{
}
- inline void NJointController::rtSetErrorState()
+ inline void NJointControllerBase::rtSetErrorState()
{
errorState.store(true);
}
- inline bool NJointController::rtGetErrorState() const
+ inline bool NJointControllerBase::rtGetErrorState() const
{
return errorState;
}
- inline std::size_t NJointController::rtGetNumberOfUsedControlDevices() const
+ inline std::size_t NJointControllerBase::rtGetNumberOfUsedControlDevices() const
{
return controlDeviceUsedIndices.size();
}
- inline const std::string& NJointController::rtGetClassName() const
+ inline const std::string& NJointControllerBase::rtGetClassName() const
{
return rtClassName_;
}
- inline const std::string& NJointController::rtGetInstanceName() const
+ inline const std::string& NJointControllerBase::rtGetInstanceName() const
{
return instanceName_;
}
@@ -276,7 +273,7 @@ namespace armarx
namespace armarx
{
template<class ItT>
- inline boost::optional<std::vector<char>> NJointController::AreNotInConflict(ItT first, ItT last)
+ inline boost::optional<std::vector<char>> NJointControllerBase::AreNotInConflict(ItT first, ItT last)
{
if (first == last)
{
@@ -296,83 +293,57 @@ namespace armarx
}
return {true, std::move(inuse)};
}
-
- namespace detail
- {
- template<class NJointControllerT>
- class NJointControllerRegistryEntryHelperBase : public NJointControllerRegistryEntry
+}
+namespace armarx::detail
+{
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigDescription, GenerateConfigDescription,
+ NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigFromVariants, GenerateConfigFromVariants,
+ NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
+
+ template<class NJointControllerT>
+ class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
+ {
+ static_assert(
+ hasGenerateConfigDescription<NJointControllerT>::value ==
+ hasGenerateConfigFromVariants<NJointControllerT>::value,
+ "Either overload both GenerateConfigDescription and GenerateConfigFromVariants, or none!"
+ );
+ static constexpr bool hasRemoteConfiguration_ = hasGenerateConfigDescription<NJointControllerT>::value;
+
+ NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& rob,
+ bool deletable,
+ bool internal,
+ const std::string& instanceName) const final override
{
- NJointControllerPtr create(RobotUnitModule::ControllerManagement* cmngr,
- const NJointControllerConfigPtr& config,
- const VirtualRobot::RobotPtr& rob,
- bool deletable,
- bool internal,
- const std::string& instanceName) const final override
- {
- ARMARX_CHECK_EXPRESSION_W_HINT(cmngr, "ControllerManagement module is NULL!");
+ ARMARX_CHECK_EXPRESSION_W_HINT(cmngr, "ControllerManagement module is NULL!");
- using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
- ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "The configuration is of the wrong type! it has to be an instance of: "
- << GetTypeString<ConfigPtrT>());
+ using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
+ ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "The configuration is of the wrong type! it has to be an instance of: "
+ << GetTypeString<ConfigPtrT>());
- ARMARX_CHECK_EXPRESSION_W_HINT(!ConstructorIsRunning(), "Two NJointControllers are created at the same time");
- NJointControllerPtr ptr;
- {
- ConstructorIsRunning_ = true;
- ARMARX_ON_SCOPE_EXIT{ConstructorIsRunning_ = false;};
- ptr = new NJointControllerT(cmngr->_modulePtr<RobotUnit>(), cfg, rob);
- }
- ptr->deletable = deletable;
- ptr->internal = internal;
- ptr->rtClassName_ = ptr->getClassName(Ice::emptyCurrent);
- ptr->instanceName_ = instanceName;
- return ptr;
- }
- };
-
-
- template<class NJointControllerT, class = void, class = void>
- class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntryHelperBase<NJointControllerT>
- {
- WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&) const final override
+ ARMARX_CHECK_EXPRESSION_W_HINT(!ConstructorIsRunning(), "Two NJointControllers are created at the same time");
+ NJointControllerBasePtr ptr;
{
- ARMARX_CHECK_EXPRESSION(!"This function should never be called");
+ ConstructorIsRunning_ = true;
+ ARMARX_ON_SCOPE_EXIT{ConstructorIsRunning_ = false;};
+ ptr = new NJointControllerT(cmngr->_modulePtr<RobotUnit>(), cfg, rob);
}
- NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&) const override
- {
- ARMARX_CHECK_EXPRESSION(!"This function should never be called");
- }
- bool hasRemoteConfiguration() const override
- {
- return false;
- }
- };
-
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigDescription, GenerateConfigDescription,
- NJointController::GenerateConfigDescriptionFunctionSignature);
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigFromVariants, GenerateConfigFromVariants,
- NJointController::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
-#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T) \
- static_assert(::armarx::detail::hasGenerateConfigDescription<T>::value , \
- #T " does not offer a construction gui (missing: static GenerateConfigDescription)" ); \
- static_assert(::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
- #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
+ ptr->deletable = deletable;
+ ptr->internal = internal;
+ ptr->rtClassName_ = ptr->getClassName(Ice::emptyCurrent);
+ ptr->instanceName_ = instanceName;
+ return ptr;
+ }
- template<class NJointControllerT>
- class NJointControllerRegistryEntryHelper <
- NJointControllerT,
- typename std::enable_if < hasGenerateConfigDescription<NJointControllerT>::value >::type,
- typename std::enable_if < hasGenerateConfigFromVariants<NJointControllerT>::value >::type
- > : public NJointControllerRegistryEntryHelperBase<NJointControllerT>
- {
- WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr& robot,
+ WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr& robot,
const std::map<std::string, ConstControlDevicePtr>& controlDevices,
- const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
- ) const final override
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
+ {
+ if constexpr(hasRemoteConfiguration_)
{
try
{
@@ -400,7 +371,15 @@ namespace armarx
throw;
}
}
- NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& variants) const override
+ else
+ {
+ ARMARX_CHECK_EXPRESSION(!"This function should never be called");
+ }
+ }
+
+ NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
+ {
+ if constexpr(hasRemoteConfiguration_)
{
try
{
@@ -428,13 +407,20 @@ namespace armarx
throw;
}
}
- bool hasRemoteConfiguration() const override
+ else
{
- return true;
+ ARMARX_CHECK_EXPRESSION(!"This function should never be called");
}
- };
- }
+ }
+ bool hasRemoteConfiguration() const final override
+ {
+ return hasRemoteConfiguration_;
+ }
+ };
+}
+namespace armarx
+{
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
template<class ControllerType>
struct NJointControllerRegistration
@@ -447,6 +433,11 @@ namespace armarx
}
};
}
+#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T) \
+ static_assert(::armarx::detail::hasGenerateConfigDescription<T>::value , \
+ #T " does not offer a construction gui (missing: static GenerateConfigDescription)" ); \
+ static_assert(::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
+ #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 639de739475ca5a2b8b12b73f8183697d0898cc2..dc96e422a047fcdfcc5ebe97d14885191e28b857 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -69,7 +69,7 @@ namespace armarx
* @ingroup Library-RobotUnit
* @brief The RobotUnit class manages a robot and its controllers.
*
- * The \ref RobotUnit manages \ref NJointController and \ref JointController for a robot.
+ * The \ref RobotUnit manages \ref NJointControllerBase and \ref JointController for a robot.
* Controllers are executed in a control thread.
*
* \section Controllers
@@ -138,7 +138,7 @@ namespace armarx
* Its tasks are:
*
* - Communication with the Robot
- * - Execution of \ref NJointController and \ref JointController
+ * - Execution of \ref NJointControllerBase and \ref JointController
* - Transferring data (\ref SensorValues, \ref ControlTargets) to other threads
* (publishing, units, logging)
*
@@ -157,7 +157,7 @@ namespace armarx
* Its tasks are:
*
* - Update Units
- * - Call publishing hooks of \ref NJointController
+ * - Call publishing hooks of \ref NJointControllerBase
* - Publish data to \ref RobotUnitListener
*
* \subsection logthrd RtLogging Thread
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
index 560677e96baeb1e9d970a48f0b71809933611da1..0807e71bb1b87c655e2e11925308c5a761155201 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -31,7 +31,6 @@
namespace armarx
{
-
std::string to_string(RobotUnitState s)
{
switch (s)
@@ -55,12 +54,13 @@ namespace armarx
"\nStacktrace\n" + LogSender::CreateBackTrace()
};
}
- namespace RobotUnitModule
- {
+}
- // //////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////// call for each module /////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////// //
+namespace armarx::RobotUnitModule
+{
+ // //////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////// call for each module /////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////// //
#define cast_to_and_call(Type, fn, rethrow) \
try \
{ \
@@ -96,386 +96,385 @@ namespace armarx
std::is_base_of<ModuleBase, Type>::value, \
"The RobotUnitModule '" #Type "' has to derived ModuleBase" \
);
- for_each_module(check_base)
+ for_each_module(check_base)
#undef check_base
- void ModuleBase::checkDerivedClasses() const
- {
+ void ModuleBase::checkDerivedClasses() const
+ {
#define check_deriving(Type) \
ARMARX_CHECK_NOT_NULL_W_HINT( \
dynamic_cast<const Type*>(this), \
"This class does not derive from " << GetTypeString<Type>() \
);
- for_each_module(check_deriving)
+ for_each_module(check_deriving)
#undef check_deriving
- }
- // //////////////////////////////////////////////////////////////////////////// //
- // ManagedIceObject life cycle
- void ModuleBase::onInitComponent()
- {
- checkDerivedClasses();
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
+ }
+ // //////////////////////////////////////////////////////////////////////////// //
+ // ManagedIceObject life cycle
+ void ModuleBase::onInitComponent()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
- cast_to_and_call(::armarx::RobotUnitModule::RobotData, _initVirtualRobot(), true);
+ cast_to_and_call(::armarx::RobotUnitModule::RobotData, _initVirtualRobot(), true);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnInitRobotUnit(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- onInitRobotUnit();
+ onInitRobotUnit();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- finishComponentInitialization();
+ finishComponentInitialization();
- }
+ }
- void ModuleBase::onConnectComponent()
- {
- checkDerivedClasses();
+ void ModuleBase::onConnectComponent()
+ {
+ checkDerivedClasses();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- onConnectRobotUnit();
+ onConnectRobotUnit();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnConnectRobotUnit(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
- void ModuleBase::onDisconnectComponent()
- {
- checkDerivedClasses();
+ }
+ void ModuleBase::onDisconnectComponent()
+ {
+ checkDerivedClasses();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- onDisconnectRobotUnit();
+ onDisconnectRobotUnit();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
+ }
- void ModuleBase::onExitComponent()
- {
- checkDerivedClasses();
- auto guard = getGuard();
+ void ModuleBase::onExitComponent()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
- finishRunning();
+ finishRunning();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- onExitRobotUnit();
+ onExitRobotUnit();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnExitRobotUnit(), false)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
- // //////////////////////////////////////////////////////////////////////////// //
- // other ManagedIceObject / Component methods
- void ModuleBase::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
- {
+ }
+ // //////////////////////////////////////////////////////////////////////////// //
+ // other ManagedIceObject / Component methods
+ void ModuleBase::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+ {
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _componentPropertiesUpdated(changedProperties), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
- void ModuleBase::icePropertiesInitialized()
- {
+ }
+ void ModuleBase::icePropertiesInitialized()
+ {
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesInitialized(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
- // //////////////////////////////////////////////////////////////////////////// //
- // RobotUnit life cycle
- void ModuleBase::finishComponentInitialization()
- {
- checkDerivedClasses();
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
+ }
+ // //////////////////////////////////////////////////////////////////////////// //
+ // RobotUnit life cycle
+ void ModuleBase::finishComponentInitialization()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishComponentInitialization(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingDevices);
+ setRobotUnitState(RobotUnitState::InitializingDevices);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishComponentInitialization(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
- void ModuleBase::finishDeviceInitialization()
- {
- checkDerivedClasses();
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ }
+ void ModuleBase::finishDeviceInitialization()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishDeviceInitialization(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingUnits);
+ setRobotUnitState(RobotUnitState::InitializingUnits);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishDeviceInitialization(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
+ }
- void ModuleBase::finishUnitInitialization()
- {
- checkDerivedClasses();
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ void ModuleBase::finishUnitInitialization()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishUnitInitialization(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingControlThread);
+ setRobotUnitState(RobotUnitState::InitializingControlThread);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishUnitInitialization(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
+ }
- void ModuleBase::finishControlThreadInitialization()
- {
- checkDerivedClasses();
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingControlThread, __FUNCTION__);
+ void ModuleBase::finishControlThreadInitialization()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingControlThread, __FUNCTION__);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishControlThreadInitialization(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::Running);
+ setRobotUnitState(RobotUnitState::Running);
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishControlThreadInitialization(), true)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
+ }
- void ModuleBase::finishRunning()
+ void ModuleBase::finishRunning()
+ {
+ checkDerivedClasses();
+ shutDown();
+ GuardType guard {dataMutex}; //DO NOT USE getGuard here!
+ if (getRobotUnitState() != RobotUnitState::Running)
{
- checkDerivedClasses();
- shutDown();
- GuardType guard {dataMutex}; //DO NOT USE getGuard here!
- if (getRobotUnitState() != RobotUnitState::Running)
- {
- ARMARX_ERROR << "called finishRunning when state was " << to_string(getRobotUnitState());
- }
+ ARMARX_ERROR << "called finishRunning when state was " << to_string(getRobotUnitState());
+ }
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishRunning(), false)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::Exiting);
- joinControlThread();
+ setRobotUnitState(RobotUnitState::Exiting);
+ joinControlThread();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishRunning(), false)
- for_each_module(call_module_hook)
+ for_each_module(call_module_hook)
#undef call_module_hook
- }
+ }
#undef for_each_module_apply
#undef for_each_module
#undef cast_to_and_call
- // //////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////////// other /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////////// other /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////// //
+
+ const std::set<RobotUnitState> ModuleBase::DevicesReadyStates
+ {
+ RobotUnitState::InitializingUnits,
+ RobotUnitState::InitializingControlThread,
+ RobotUnitState::Running
+ };
+
+ void ModuleBase::setRobotUnitState(RobotUnitState to)
+ {
+ auto guard = getGuard();
- const std::set<RobotUnitState> ModuleBase::DevicesReadyStates
+ const auto transitionErrorMessage = [ = ](RobotUnitState expectedFrom)
{
- RobotUnitState::InitializingUnits,
- RobotUnitState::InitializingControlThread,
- RobotUnitState::Running
+ return "Can't switch to state " + to_string(to) + " from " + to_string(state) +
+ "! The expected source state is " + to_string(expectedFrom) + ".";
};
-
- void ModuleBase::setRobotUnitState(RobotUnitState to)
+ const auto transitionErrorThrow = [ = ](RobotUnitState expectedFrom)
{
- auto guard = getGuard();
-
- const auto transitionErrorMessage = [ = ](RobotUnitState expectedFrom)
- {
- return "Can't switch to state " + to_string(to) + " from " + to_string(state) +
- "! The expected source state is " + to_string(expectedFrom) + ".";
- };
- const auto transitionErrorThrow = [ = ](RobotUnitState expectedFrom)
+ if (state != expectedFrom)
{
- if (state != expectedFrom)
+ const auto msg = transitionErrorMessage(expectedFrom);
+ ARMARX_ERROR << msg;
+ throw std::invalid_argument {msg};
+ }
+ };
+
+ switch (to)
+ {
+ case armarx::RobotUnitState::InitializingComponent:
+ throw std::invalid_argument {"Can't switch to state RobotUnitState::InitializingComponent"};
+ case armarx::RobotUnitState::InitializingDevices:
+ transitionErrorThrow(RobotUnitState::InitializingComponent);
+ break;
+ case RobotUnitState::InitializingUnits:
+ transitionErrorThrow(RobotUnitState::InitializingDevices);
+ break;
+ case RobotUnitState::InitializingControlThread:
+ transitionErrorThrow(RobotUnitState::InitializingUnits);
+ break;
+ case RobotUnitState::Running:
+ transitionErrorThrow(RobotUnitState::InitializingControlThread);
+ break;
+ case RobotUnitState::Exiting:
+ if (state != RobotUnitState::Running)
{
- const auto msg = transitionErrorMessage(expectedFrom);
- ARMARX_ERROR << msg;
- throw std::invalid_argument {msg};
+ ARMARX_ERROR << transitionErrorMessage(RobotUnitState::Running);
}
- };
-
- switch (to)
- {
- case armarx::RobotUnitState::InitializingComponent:
- throw std::invalid_argument {"Can't switch to state RobotUnitState::InitializingComponent"};
- case armarx::RobotUnitState::InitializingDevices:
- transitionErrorThrow(RobotUnitState::InitializingComponent);
- break;
- case RobotUnitState::InitializingUnits:
- transitionErrorThrow(RobotUnitState::InitializingDevices);
- break;
- case RobotUnitState::InitializingControlThread:
- transitionErrorThrow(RobotUnitState::InitializingUnits);
- break;
- case RobotUnitState::Running:
- transitionErrorThrow(RobotUnitState::InitializingControlThread);
- break;
- case RobotUnitState::Exiting:
- if (state != RobotUnitState::Running)
- {
- ARMARX_ERROR << transitionErrorMessage(RobotUnitState::Running);
- }
- break;
- default:
- throw std::invalid_argument
- {
- "setRobotUnitState: Unknown target state " + to_string(static_cast<std::size_t>(to)) +
- "\nStacktrace\n" + LogSender::CreateBackTrace()
- };
- }
- ARMARX_INFO << "switching state from " << to_string(state) << " to " << to_string(to);
- state = to;
+ break;
+ default:
+ throw std::invalid_argument
+ {
+ "setRobotUnitState: Unknown target state " + to_string(static_cast<std::size_t>(to)) +
+ "\nStacktrace\n" + LogSender::CreateBackTrace()
+ };
}
+ ARMARX_INFO << "switching state from " << to_string(state) << " to " << to_string(to);
+ state = to;
+ }
+
+
+ bool ModuleBase::areDevicesReady() const
+ {
+ return DevicesReadyStates.count(state);
+ }
+ bool ModuleBase::inControlThread() const
+ {
+ return getRobotUnitState() == RobotUnitState::Running &&
+ std::this_thread::get_id() == _module<ControlThread>().getControlThreadId();
+ }
- bool ModuleBase::areDevicesReady() const
+ void ModuleBase::throwIfInControlThread(const std::string& fnc) const
+ {
+ if (inControlThread())
{
- return DevicesReadyStates.count(state);
+ std::stringstream str;
+ str << "Called function '" << fnc << "' in the Control Thread\nStack trace:\n" << LogSender::CreateBackTrace();
+ ARMARX_ERROR << str.str();
+ throw LogicError {str.str()};
}
+ }
- bool ModuleBase::inControlThread() const
+ ModuleBase::GuardType ModuleBase::getGuard() const
+ {
+ if (inControlThread())
+ {
+ throw LogicError
+ {
+ "Attempted to lock mutex in Control Thread\nStack trace:\n" +
+ LogSender::CreateBackTrace()
+ };
+ }
+ GuardType guard {dataMutex, std::defer_lock};
+ if (guard.try_lock())
{
- return getRobotUnitState() == RobotUnitState::Running &&
- std::this_thread::get_id() == _module<ControlThread>().getControlThreadId();
+ return guard;
}
-
- void ModuleBase::throwIfInControlThread(const std::string& fnc) const
+ while (!guard.try_lock_for(std::chrono::microseconds {100}))
{
- if (inControlThread())
+ if (isShuttingDown())
{
- std::stringstream str;
- str << "Called function '" << fnc << "' in the Control Thread\nStack trace:\n" << LogSender::CreateBackTrace();
- ARMARX_ERROR << str.str();
- throw LogicError {str.str()};
+ throw LogicError {"Attempting to lock mutex during shutdown\nStacktrace\n" + LogSender::CreateBackTrace()};
}
}
- ModuleBase::GuardType ModuleBase::getGuard() const
+ return guard;
+ }
+
+ void ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
+ {
+ if (! stateSet.count(state))
{
- if (inControlThread())
+ std::stringstream ss;
+ ss << fnc << ": Can't be called if state is not in {";
+ bool fst = true;
+ for (RobotUnitState st : stateSet)
{
- throw LogicError
+ if (!fst)
{
- "Attempted to lock mutex in Control Thread\nStack trace:\n" +
- LogSender::CreateBackTrace()
- };
+ ss << ", ";
+ }
+ ss << st;
+ fst = false;
}
- GuardType guard {dataMutex, std::defer_lock};
- if (guard.try_lock())
+ ss << "} (current state " << getRobotUnitState() << ")\n"
+ << "Stacktrace:\n" << LogSender::CreateBackTrace();
+ ARMARX_ERROR << ss.str();
+ if (!onlyWarn)
{
- return guard;
+ throw LogicError {ss.str()};
}
- while (!guard.try_lock_for(std::chrono::microseconds {100}))
- {
- if (isShuttingDown())
- {
- throw LogicError {"Attempting to lock mutex during shutdown\nStacktrace\n" + LogSender::CreateBackTrace()};
- }
- }
-
- return guard;
}
+ }
+ void ModuleBase::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
+ {
+ throwIfStateIsNot(std::set<RobotUnitState> {s}, fnc, onlyWarn);
+ }
- void ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
+ void ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
+ {
+ if (stateSet.count(state))
{
- if (! stateSet.count(state))
+ std::stringstream ss;
+ ss << fnc << ": Can't be called if state is in {";
+ bool fst = true;
+ for (RobotUnitState st : stateSet)
{
- std::stringstream ss;
- ss << fnc << ": Can't be called if state is not in {";
- bool fst = true;
- for (RobotUnitState st : stateSet)
- {
- if (!fst)
- {
- ss << ", ";
- }
- ss << st;
- fst = false;
- }
- ss << "} (current state " << getRobotUnitState() << ")\n"
- << "Stacktrace:\n" << LogSender::CreateBackTrace();
- ARMARX_ERROR << ss.str();
- if (!onlyWarn)
+ if (!fst)
{
- throw LogicError {ss.str()};
+ ss << ", ";
}
+ ss << st;
+ fst = false;
}
- }
- void ModuleBase::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
- {
- throwIfStateIsNot(std::set<RobotUnitState> {s}, fnc, onlyWarn);
- }
-
- void ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
- {
- if (stateSet.count(state))
+ ss << "} (current state " << getRobotUnitState() << ")\n"
+ << "Stacktrace:\n" << LogSender::CreateBackTrace();
+ ARMARX_ERROR << ss.str();
+ if (!onlyWarn)
{
- std::stringstream ss;
- ss << fnc << ": Can't be called if state is in {";
- bool fst = true;
- for (RobotUnitState st : stateSet)
- {
- if (!fst)
- {
- ss << ", ";
- }
- ss << st;
- fst = false;
- }
- ss << "} (current state " << getRobotUnitState() << ")\n"
- << "Stacktrace:\n" << LogSender::CreateBackTrace();
- ARMARX_ERROR << ss.str();
- if (!onlyWarn)
- {
- throw LogicError {ss.str()};
- }
+ throw LogicError {ss.str()};
}
}
- void ModuleBase::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
- {
- throwIfStateIs(std::set<RobotUnitState> {s}, fnc, onlyWarn);
- }
+ }
+ void ModuleBase::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
+ {
+ throwIfStateIs(std::set<RobotUnitState> {s}, fnc, onlyWarn);
+ }
- void ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
- {
- throwIfStateIsNot(DevicesReadyStates, fnc);
- }
+ void ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
+ {
+ throwIfStateIsNot(DevicesReadyStates, fnc);
+ }
- std::atomic<ModuleBase*> ModuleBase::Instance_ {nullptr};
+ std::atomic<ModuleBase*> ModuleBase::Instance_ {nullptr};
- ModuleBase::ModuleBase()
+ ModuleBase::ModuleBase()
+ {
+ if (Instance_ && this != Instance_)
{
- if (Instance_ && this != Instance_)
- {
- ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = " << Instance_ << ", this = " << this << ")";
- std::terminate();
- }
- Instance_ = this;
+ ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = " << Instance_ << ", this = " << this << ")";
+ std::terminate();
}
-
+ Instance_ = this;
}
+
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
index 8c107ae88887a6ba8a98fd298efdd6a3bd520f26..474c2d611ba4d12b20cd402851b3ab26cc84cba1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
@@ -49,7 +49,7 @@
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
- TYPEDEF_PTRS_HANDLE(NJointController);
+ TYPEDEF_PTRS_HANDLE(NJointControllerBase);
template<class Targ, class Src>
Targ& CheckedCastAndDeref(Src* src)
@@ -91,536 +91,534 @@ namespace armarx
}
}
-namespace armarx
+namespace armarx::RobotUnitModule::detail
{
- namespace RobotUnitModule
+ class ModuleBaseIntermediatePropertyDefinitions: public ComponentPropertyDefinitions
{
- namespace detail
- {
- class ModuleBaseIntermediatePropertyDefinitions: public ComponentPropertyDefinitions
- {
- public:
- ModuleBaseIntermediatePropertyDefinitions(): ComponentPropertyDefinitions("") {}
- };
- }
+ public:
+ ModuleBaseIntermediatePropertyDefinitions(): ComponentPropertyDefinitions("") {}
+ };
+}
- class ModuleBasePropertyDefinitions: virtual public detail::ModuleBaseIntermediatePropertyDefinitions
+namespace armarx::RobotUnitModule
+{
+ class ModuleBasePropertyDefinitions: virtual public detail::ModuleBaseIntermediatePropertyDefinitions
+ {
+ public:
+ ModuleBasePropertyDefinitions(std::string prefix)
{
- public:
- ModuleBasePropertyDefinitions(std::string prefix)
- {
- setDescription(prefix + " properties");
- setPrefix(prefix + ".");
- }
- };
+ setDescription(prefix + " properties");
+ setPrefix(prefix + ".");
+ }
+ };
#define forward_declare(r, data, Mod) TYPEDEF_PTRS_HANDLE(Mod);
- BOOST_PP_SEQ_FOR_EACH(forward_declare,, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
+ BOOST_PP_SEQ_FOR_EACH(forward_declare,, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
#undef forward_declare
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief Base class for all RobotUnitModules
+ *
+ * Since the robot unit is a complex class, it is split into several modules.
+ * One reason behind this is stopping developers from accidently using datastructures in the wrong way (e.g. causing racing conditions)
+ * For a \ref RobotUnit to work, ALL modules are required (A class has to derive all of them)
+ *
+ * \section Modules
+ * The list of Modules is:
+ * - \ref ControllerManagement
+ * - \ref ControlThread
+ * - \ref ControlThreadDataBuffer
+ * - \ref Devices
+ * - \ref Logging
+ * - \ref Management
+ * - \ref Publisher
+ * - \ref RobotData
+ * - \ref SelfCollisionChecker
+ * - \ref Units
+ *
+ * \section LifeCycle Life cycle
+ * The RobotUnit has multiple Phases (see \ref RobotUnitState) as well as the life cycle of a \ref ManagedIceObject.
+ *
+ * \subsection LifeCycleMIO ManagedIceObject Life cycle
+ * The hooks (\ref onInitComponent, \ref onConnectComponent, \ref onDisconnectComponent, \ref onExitComponent)
+ * for state transitions provided by \ref ManagedIceObject have a final overrider.
+ * Deriving classes have to use the replacement hooks:
+ *
+ <table>
+ <tr><th> Hook <th> Replacement \th
+ <tr><td> \ref onInitComponent <td> \ref onInitRobotUnit
+ <tr><td> \ref onConnectComponent <td> \ref onConnectRobotUnit
+ <tr><td> \ref onDisconnectComponent <td> \ref onDisconnectRobotUnit
+ <tr><td> \ref onExitComponent <td> \ref onExitRobotUnit
+ </table>
+
+ * \subsection LifeCycleRU RobotUnit Life cycle
+ *
+ * The initial phase is \ref RobotUnitState::InitializingComponent.
+ * The following table shows all Phase transitions and the functions causing it:
+ *
+ <table>
+ <tr><th> From <th> To <th> Transition function <th> Note \th
+ <tr><td> \ref RobotUnitState::InitializingComponent <td> \ref RobotUnitState::InitializingDevices <td> \ref finishComponentInitialization <td> Called in \ref onInitComponent
+ <tr><td> \ref RobotUnitState::InitializingDevices <td> \ref RobotUnitState::InitializingUnits <td> \ref finishDeviceInitialization <td>
+ <tr><td> \ref RobotUnitState::InitializingUnits <td> \ref RobotUnitState::InitializingControlThread <td> \ref finishUnitInitialization <td>
+ <tr><td> \ref RobotUnitState::InitializingControlThread <td> \ref RobotUnitState::Running <td> \ref finishControlThreadInitialization <td> Has to be called in the control thread
+ <tr><td> \ref RobotUnitState::Running <td> \ref RobotUnitState::Exiting <td> \ref finishRunning <td> Called in \ref onExitComponent
+ </table>
+ *
+ *
+ *
+ * \subsection RUMLifeCycleHooks RobotUnitModule Hooks for Life Cycle transition functions
+ *
+ * Each RobotUnitModule has two hooks each transition function ().
+ * - The '_pre<TRANSITION_FUNCTION_NAME>' hook is called in the transition function before the state has changed.
+ * - The '_post<TRANSITION_FUNCTION_NAME>' hook is called in the transition function after the state has changed.
+ *
+ * \warning The same hook of different modules (e.g.: \ref _preFinishDeviceInitialization, \ref _postOnInitRobotUnit) must not depend on each other.
+ */
+ class ModuleBase : virtual public Component
+ {
+ /// @brief The singleton instance
+ static std::atomic<ModuleBase*> Instance_;
+ public:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief Base class for all RobotUnitModules
- *
- * Since the robot unit is a complex class, it is split into several modules.
- * One reason behind this is stopping developers from accidently using datastructures in the wrong way (e.g. causing racing conditions)
- * For a \ref RobotUnit to work, ALL modules are required (A class has to derive all of them)
- *
- * \section Modules
- * The list of Modules is:
- * - \ref ControllerManagement
- * - \ref ControlThread
- * - \ref ControlThreadDataBuffer
- * - \ref Devices
- * - \ref Logging
- * - \ref Management
- * - \ref Publisher
- * - \ref RobotData
- * - \ref SelfCollisionChecker
- * - \ref Units
- *
- * \section LifeCycle Life cycle
- * The RobotUnit has multiple Phases (see \ref RobotUnitState) as well as the life cycle of a \ref ManagedIceObject.
- *
- * \subsection LifeCycleMIO ManagedIceObject Life cycle
- * The hooks (\ref onInitComponent, \ref onConnectComponent, \ref onDisconnectComponent, \ref onExitComponent)
- * for state transitions provided by \ref ManagedIceObject have a final overrider.
- * Deriving classes have to use the replacement hooks:
- *
- <table>
- <tr><th> Hook <th> Replacement \th
- <tr><td> \ref onInitComponent <td> \ref onInitRobotUnit
- <tr><td> \ref onConnectComponent <td> \ref onConnectRobotUnit
- <tr><td> \ref onDisconnectComponent <td> \ref onDisconnectRobotUnit
- <tr><td> \ref onExitComponent <td> \ref onExitRobotUnit
- </table>
-
- * \subsection LifeCycleRU RobotUnit Life cycle
- *
- * The initial phase is \ref RobotUnitState::InitializingComponent.
- * The following table shows all Phase transitions and the functions causing it:
- *
- <table>
- <tr><th> From <th> To <th> Transition function <th> Note \th
- <tr><td> \ref RobotUnitState::InitializingComponent <td> \ref RobotUnitState::InitializingDevices <td> \ref finishComponentInitialization <td> Called in \ref onInitComponent
- <tr><td> \ref RobotUnitState::InitializingDevices <td> \ref RobotUnitState::InitializingUnits <td> \ref finishDeviceInitialization <td>
- <tr><td> \ref RobotUnitState::InitializingUnits <td> \ref RobotUnitState::InitializingControlThread <td> \ref finishUnitInitialization <td>
- <tr><td> \ref RobotUnitState::InitializingControlThread <td> \ref RobotUnitState::Running <td> \ref finishControlThreadInitialization <td> Has to be called in the control thread
- <tr><td> \ref RobotUnitState::Running <td> \ref RobotUnitState::Exiting <td> \ref finishRunning <td> Called in \ref onExitComponent
- </table>
- *
- *
- *
- * \subsection RUMLifeCycleHooks RobotUnitModule Hooks for Life Cycle transition functions
- *
- * Each RobotUnitModule has two hooks each transition function ().
- * - The '_pre<TRANSITION_FUNCTION_NAME>' hook is called in the transition function before the state has changed.
- * - The '_post<TRANSITION_FUNCTION_NAME>' hook is called in the transition function after the state has changed.
- *
- * \warning The same hook of different modules (e.g.: \ref _preFinishDeviceInitialization, \ref _postOnInitRobotUnit) must not depend on each other.
- */
- class ModuleBase : virtual public Component
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
+ */
+ static ModuleBase& Instance()
{
- /// @brief The singleton instance
- static std::atomic<ModuleBase*> Instance_;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static ModuleBase& Instance()
- {
- ARMARX_CHECK_NOT_NULL(Instance_);
- return *Instance_;
- }
- /**
- * @brief Returns the singleton instance of the given class
- * @return The singleton instance of the given class
- */
- template<class T>
- static T& Instance()
- {
- static_assert(std::is_base_of<ModuleBase, T>::value, "The type has to derive ModuleBase");
- ARMARX_CHECK_NOT_NULL(Instance_);
- return dynamic_cast<T&>(*Instance_);
- }
- protected:
- /// @brief Ctor
- ModuleBase();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////////// types ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- /**
- * @brief The type of recursive_mutex used in this class.
- * This typedef is used to quickly replace the mutex type with a logging mutex type for debugging.
- */
- using MutexType = std::recursive_timed_mutex;
- using GuardType = std::unique_lock<MutexType>;
- using ClockType = std::chrono::high_resolution_clock;
- using TimePointType = std::chrono::high_resolution_clock::time_point;
+ ARMARX_CHECK_NOT_NULL(Instance_);
+ return *Instance_;
+ }
+ /**
+ * @brief Returns the singleton instance of the given class
+ * @return The singleton instance of the given class
+ */
+ template<class T>
+ static T& Instance()
+ {
+ static_assert(std::is_base_of<ModuleBase, T>::value, "The type has to derive ModuleBase");
+ ARMARX_CHECK_NOT_NULL(Instance_);
+ return dynamic_cast<T&>(*Instance_);
+ }
+ protected:
+ /// @brief Ctor
+ ModuleBase();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////////// types ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ /**
+ * @brief The type of recursive_mutex used in this class.
+ * This typedef is used to quickly replace the mutex type with a logging mutex type for debugging.
+ */
+ using MutexType = std::recursive_timed_mutex;
+ using GuardType = std::unique_lock<MutexType>;
+ using ClockType = std::chrono::high_resolution_clock;
+ using TimePointType = std::chrono::high_resolution_clock::time_point;
#define using_module_types(...) using_module_types_impl(__VA_ARGS__)
#define using_module_types_impl(r, data, Mod) using Module ## Mod = Mod;
- BOOST_PP_SEQ_FOR_EACH(using_module_types,, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
+ BOOST_PP_SEQ_FOR_EACH(using_module_types,, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
#undef using_module_types
#undef using_module_types_impl
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////////// access to modules /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- /// @brief Checks whether the implementing class derives all modules.
- void checkDerivedClasses() const;
- public:
- /**
- * @brief Returns this as ref to the given type.
- * @return This as ref to the given type.
- */
- template<class T> T& _module()
- {
- return dynamic_cast< T&>(*this);
- }
- /**
- * @brief Returns this as ref to the given type.
- * @return This as ref to the given type.
- */
- template<class T> const T& _module() const
- {
- return dynamic_cast<const T&>(*this);
- }
- /**
- * @brief Returns this as ptr to the given type.
- * @return This as ptr to the given type.
- */
- template<class T> T* _modulePtr()
- {
- return &_module< T>();
- }
- /**
- * @brief Returns this as ptr to the given type.
- * @return This as ptr to the given type.
- */
- template<class T> const T* _modulePtr() const
- {
- return &_module<const T>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////// ManagedIceObject life cycle ///////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- //ManagedIceObject life cycle
- /**
- * @brief \note This function is protected with \ref getGuard()
- * @see \ref ManagedIceObject::onInitComponent
- */
- void onInitComponent() final override;
- /**
- * @brief \warning This function is not protected with \ref getGuard()
- * @see \ref ManagedIceObject::onConnectComponent
- */
- void onConnectComponent() final override;
- /**
- * @brief \warning This function is not protected with \ref getGuard()
- * @see \ref ManagedIceObject::onDisconnectComponent
- */
- void onDisconnectComponent() final override;
- /**
- * @brief \note This function is protected with \ref getGuard()
- * @see \ref ManagedIceObject::onExitComponent
- */
- void onExitComponent() final override;
-
- //ManagedIceObject life cycle module hooks
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called before \ref onInitRobotUnit was called)
- * \note This function is protected with \ref getGuard()
- * @see onInitComponent
- */
- void _preOnInitRobotUnit() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called after \ref onInitRobotUnit was called)
- * \note This function is protected with \ref getGuard()
- * @see onInitComponent
- */
- void _postOnInitRobotUnit() {}
-
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref onConnectComponent (called before \ref onConnectRobotUnit was called)
- * \warning This function is not protected with \ref getGuard()
- * @see onConnectComponent
- */
- void _preOnConnectRobotUnit() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref onConnectComponent (called after \ref onConnectRobotUnit was called)
- * \warning This function is not protected with \ref getGuard()
- * @see onConnectComponent
- */
- void _postOnConnectRobotUnit() {}
-
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref onDisconnectComponent (called before \ref onDisconnectRobotUnit was called)
- * \warning This function is not protected with \ref getGuard()
- * @see onDisconnectComponent
- */
- void _preOnDisconnectRobotUnit() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref onDisconnectComponent (called after \ref onDisconnectRobotUnit was called)
- * \warning This function is not protected with \ref getGuard()
- * @see onDisconnectComponent
- */
- void _postOnDisconnectRobotUnit() {}
-
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref onExitComponent (called before \ref onExitRobotUnit was called)
- * \note This function is protected with \ref getGuard()
- * @see onExitComponent
- */
- void _preOnExitRobotUnit() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref onExitComponent (called after \ref onExitRobotUnit was called)
- * \note This function is protected with \ref getGuard()
- * @see onExitComponent
- */
- void _postOnExitRobotUnit() {}
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ////////////////////// other ManagedIceObject / Component methods ////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- /// @see \ref ManagedIceObject::componentPropertiesUpdated
- void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
- /// @see \ref ManagedIceObject::icePropertiesInitialized
- void icePropertiesInitialized() override;
- /// @see \ref ManagedIceObject::getDefaultName
- std::string getDefaultName() const override
- {
- return "RobotUnit";
- }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////////// access to modules /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ /// @brief Checks whether the implementing class derives all modules.
+ void checkDerivedClasses() const;
+ public:
+ /**
+ * @brief Returns this as ref to the given type.
+ * @return This as ref to the given type.
+ */
+ template<class T> T& _module()
+ {
+ return dynamic_cast< T&>(*this);
+ }
+ /**
+ * @brief Returns this as ref to the given type.
+ * @return This as ref to the given type.
+ */
+ template<class T> const T& _module() const
+ {
+ return dynamic_cast<const T&>(*this);
+ }
+ /**
+ * @brief Returns this as ptr to the given type.
+ * @return This as ptr to the given type.
+ */
+ template<class T> T* _modulePtr()
+ {
+ return &_module< T>();
+ }
+ /**
+ * @brief Returns this as ptr to the given type.
+ * @return This as ptr to the given type.
+ */
+ template<class T> const T* _modulePtr() const
+ {
+ return &_module<const T>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////// ManagedIceObject life cycle ///////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ //ManagedIceObject life cycle
+ /**
+ * @brief \note This function is protected with \ref getGuard()
+ * @see \ref ManagedIceObject::onInitComponent
+ */
+ void onInitComponent() final override;
+ /**
+ * @brief \warning This function is not protected with \ref getGuard()
+ * @see \ref ManagedIceObject::onConnectComponent
+ */
+ void onConnectComponent() final override;
+ /**
+ * @brief \warning This function is not protected with \ref getGuard()
+ * @see \ref ManagedIceObject::onDisconnectComponent
+ */
+ void onDisconnectComponent() final override;
+ /**
+ * @brief \note This function is protected with \ref getGuard()
+ * @see \ref ManagedIceObject::onExitComponent
+ */
+ void onExitComponent() final override;
- //module hooks
- /// @brief Hook for deriving RobotUnitModules called in \ref componentPropertiesUpdated
- void _componentPropertiesUpdated(const std::set<std::string>& /*changedProperties*/) {}
- /// @brief Hook for deriving RobotUnitModules called in \ref icePropertiesInitialized
- void _icePropertiesInitialized() {}
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////// state and state transition ////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * @brief Sets the \ref RobotUnit's state to \param s
- * @param s The state to set
- */
- void setRobotUnitState(RobotUnitState s);
- protected:
- //state transitions
- /**
- * @brief Transition \ref RobotUnitState::InitializingComponent -> \ref RobotUnitState::InitializingDevices
- * \note This function is protected with \ref getGuard()
- * @see _preFinishComponentInitialization
- * @see _postFinishComponentInitialization
- */
- virtual void finishComponentInitialization();
- /**
- * @brief Transition \ref RobotUnitState::InitializingDevices -> \ref RobotUnitState::InitializingUnits
- * \note This function is protected with \ref getGuard()
- * @see _preFinishDeviceInitialization
- * @see _postFinishDeviceInitialization
- */
- virtual void finishDeviceInitialization();
- /**
- * @brief Transition \ref RobotUnitState::InitializingUnits -> \ref RobotUnitState::WaitingForRTThreadInitialization
- * \note This function is protected with \ref getGuard()
- * @see _preFinishUnitInitialization
- * @see _postFinishUnitInitialization
- */
- virtual void finishUnitInitialization();
- /**
- * @brief Transition \ref RobotUnitState::InitializingControlThread -> \ref RobotUnitState::Running
- * \note This function is protected with \ref getGuard()
- * @see _preFinishControlThreadInitialization
- * @see _postFinishControlThreadInitialization
- */
- virtual void finishControlThreadInitialization();
- /**
- * @brief Transition \ref RobotUnitState::Running -> \ref RobotUnitState::Exiting
- * \note This function is protected with \ref getGuard()
- * @see _preFinishRunning
- * @see _postFinishRunning
- */
- virtual void finishRunning();
-
- //state transition module hooks
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishComponentInitialization (called before the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishComponentInitialization
- */
- void _preFinishComponentInitialization() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishComponentInitialization (called after the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishComponentInitialization
- */
- void _postFinishComponentInitialization() {}
-
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called before the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishDeviceInitialization
- */
- void _preFinishDeviceInitialization() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called after the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishDeviceInitialization
- */
- void _postFinishDeviceInitialization() {}
-
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called before the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishUnitInitialization
- */
- void _preFinishUnitInitialization() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called after the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishUnitInitialization
- */
- void _postFinishUnitInitialization() {}
-
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called before the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishControlThreadInitialization
- */
- void _preFinishControlThreadInitialization() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called after the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishControlThreadInitialization
- */
- void _postFinishControlThreadInitialization() {}
-
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called before the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishRunning
- */
- void _preFinishRunning() {}
- /**
- * @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called after the state has changed)
- * \note This function is protected with \ref getGuard()
- * @see finishRunning
- */
- void _postFinishRunning() {}
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////// utility //////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Returns the \ref RobotUnit's State
- * @return The \ref RobotUnit's State
- */
- RobotUnitState getRobotUnitState() const;
-
- /**
- * @brief Throws an exception if the current state is not in \param stateSet
- * @param stateSet The set of acceptable \ref RobotUnitState "RobotUnitStates"
- * @param fnc The callers function name.
- * @param onlyWarn Only print a warning instead of throwing an exception.
- */
- void throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn = false) const;
- /**
- * @brief Throws an exception if the current state is not \param state
- * @param state The acceptable \ref RobotUnitState
- * @param fnc The callers function name.
- * @param onlyWarn Only print a warning instead of throwing an exception.
- */
- void throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
- /**
- * @brief Throws an exception if the current state is in \param stateSet
- * @param stateSet The set of forbidden \ref RobotUnitState "RobotUnitStates"
- * @param fnc The callers function name.
- * @param onlyWarn Only print a warning instead of throwing an exception.
- */
- void throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn = false) const;
- /**
- * @brief Throws an exception if the current state is \param state
- * @param state The forbidden \ref RobotUnitState
- * @param fnc The callers function name.
- * @param onlyWarn Only print a warning instead of throwing an exception.
- */
- void throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
- /**
- * @brief Throws if the \ref Device "Devices" are not ready.
- * @param fnc The callers function name.
- * @see areDevicesReady
- */
- void throwIfDevicesNotReady(const std::string& fnc) const;
-
- /**
- * @brief Returns whether \ref Device "Devices" are ready
- * @return Whether \ref Device "Devices" are ready
- * @see \ref DevicesReadyStates
- */
- bool areDevicesReady() const;
-
- /**
- * @brief Returns whether the current thread is the \ref ControlThread.
- * @return Whether the current thread is the \ref ControlThread.
- */
- bool inControlThread() const;
-
- /**
- * @brief Throws if the current thread is the \ref ControlThread.
- * @param fnc The callers function name.
- */
- void throwIfInControlThread(const std::string& fnc) const;
-
- /**
- * @brief Returns whether the \ref RobotUnit is shutting down
- * @return Whether the \ref RobotUnit is shutting down
- * @see shutDown
- */
- bool isShuttingDown() const ///TODO use this function in implementations
- {
- return isShuttingDown_.load();
- }
- /**
- * @brief Requests the \ref RobotUnit to shut down.
- * @see isShuttingDown
- */
- void shutDown() ///TODO use this function in implementations
- {
- isShuttingDown_.store(true);
- }
- // //////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////// robot unit impl hooks ////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////// //
- /**
- * @brief called in onInitComponent
- * \note This function is protected with \ref getGuard()
- * @see onInitComponent
- */
- virtual void onInitRobotUnit() {}
- /**
- * @brief called in onConnectComponent
- * \warning This function is not protected with \ref getGuard()
- * @see onConnectComponent
- */
- virtual void onConnectRobotUnit() {}
- /**
- * @brief called in onDisconnecComponent
- * \warning This function is not protected with \ref getGuard()
- * @see onDisconnecComponent
- */
- virtual void onDisconnectRobotUnit() {}
- /**
- * @brief called in onExitComponent before calling finishRunning
- * \note This function is protected with \ref getGuard()
- * @see finishRunning
- * @see onExitComponent
- */
- virtual void onExitRobotUnit() {}
-
- /// @brief Implementations have to join their \ref ControlThread in this hook. (used by RobotUnit::finishRunning())
- virtual void joinControlThread() = 0;
-
- /// @brief The \ref ControlThread's period
- virtual IceUtil::Time getControlThreadTargetPeriod() const = 0;
- // //////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////////// other /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////// //
- protected:
- /**
- * @brief Returns a sentinel value for an index (std::numeric_limits<std::size_t>::max())
- * @return A sentinel value for an index (std::numeric_limits<std::size_t>::max())
- */
- static constexpr std::size_t IndexSentinel()
- {
- return std::numeric_limits<std::size_t>::max();
- }
- /**
- * @brief Returns a guard to the \ref RobotUnits mutex.
- * @return A guard to the \ref RobotUnits mutex.
- * @throw throws If called in the \ref ControlThread or on shutdown.
- */
- GuardType getGuard() const;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief The \ref RobotUnits mutex.
- mutable MutexType dataMutex;
- /// @brief Set of \ref RobotUnitState "RobotUnitStates" where \ref Device "Devices" are usable
- static const std::set<RobotUnitState> DevicesReadyStates;
- /// @brief The \ref RobotUnit's current state
- std::atomic<RobotUnitState> state {RobotUnitState::InitializingComponent};
- /// @brief Whether the \ref RobotUnit is shutting down
- std::atomic_bool isShuttingDown_ {false};
- };
- }
+ //ManagedIceObject life cycle module hooks
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called before \ref onInitRobotUnit was called)
+ * \note This function is protected with \ref getGuard()
+ * @see onInitComponent
+ */
+ void _preOnInitRobotUnit() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called after \ref onInitRobotUnit was called)
+ * \note This function is protected with \ref getGuard()
+ * @see onInitComponent
+ */
+ void _postOnInitRobotUnit() {}
+
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref onConnectComponent (called before \ref onConnectRobotUnit was called)
+ * \warning This function is not protected with \ref getGuard()
+ * @see onConnectComponent
+ */
+ void _preOnConnectRobotUnit() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref onConnectComponent (called after \ref onConnectRobotUnit was called)
+ * \warning This function is not protected with \ref getGuard()
+ * @see onConnectComponent
+ */
+ void _postOnConnectRobotUnit() {}
+
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref onDisconnectComponent (called before \ref onDisconnectRobotUnit was called)
+ * \warning This function is not protected with \ref getGuard()
+ * @see onDisconnectComponent
+ */
+ void _preOnDisconnectRobotUnit() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref onDisconnectComponent (called after \ref onDisconnectRobotUnit was called)
+ * \warning This function is not protected with \ref getGuard()
+ * @see onDisconnectComponent
+ */
+ void _postOnDisconnectRobotUnit() {}
+
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref onExitComponent (called before \ref onExitRobotUnit was called)
+ * \note This function is protected with \ref getGuard()
+ * @see onExitComponent
+ */
+ void _preOnExitRobotUnit() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref onExitComponent (called after \ref onExitRobotUnit was called)
+ * \note This function is protected with \ref getGuard()
+ * @see onExitComponent
+ */
+ void _postOnExitRobotUnit() {}
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////// other ManagedIceObject / Component methods ////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ /// @see \ref ManagedIceObject::componentPropertiesUpdated
+ void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
+ /// @see \ref ManagedIceObject::icePropertiesInitialized
+ void icePropertiesInitialized() override;
+ /// @see \ref ManagedIceObject::getDefaultName
+ std::string getDefaultName() const override
+ {
+ return "RobotUnit";
+ }
+
+ //module hooks
+ /// @brief Hook for deriving RobotUnitModules called in \ref componentPropertiesUpdated
+ void _componentPropertiesUpdated(const std::set<std::string>& /*changedProperties*/) {}
+ /// @brief Hook for deriving RobotUnitModules called in \ref icePropertiesInitialized
+ void _icePropertiesInitialized() {}
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////// state and state transition ////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * @brief Sets the \ref RobotUnit's state to \param s
+ * @param s The state to set
+ */
+ void setRobotUnitState(RobotUnitState s);
+ protected:
+ //state transitions
+ /**
+ * @brief Transition \ref RobotUnitState::InitializingComponent -> \ref RobotUnitState::InitializingDevices
+ * \note This function is protected with \ref getGuard()
+ * @see _preFinishComponentInitialization
+ * @see _postFinishComponentInitialization
+ */
+ virtual void finishComponentInitialization();
+ /**
+ * @brief Transition \ref RobotUnitState::InitializingDevices -> \ref RobotUnitState::InitializingUnits
+ * \note This function is protected with \ref getGuard()
+ * @see _preFinishDeviceInitialization
+ * @see _postFinishDeviceInitialization
+ */
+ virtual void finishDeviceInitialization();
+ /**
+ * @brief Transition \ref RobotUnitState::InitializingUnits -> \ref RobotUnitState::WaitingForRTThreadInitialization
+ * \note This function is protected with \ref getGuard()
+ * @see _preFinishUnitInitialization
+ * @see _postFinishUnitInitialization
+ */
+ virtual void finishUnitInitialization();
+ /**
+ * @brief Transition \ref RobotUnitState::InitializingControlThread -> \ref RobotUnitState::Running
+ * \note This function is protected with \ref getGuard()
+ * @see _preFinishControlThreadInitialization
+ * @see _postFinishControlThreadInitialization
+ */
+ virtual void finishControlThreadInitialization();
+ /**
+ * @brief Transition \ref RobotUnitState::Running -> \ref RobotUnitState::Exiting
+ * \note This function is protected with \ref getGuard()
+ * @see _preFinishRunning
+ * @see _postFinishRunning
+ */
+ virtual void finishRunning();
+
+ //state transition module hooks
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishComponentInitialization (called before the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishComponentInitialization
+ */
+ void _preFinishComponentInitialization() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishComponentInitialization (called after the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishComponentInitialization
+ */
+ void _postFinishComponentInitialization() {}
+
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called before the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishDeviceInitialization
+ */
+ void _preFinishDeviceInitialization() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called after the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishDeviceInitialization
+ */
+ void _postFinishDeviceInitialization() {}
+
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called before the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishUnitInitialization
+ */
+ void _preFinishUnitInitialization() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called after the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishUnitInitialization
+ */
+ void _postFinishUnitInitialization() {}
+
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called before the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishControlThreadInitialization
+ */
+ void _preFinishControlThreadInitialization() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called after the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishControlThreadInitialization
+ */
+ void _postFinishControlThreadInitialization() {}
+
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called before the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishRunning
+ */
+ void _preFinishRunning() {}
+ /**
+ * @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called after the state has changed)
+ * \note This function is protected with \ref getGuard()
+ * @see finishRunning
+ */
+ void _postFinishRunning() {}
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// utility //////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns the \ref RobotUnit's State
+ * @return The \ref RobotUnit's State
+ */
+ RobotUnitState getRobotUnitState() const;
+
+ /**
+ * @brief Throws an exception if the current state is not in \param stateSet
+ * @param stateSet The set of acceptable \ref RobotUnitState "RobotUnitStates"
+ * @param fnc The callers function name.
+ * @param onlyWarn Only print a warning instead of throwing an exception.
+ */
+ void throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn = false) const;
+ /**
+ * @brief Throws an exception if the current state is not \param state
+ * @param state The acceptable \ref RobotUnitState
+ * @param fnc The callers function name.
+ * @param onlyWarn Only print a warning instead of throwing an exception.
+ */
+ void throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
+ /**
+ * @brief Throws an exception if the current state is in \param stateSet
+ * @param stateSet The set of forbidden \ref RobotUnitState "RobotUnitStates"
+ * @param fnc The callers function name.
+ * @param onlyWarn Only print a warning instead of throwing an exception.
+ */
+ void throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn = false) const;
+ /**
+ * @brief Throws an exception if the current state is \param state
+ * @param state The forbidden \ref RobotUnitState
+ * @param fnc The callers function name.
+ * @param onlyWarn Only print a warning instead of throwing an exception.
+ */
+ void throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
+ /**
+ * @brief Throws if the \ref Device "Devices" are not ready.
+ * @param fnc The callers function name.
+ * @see areDevicesReady
+ */
+ void throwIfDevicesNotReady(const std::string& fnc) const;
+
+ /**
+ * @brief Returns whether \ref Device "Devices" are ready
+ * @return Whether \ref Device "Devices" are ready
+ * @see \ref DevicesReadyStates
+ */
+ bool areDevicesReady() const;
+
+ /**
+ * @brief Returns whether the current thread is the \ref ControlThread.
+ * @return Whether the current thread is the \ref ControlThread.
+ */
+ bool inControlThread() const;
+
+ /**
+ * @brief Throws if the current thread is the \ref ControlThread.
+ * @param fnc The callers function name.
+ */
+ void throwIfInControlThread(const std::string& fnc) const;
+
+ /**
+ * @brief Returns whether the \ref RobotUnit is shutting down
+ * @return Whether the \ref RobotUnit is shutting down
+ * @see shutDown
+ */
+ bool isShuttingDown() const ///TODO use this function in implementations
+ {
+ return isShuttingDown_.load();
+ }
+ /**
+ * @brief Requests the \ref RobotUnit to shut down.
+ * @see isShuttingDown
+ */
+ void shutDown() ///TODO use this function in implementations
+ {
+ isShuttingDown_.store(true);
+ }
+ // //////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////// robot unit impl hooks ////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////// //
+ /**
+ * @brief called in onInitComponent
+ * \note This function is protected with \ref getGuard()
+ * @see onInitComponent
+ */
+ virtual void onInitRobotUnit() {}
+ /**
+ * @brief called in onConnectComponent
+ * \warning This function is not protected with \ref getGuard()
+ * @see onConnectComponent
+ */
+ virtual void onConnectRobotUnit() {}
+ /**
+ * @brief called in onDisconnecComponent
+ * \warning This function is not protected with \ref getGuard()
+ * @see onDisconnecComponent
+ */
+ virtual void onDisconnectRobotUnit() {}
+ /**
+ * @brief called in onExitComponent before calling finishRunning
+ * \note This function is protected with \ref getGuard()
+ * @see finishRunning
+ * @see onExitComponent
+ */
+ virtual void onExitRobotUnit() {}
+
+ /// @brief Implementations have to join their \ref ControlThread in this hook. (used by RobotUnit::finishRunning())
+ virtual void joinControlThread() = 0;
+
+ /// @brief The \ref ControlThread's period
+ virtual IceUtil::Time getControlThreadTargetPeriod() const = 0;
+ // //////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////////// other /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////// //
+ protected:
+ /**
+ * @brief Returns a sentinel value for an index (std::numeric_limits<std::size_t>::max())
+ * @return A sentinel value for an index (std::numeric_limits<std::size_t>::max())
+ */
+ static constexpr std::size_t IndexSentinel()
+ {
+ return std::numeric_limits<std::size_t>::max();
+ }
+ /**
+ * @brief Returns a guard to the \ref RobotUnits mutex.
+ * @return A guard to the \ref RobotUnits mutex.
+ * @throw throws If called in the \ref ControlThread or on shutdown.
+ */
+ GuardType getGuard() const;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief The \ref RobotUnits mutex.
+ mutable MutexType dataMutex;
+ /// @brief Set of \ref RobotUnitState "RobotUnitStates" where \ref Device "Devices" are usable
+ static const std::set<RobotUnitState> DevicesReadyStates;
+ /// @brief The \ref RobotUnit's current state
+ std::atomic<RobotUnitState> state {RobotUnitState::InitializingComponent};
+ /// @brief Whether the \ref RobotUnit is shutting down
+ std::atomic_bool isShuttingDown_ {false};
+ };
}
+
#include "RobotUnitModuleBase.ipp"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.ipp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.ipp
index 4980c7fb7fa6b7c764c862b3a3c93cede7e4e9fc..9c09a51a11ca4409c9eadf29e1be9634de6dfb0c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.ipp
@@ -24,13 +24,10 @@
#include "RobotUnitModuleBase.h"
-namespace armarx
-{
- namespace RobotUnitModule
+namespace armarx::RobotUnitModule
{
inline RobotUnitState ModuleBase::getRobotUnitState() const
{
return state;
}
}
-}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 97b3b4275ddd97e35370f065068f7fbba35dff11..3cc35d38bb8c3146688ceb2b2be0b4334bc362f3 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -33,821 +33,981 @@
#include "../Devices/RTThreadTimingsSensorDevice.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ /**
+ * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref ControlThread.
+ * \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 NJointControllerAttorneyForControlThread
{
- /**
- * \brief This class allows minimal access to private members of \ref NJointController in a sane fashion for \ref ControlThread.
- * \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 NJointControllerAttorneyForControlThread
+ friend class ControlThread;
+ static void RtDeactivateController(const NJointControllerBasePtr& nJointCtrl)
{
- friend class ControlThread;
- static void RtDeactivateController(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtDeactivateController();
- }
- static void RtActivateController(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtActivateController();
- }
- static void RtDeactivateControllerBecauseOfError(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtDeactivateControllerBecauseOfError();
- }
- };
- /**
- * \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer in a sane fashion for \ref ControlThread.
- * \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 ControlThreadDataBufferAttorneyForControlThread
- {
- friend class ControlThread;
- static std::vector<JointController*>& GetActivatedJointControllers(ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
- }
- static std::vector<NJointController*>& GetActivatedNJointControllers(ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
- }
- static std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
- }
- static const std::vector<JointController*>& GetActivatedJointControllers(const ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
- }
- static const std::vector<NJointController*>& GetActivatedNJointControllers(const ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
- }
- static const std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
- }
+ nJointCtrl->rtDeactivateController();
+ }
+ static void RtActivateController(const NJointControllerBasePtr& nJointCtrl)
+ {
+ nJointCtrl->rtActivateController();
+ }
+ static void RtDeactivateControllerBecauseOfError(const NJointControllerBasePtr& nJointCtrl)
+ {
+ nJointCtrl->rtDeactivateControllerBecauseOfError();
+ }
+ };
+ /**
+ * \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer in a sane fashion for \ref ControlThread.
+ * \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 ControlThreadDataBufferAttorneyForControlThread
+ {
+ friend class ControlThread;
+ static std::vector<JointController*>& GetActivatedJointControllers(ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
+ }
+ static std::vector<NJointControllerBase*>& GetActivatedNJointControllers(ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
+ }
+ static std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
+ }
+ static const std::vector<JointController*>& GetActivatedJointControllers(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
+ }
+ static const std::vector<NJointControllerBase*>& GetActivatedNJointControllers(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
+ }
+ static const std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
+ }
- static void AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
- {
- p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement =
- p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
- }
- static void CommitActivatedControllers(ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersActivated.commitWrite();
- }
- static void ResetActivatedControllerAssignement(ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().resetAssignement();
- }
+ static void AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
+ {
+ p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement =
+ p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
+ }
+ static void CommitActivatedControllers(ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.commitWrite();
+ }
+ static void ResetActivatedControllerAssignement(ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().resetAssignement();
+ }
- static const std::vector<JointController*>& GetRequestedJointControllers(const ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointControllers;
- }
- static const std::vector<NJointController*>& GetRequestedNJointControllers(const ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().nJointControllers;
- }
- static const std::vector<std::size_t>& GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
- }
+ static const std::vector<JointController*>& GetRequestedJointControllers(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointControllers;
+ }
+ static const std::vector<NJointControllerBase*>& GetRequestedNJointControllers(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().nJointControllers;
+ }
+ static const std::vector<std::size_t>& GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
+ }
- static bool RequestedControllersChanged(const ControlThread* p)
- {
- return p->_module<ControlThreadDataBuffer>().controllersRequested.updateReadBuffer();
- }
- };
- /**
- * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
- * \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 DevicesAttorneyForControlThread
- {
- friend class ControlThread;
- static const std::vector<ControlDevicePtr>& GetControlDevices(const ControlThread* p)
- {
- return p->_module<Devices>().controlDevices.values();
- }
- static const std::vector<SensorDevicePtr>& GetSensorDevices(const ControlThread* p)
- {
- return p->_module<Devices>().sensorDevices.values();
- }
+ static bool RequestedControllersChanged(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersRequested.updateReadBuffer();
+ }
+ };
+ /**
+ * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
+ * \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 DevicesAttorneyForControlThread
+ {
+ friend class ControlThread;
+ static const std::vector<ControlDevicePtr>& GetControlDevices(const ControlThread* p)
+ {
+ return p->_module<Devices>().controlDevices.values();
+ }
+ static const std::vector<SensorDevicePtr>& GetSensorDevices(const ControlThread* p)
+ {
+ return p->_module<Devices>().sensorDevices.values();
+ }
- static const std::vector<const SensorValueBase*>& GetSensorValues(const ControlThread* p)
- {
- return p->_module<Devices>().sensorValues;
- }
- static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>& GetControlTargets(const ControlThread* p)
- {
- return p->_module<Devices>().controlTargets;
- }
- static RTThreadTimingsSensorDevice& GetThreadTimingsSensorDevice(const ControlThread* p)
- {
- return *(p->_module<Devices>().rtThreadTimingsSensorDevice);
- }
+ static const std::vector<const SensorValueBase*>& GetSensorValues(const ControlThread* p)
+ {
+ return p->_module<Devices>().sensorValues;
+ }
+ static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>& GetControlTargets(const ControlThread* p)
+ {
+ return p->_module<Devices>().controlTargets;
+ }
+ static RTThreadTimingsSensorDevice& GetThreadTimingsSensorDevice(const ControlThread* p)
+ {
+ return *(p->_module<Devices>().rtThreadTimingsSensorDevice);
+ }
- static void UpdateRobotWithSensorValues(const ControlThread* p, const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& robotNodes)
- {
- p->_module<Devices>().updateVirtualRobotFromSensorValues(robot, robotNodes, p->_module<Devices>().sensorValues);
- }
- };
- /**
- * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
- * \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 ManagementAttorneyForControlThread
- {
- friend class ControlThread;
- static bool HeartbeatMissing(const ControlThread* p)
+ static void UpdateRobotWithSensorValues(const ControlThread* p, const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& robotNodes)
+ {
+ p->_module<Devices>().updateVirtualRobotFromSensorValues(robot, robotNodes, p->_module<Devices>().sensorValues);
+ }
+ };
+ /**
+ * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
+ * \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 ManagementAttorneyForControlThread
+ {
+ friend class ControlThread;
+ static bool HeartbeatMissing(const ControlThread* p)
+ {
+ const Management& m = p->_module<Management>();
+ long now = TimeUtil::GetTime(true).toMilliSeconds();
+ if (!m.heartbeatRequired || now < m.controlLoopStartTime + m.heartbeatStartupMarginMS)
{
- const Management& m = p->_module<Management>();
- long now = TimeUtil::GetTime(true).toMilliSeconds();
- if (!m.heartbeatRequired || now < m.controlLoopStartTime + m.heartbeatStartupMarginMS)
- {
- return false;
- }
- return (now - m.lastHeartbeat) > m.heartbeatMaxCycleMS;
+ return false;
}
- };
- }
+ return (now - m.lastHeartbeat) > m.heartbeatMaxCycleMS;
+ }
+ };
}
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ bool ControlThread::rtSwitchControllerSetup()
{
- bool ControlThread::rtSwitchControllerSetup()
- {
- rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupStart();
- ARMARX_ON_SCOPE_EXIT { rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd(); };
+ rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupStart();
+ ARMARX_ON_SCOPE_EXIT { rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd(); };
+ {
+ const auto& actJC = rtGetActivatedJointControllers();
+ const auto& assig = rtGetActivatedJointToNJointControllerAssignement();
+ const auto& actNJC = rtGetActivatedNJointControllers();
+ for (std::size_t i = 0; i < actJC.size(); ++i)
{
- const auto& actJC = rtGetActivatedJointControllers();
- const auto& assig = rtGetActivatedJointToNJointControllerAssignement();
- const auto& actNJC = rtGetActivatedNJointControllers();
- for (std::size_t i = 0; i < actJC.size(); ++i)
- {
- preSwitchSetup_ActivatedJointControllers.at(i) = actJC.at(i);
- preSwitchSetup_ActivatedJointToNJointControllerAssignement.at(i) = assig.at(i);
- preSwitchSetup_ActivatedNJointControllers.at(i) = actNJC.at(i);
- }
+ preSwitchSetup_ActivatedJointControllers.at(i) = actJC.at(i);
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement.at(i) = assig.at(i);
+ preSwitchSetup_ActivatedNJointControllers.at(i) = actNJC.at(i);
}
+ }
- ARMARX_ON_SCOPE_EXIT
- {
- const auto& actJC = rtGetActivatedJointControllers();
- const auto& assig = rtGetActivatedJointToNJointControllerAssignement();
- const auto& actNJC = rtGetActivatedNJointControllers();
- for (std::size_t i = 0; i < actJC.size(); ++i)
- {
- postSwitchSetup_ActivatedJointControllers.at(i) = actJC.at(i);
- postSwitchSetup_ActivatedJointToNJointControllerAssignement.at(i) = assig.at(i);
- postSwitchSetup_ActivatedNJointControllers.at(i) = actNJC.at(i);
- }
- };
-
+ ARMARX_ON_SCOPE_EXIT
+ {
+ const auto& actJC = rtGetActivatedJointControllers();
+ const auto& assig = rtGetActivatedJointToNJointControllerAssignement();
+ const auto& actNJC = rtGetActivatedNJointControllers();
- rtSwitchControllerSetupChangedControllers = false;
+ std::size_t numSyncNj = 0;
+ std::size_t numAsyncNj = 0;
- // a missing heartbeat (if required by the config) is interpreted as emergencyStop == true
- if (!emergencyStop && ManagementAttorneyForControlThread::HeartbeatMissing(this))
+ for (std::size_t i = 0; i < actJC.size(); ++i)
{
- rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!").deactivateSpam(1);
- }
+ postSwitchSetup_ActivatedJointControllers.at(i) = actJC.at(i);
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement.at(i) = assig.at(i);
+ postSwitchSetup_ActivatedNJointControllers.at(i) = actNJC.at(i);
- // !emergencyStop && !rtIsInEmergencyStop() -> normal control flow
- // !emergencyStop && rtIsInEmergencyStop() -> force switch to reactivate old ( + reset flag)
- // emergencyStop && !rtIsInEmergencyStop() -> deactivate all + set flag
- // emergencyStop && rtIsInEmergencyStop() -> nothing to do
- if (emergencyStop)
- {
- if (rtIsInEmergencyStop())
+ if (dynamic_cast<SynchronousNJointController*>(actNJC.at(i)))
{
- return false;
+ _activatedSynchronousNJointControllersIdx.at(numSyncNj++) = i;
}
- rtIsInEmergencyStop_ = true;
- //deactivate all nJointCtrl
- for (auto& nJointCtrl : rtGetActivatedNJointControllers())
+ else if (dynamic_cast<AsynchronousNJointController*>(actNJC.at(i)))
{
- if (nJointCtrl)
- {
- NJointControllerAttorneyForControlThread::RtDeactivateController(nJointCtrl);
- nJointCtrl = nullptr;
- rtSwitchControllerSetupChangedControllers = true;
- }
- }
- //set all JointCtrl to emergency stop (except stop movement)
- for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
- {
- const ControlDevicePtr& controlDevice = rtGetControlDevices().at(i);
- const auto active = controlDevice->rtGetActiveJointController();
- const auto stopMov = controlDevice->rtGetJointStopMovementController();
- const auto emergency = controlDevice->rtGetJointEmergencyStopController();
- if (active != stopMov && active != emergency)
- {
- controlDevice->rtSetActiveJointController(emergency);
- rtGetActivatedJointControllers().at(i) = emergency;
- rtSwitchControllerSetupChangedControllers = true;
- }
+ _activatedAsynchronousNJointControllersIdx.at(numAsyncNj++) = i;
}
- if (rtSwitchControllerSetupChangedControllers)
+ else
{
- ControlThreadDataBufferAttorneyForControlThread::ResetActivatedControllerAssignement(this);
- // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
+ throw std::logic_error{"NJoint controller that is neither SynchronousNJointController"
+ " nor AsynchronousNJointController"};
}
- return rtSwitchControllerSetupChangedControllers;
}
-
- if (
- !ControlThreadDataBufferAttorneyForControlThread::RequestedControllersChanged(this) &&
- !rtIsInEmergencyStop()
+ for (
+ std::size_t i = numSyncNj;
+ i < _maxControllerCount &&
+ _activatedSynchronousNJointControllersIdx.at(i) != _maxControllerCount;
+ ++i
)
{
- return false;
+ _activatedSynchronousNJointControllersIdx.at(i) = _maxControllerCount;
+ }
+ for (
+ std::size_t i = numAsyncNj;
+ i < _maxControllerCount &&
+ _activatedAsynchronousNJointControllersIdx.at(i) != _maxControllerCount;
+ ++i
+ )
+ {
+ _activatedAsynchronousNJointControllersIdx.at(i) = _maxControllerCount;
}
- rtIsInEmergencyStop_ = false;
+ };
- //handle nJointCtrl
+
+ rtSwitchControllerSetupChangedControllers = false;
+
+ // a missing heartbeat (if required by the config) is interpreted as emergencyStop == true
+ if (!emergencyStop && ManagementAttorneyForControlThread::HeartbeatMissing(this))
+ {
+ rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!").deactivateSpam(1);
+ }
+
+ // !emergencyStop && !rtIsInEmergencyStop() -> normal control flow
+ // !emergencyStop && rtIsInEmergencyStop() -> force switch to reactivate old ( + reset flag)
+ // emergencyStop && !rtIsInEmergencyStop() -> deactivate all + set flag
+ // emergencyStop && rtIsInEmergencyStop() -> nothing to do
+ if (emergencyStop)
+ {
+ if (rtIsInEmergencyStop())
{
- //"merge"
- std::size_t n = rtGetControlDevices().size();
- std::size_t idxAct = 0;
- std::size_t idxReq = 0;
- for (std::size_t i = 0; i < 2 * n; ++i)
+ return false;
+ }
+ rtIsInEmergencyStop_ = true;
+ //deactivate all nJointCtrl
+ for (auto& nJointCtrl : rtGetActivatedNJointControllers())
+ {
+ if (nJointCtrl)
{
- //skip nullptrs in act
- while (idxAct < n && !rtGetActivatedNJointControllers().at(idxAct))
- {
- ++idxAct;
- }
- const NJointControllerPtr& req = idxReq < n ? rtGetRequestedNJointControllers().at(idxReq) : nullptr; //may be null
- const NJointControllerPtr& act = idxAct < n ? rtGetActivatedNJointControllers().at(idxAct) : nullptr; //may be null if it is the last
- const auto reqId = reinterpret_cast<std::uintptr_t>(req.get());
- const auto actId = reinterpret_cast<std::uintptr_t>(act.get());
-
- if (reqId > actId)
- {
- //new ctrl
- rtSyncNJointControllerRobot(req.get());
- NJointControllerAttorneyForControlThread::RtActivateController(req);
- ++idxReq;
- rtSwitchControllerSetupChangedControllers = true;
- }
- else if (reqId < actId)
- {
- NJointControllerAttorneyForControlThread::RtDeactivateController(act);
- rtSwitchControllerSetupChangedControllers = true;
- ++idxAct;
- }
- else //if(reqId == actId)
- {
- //same ctrl or both null ctrl
- ++idxReq;
- ++idxAct;
- }
- if (idxAct >= n && !req)
- {
- break;
- }
+ NJointControllerAttorneyForControlThread::RtDeactivateController(nJointCtrl);
+ nJointCtrl = nullptr;
+ rtSwitchControllerSetupChangedControllers = true;
}
- rtGetActivatedNJointControllers() = rtGetRequestedNJointControllers();
}
-
- //handle Joint Ctrl
+ //set all JointCtrl to emergency stop (except stop movement)
+ for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
{
- ARMARX_CHECK_EQUAL(rtGetRequestedJointControllers().size(), rtGetControlDevices().size());
- for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
+ const ControlDevicePtr& controlDevice = rtGetControlDevices().at(i);
+ const auto active = controlDevice->rtGetActiveJointController();
+ const auto stopMov = controlDevice->rtGetJointStopMovementController();
+ const auto emergency = controlDevice->rtGetJointEmergencyStopController();
+ if (active != stopMov && active != emergency)
{
- auto& controlDevice = rtGetControlDevices().at(i);
- const auto requestedJointCtrl = rtGetRequestedJointControllers().at(i);
- controlDevice->rtSetActiveJointController(requestedJointCtrl);
- rtGetActivatedJointControllers().at(i) = requestedJointCtrl;
+ controlDevice->rtSetActiveJointController(emergency);
+ rtGetActivatedJointControllers().at(i) = emergency;
+ rtSwitchControllerSetupChangedControllers = true;
}
}
- ControlThreadDataBufferAttorneyForControlThread::AcceptRequestedJointToNJointControllerAssignement(this);
- // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
- return true;
+ if (rtSwitchControllerSetupChangedControllers)
+ {
+ ControlThreadDataBufferAttorneyForControlThread::ResetActivatedControllerAssignement(this);
+ // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
+ }
+ return rtSwitchControllerSetupChangedControllers;
}
- void ControlThread::rtResetAllTargets()
+ if (
+ !ControlThreadDataBufferAttorneyForControlThread::RequestedControllersChanged(this) &&
+ !rtIsInEmergencyStop()
+ )
{
- rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsStart();
- for (const ControlDevicePtr& controlDev : rtGetControlDevices())
- {
- controlDev->rtGetActiveJointController()->rtResetTarget();
- }
- rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsEnd();
+ return false;
}
+ rtIsInEmergencyStop_ = false;
- void ControlThread::rtHandleInvalidTargets()
+ //handle nJointCtrl
{
- rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart();
- numberOfInvalidTargetsInThisIteration = 0;
- const auto& cdevs = rtGetControlDevices();
- for (std::size_t i = 0; i < cdevs.size(); ++i)
+ //"merge"
+ std::size_t n = rtGetControlDevices().size();
+ std::size_t idxAct = 0;
+ std::size_t idxReq = 0;
+ for (std::size_t i = 0; i < 2 * n; ++i)
{
- if (!rtGetActivatedJointControllers().at(i)->rtIsTargetValid())
+ //skip nullptrs in act
+ while (idxAct < n && !rtGetActivatedNJointControllers().at(idxAct))
{
- ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'", i, cdevs.at(i)->rtGetDeviceName().c_str());
- ARMARX_ERROR << ">>>INVALID TARGET for JointController (idx = " << i << ") '" << cdevs.at(i)->rtGetDeviceName().c_str() << "'";
- rtDeactivateAssignedNJointControllerBecauseOfError(i);
- ++numberOfInvalidTargetsInThisIteration;
+ ++idxAct;
+ }
+ const NJointControllerBasePtr& req = idxReq < n ? rtGetRequestedNJointControllers().at(idxReq) : nullptr; //may be null
+ const NJointControllerBasePtr& act = idxAct < n ? rtGetActivatedNJointControllers().at(idxAct) : nullptr; //may be null if it is the last
+ const auto reqId = reinterpret_cast<std::uintptr_t>(req.get());
+ const auto actId = reinterpret_cast<std::uintptr_t>(act.get());
+
+ if (reqId > actId)
+ {
+ //new ctrl
+ rtSyncNJointControllerRobot(req.get());
+ NJointControllerAttorneyForControlThread::RtActivateController(req);
+ ++idxReq;
+ rtSwitchControllerSetupChangedControllers = true;
+ }
+ else if (reqId < actId)
+ {
+ NJointControllerAttorneyForControlThread::RtDeactivateController(act);
+ rtSwitchControllerSetupChangedControllers = true;
+ ++idxAct;
+ }
+ else //if(reqId == actId)
+ {
+ //same ctrl or both null ctrl
+ ++idxReq;
+ ++idxAct;
+ }
+ if (idxAct >= n && !req)
+ {
+ break;
}
}
- // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
- rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsEnd();
+ rtGetActivatedNJointControllers() = rtGetRequestedNJointControllers();
}
- void ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ //handle Joint Ctrl
{
- rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart();
- for (const SensorDevicePtr& device : rtGetSensorDevices())
+ ARMARX_CHECK_EQUAL(rtGetRequestedJointControllers().size(), rtGetControlDevices().size());
+ for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
{
- device->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
+ auto& controlDevice = rtGetControlDevices().at(i);
+ const auto requestedJointCtrl = rtGetRequestedJointControllers().at(i);
+ controlDevice->rtSetActiveJointController(requestedJointCtrl);
+ rtGetActivatedJointControllers().at(i) = requestedJointCtrl;
}
- DevicesAttorneyForControlThread::UpdateRobotWithSensorValues(this, rtRobot, rtRobotNodes);
- rtPostReadSensorDeviceValues(sensorValuesTimestamp, timeSinceLastIteration);
- rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesEnd();
}
+ ControlThreadDataBufferAttorneyForControlThread::AcceptRequestedJointToNJointControllerAssignement(this);
+ // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
+ return true;
+ }
- void ControlThread::rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void ControlThread::rtResetAllTargets()
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsStart();
+ for (const ControlDevicePtr& controlDev : rtGetControlDevices())
{
- if (dynamicsHelpers)
- {
- // auto start = IceUtil::Time::now();
- dynamicsHelpers->update(sensorValuesTimestamp, timeSinceLastIteration);
- // auto end = IceUtil::Time::now();
- // ARMARX_INFO << "Dynamics duration: " << (end-start).toMicroSeconds();
- }
+ controlDev->rtGetActiveJointController()->rtResetTarget();
}
+ rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsEnd();
+ }
- void ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void ControlThread::rtHandleInvalidTargets()
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart();
+ numberOfInvalidTargetsInThisIteration = 0;
+ const auto& cdevs = rtGetControlDevices();
+ for (std::size_t i = 0; i < cdevs.size(); ++i)
{
- rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersStart();
- for (const ControlDevicePtr& device : rtGetControlDevices())
+ if (!rtGetActivatedJointControllers().at(i)->rtIsTargetValid())
{
- device->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'", i, cdevs.at(i)->rtGetDeviceName().c_str());
+ ARMARX_ERROR << ">>>INVALID TARGET for JointController (idx = " << i << ") '" << cdevs.at(i)->rtGetDeviceName().c_str() << "'";
+ rtDeactivateAssignedNJointControllerBecauseOfError(i);
+ ++numberOfInvalidTargetsInThisIteration;
}
- rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersEnd();
}
+ // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
+ rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsEnd();
+ }
+
+ void ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart();
+ for (const SensorDevicePtr& device : rtGetSensorDevices())
+ {
+ device->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ DevicesAttorneyForControlThread::UpdateRobotWithSensorValues(this, rtRobot, rtRobotNodes);
+ rtPostReadSensorDeviceValues(sensorValuesTimestamp, timeSinceLastIteration);
+ rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesEnd();
+ }
- void ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void ControlThread::rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ if (dynamicsHelpers)
{
- rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
- // bool activeControllersChanged = false;
- for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < rtGetActivatedNJointControllers().size(); ++nJointCtrlIndex)
+ // auto start = IceUtil::Time::now();
+ dynamicsHelpers->update(sensorValuesTimestamp, timeSinceLastIteration);
+ // auto end = IceUtil::Time::now();
+ // ARMARX_INFO << "Dynamics duration: " << (end-start).toMicroSeconds();
+ }
+ }
+
+ void ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersStart();
+ for (const ControlDevicePtr& device : rtGetControlDevices())
+ {
+ device->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersEnd();
+ }
+
+ void ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
+ // bool activeControllersChanged = false;
+ auto activatedNjointCtrls = rtGetActivatedNJointControllers();
+ //start async
+ for (std::size_t nJointCtrlIndex : _activatedAsynchronousNJointControllersIdx)
+ {
+ if (nJointCtrlIndex == _maxControllerCount)
+ {
+ break;
+ }
+ auto nJointCtrl = static_cast<AsynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ if (!nJointCtrl)
+ {
+ continue;
+ }
+ try
{
- NJointController* nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
- try
+ if (nJointCtrl->rtGetErrorState())
{
- if (nJointCtrl)
- {
- if (nJointCtrl->rtGetErrorState())
- {
- ARMARX_RT_LOGF_ERROR(
- "NJointController '%s' requested deactivation while activating it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
- rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
- // activeControllersChanged = true;
- }
-
- auto start = TimeUtil::GetTime(true);
- rtSyncNJointControllerRobot(nJointCtrl);
- nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
- auto duration = TimeUtil::GetTime(true) - start;
- if (nJointCtrl->rtGetErrorState())
- {
- ARMARX_RT_LOGF_ERROR(
- "NJointController '%s' requested deactivation while running it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
- rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
- // activeControllersChanged = true;
- }
- if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
- {
- ARMARX_RT_LOGF_ERROR(
- "The NJointController '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
- }
- else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
- {
- ARMARX_RT_LOGF_WARNING(
- "The NJointController '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
- }
- }
+ ARMARX_RT_LOGF_ERROR(
+ "NJointControllerBase '%s' requested deactivation while activating it",
+ nJointCtrl->rtGetInstanceName().c_str()
+ );
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
- catch (...)
+ auto start = TimeUtil::GetTime(true);
+ rtSyncNJointControllerRobot(nJointCtrl);
+ nJointCtrl->rtRunIterationBegin(sensorValuesTimestamp, timeSinceLastIteration);
+ auto duration = TimeUtil::GetTime(true) - start;
+ if (nJointCtrl->rtGetErrorState())
{
- ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
- << " threw an exception and is now deactivated: "
- << GetHandledExceptionString();
+ ARMARX_RT_LOGF_ERROR(
+ "NJointControllerBase '%s' requested deactivation while running it",
+ nJointCtrl->rtGetInstanceName().c_str()
+ );
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
- // activeControllersChanged = true;
+ }
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ {
+ ARMARX_RT_LOGF_ERROR(
+ "The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds()
+ ).deactivateSpam(5);
+ }
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
+ {
+ ARMARX_RT_LOGF_WARNING(
+ "The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds()
+ ).deactivateSpam(5);
}
}
- // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
- rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
+ catch (...)
+ {
+ ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
+ << " threw an exception and is now deactivated: "
+ << GetHandledExceptionString();
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ }
}
- void ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex)
+ //start sync
+ for (std::size_t nJointCtrlIndex : _activatedSynchronousNJointControllersIdx)
{
- const NJointControllerPtr& nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
- NJointControllerAttorneyForControlThread::RtDeactivateControllerBecauseOfError(nJointCtrl);
- for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
+ if (nJointCtrlIndex == _maxControllerCount)
{
- const ControlDevicePtr& controlDevice = rtGetControlDevices().at(ctrlDevIdx);
- JointController* es = controlDevice->rtGetJointEmergencyStopController();
-
- ARMARX_CHECK_EQUAL_W_HINT(
- rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx),
- nJointCtrlIndex,
- VAROUT(ctrlDevIdx) << "\n"
- << VAROUT(controlDevice->getDeviceName()) << "\n"
- << dumpRtState()
- );
-
- controlDevice->rtSetActiveJointController(es);
- rtGetActivatedJointControllers().at(ctrlDevIdx) = es;
- rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) = IndexSentinel();
+ break;
}
- rtGetActivatedNJointControllers().at(nJointCtrlIndex) = nullptr;
- //check ControlDeviceHardwareControlModeGroups
+ auto nJointCtrl = static_cast<SynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ try
{
- for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
+ if (nJointCtrl)
{
- const auto& ctrlModeGroups = _module<Devices>().getControlModeGroups();
- const auto groupIdx = ctrlModeGroups.groupIndices.at(ctrlDevIdx);
- if (groupIdx == IndexSentinel())
+ if (nJointCtrl->rtGetErrorState())
{
- continue;
+ ARMARX_RT_LOGF_ERROR(
+ "NJointControllerBase '%s' requested deactivation while activating it",
+ nJointCtrl->rtGetInstanceName().c_str()
+ );
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ // activeControllersChanged = true;
+ }
+
+ auto start = TimeUtil::GetTime(true);
+ rtSyncNJointControllerRobot(nJointCtrl);
+ nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
+ auto duration = TimeUtil::GetTime(true) - start;
+ if (nJointCtrl->rtGetErrorState())
+ {
+ ARMARX_RT_LOGF_ERROR(
+ "NJointControllerBase '%s' requested deactivation while running it",
+ nJointCtrl->rtGetInstanceName().c_str()
+ );
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ // activeControllersChanged = true;
+ }
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ {
+ ARMARX_RT_LOGF_ERROR(
+ "The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds()
+ ).deactivateSpam(5);
}
- ControlDevice* const dev = rtGetControlDevices().at(ctrlDevIdx).get();
- JointController* const jointCtrl = dev->rtGetActiveJointController();
- const auto hwModeHash = jointCtrl->rtGetHardwareControlModeHash();
- //this device is in a group!
- // -> check all other devices
- for (const auto otherIdx : ctrlModeGroups.deviceIndices.at(groupIdx))
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
{
- ControlDevice* const otherDev = rtGetControlDevices().at(otherIdx).get();
- JointController* const otherJointCtrl = otherDev->rtGetActiveJointController();
- const auto otherHwModeHash = otherJointCtrl->rtGetHardwareControlModeHash();
- if (hwModeHash == otherHwModeHash)
- {
- //the assigend ctrl has the same hwMode -> don't do anything
- continue;
- }
- const auto otherNJointCtrl1Idx = rtGetActivatedJointToNJointControllerAssignement().at(otherIdx);
- if (otherNJointCtrl1Idx == IndexSentinel())
- {
- //the hwmodes are different! (hence the other ctrl must be in stop movement
- ARMARX_CHECK_EXPRESSION(otherJointCtrl == otherDev->rtGetJointStopMovementController());
- //we need to activate the es contoller
- JointController* const es = otherDev->rtGetJointEmergencyStopController();
- otherDev->rtSetActiveJointController(es);
- ARMARX_CHECK_EXPRESSION(es->rtGetHardwareControlModeHash() == hwModeHash);
- rtGetActivatedJointControllers().at(otherIdx) = es;
- continue;
- }
- //the devs NJoint controller needs to be deactivated
- rtDeactivateAssignedNJointControllerBecauseOfError(otherIdx);
+ ARMARX_RT_LOGF_WARNING(
+ "The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds()
+ ).deactivateSpam(5);
}
}
}
- rtDeactivatedNJointControllerBecauseOfError(nJointCtrl);
- }
-
- void ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex)
- {
- std::size_t nJointCtrlIndex = rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
- ARMARX_CHECK_LESS_W_HINT(
- nJointCtrlIndex, rtGetControlDevices().size(),
- "no NJoint controller controls this device (name = "
- << rtGetControlDevices().at(ctrlDevIndex)->rtGetDeviceName()
- << ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
- << "This means an invariant is violated! Dumping data for debugging:\n"
- << VAROUT(ctrlDevIndex) << "\n"
- << dumpRtState()
- );
-
- rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ catch (...)
+ {
+ ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
+ << " threw an exception and is now deactivated: "
+ << GetHandledExceptionString();
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ // activeControllersChanged = true;
+ }
}
-
- void ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ //stop async
+ for (std::size_t nJointCtrlIndex : _activatedAsynchronousNJointControllersIdx)
{
- rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferStart();
- //commit all changes to activated controllers (joint, njoint, assignement)
+ if (nJointCtrlIndex == _maxControllerCount)
{
- ControlThreadDataBufferAttorneyForControlThread::CommitActivatedControllers(this);
+ break;
}
-
- SensorAndControl& sc = _module<ControlThreadDataBuffer>().rtGetSensorAndControlBuffer();
- sc.writeTimestamp = IceUtil::Time::now(); // this has to be in real time
- sc.sensorValuesTimestamp = sensorValuesTimestamp;
- sc.timeSinceLastIteration = timeSinceLastIteration;
- ARMARX_CHECK_EQUAL(rtGetSensorDevices().size(), sc.sensors.size());
- for (std::size_t sensIdx = 0; sensIdx < rtGetSensorDevices().size(); ++sensIdx)
+ auto nJointCtrl = static_cast<AsynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ if (!nJointCtrl)
{
- rtGetSensorDevices().at(sensIdx)->getSensorValue()->_copyTo(sc.sensors.at(sensIdx));
+ continue;
}
-
- ARMARX_CHECK_EQUAL(rtGetControlDevices().size(), sc.control.size());
- for (std::size_t ctrlIdx = 0; ctrlIdx < rtGetControlDevices().size(); ++ctrlIdx)
+ try
{
- ControlDevice& controlDevice = *rtGetControlDevices().at(ctrlIdx);
- ARMARX_CHECK_EQUAL(controlDevice.rtGetJointControllers().size(), sc.control.at(ctrlIdx).size());
- for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetJointControllers().size(); ++targIdx)
+ if (nJointCtrl->rtGetErrorState())
+ {
+ ARMARX_RT_LOGF_ERROR(
+ "NJointControllerBase '%s' requested deactivation while activating it",
+ nJointCtrl->rtGetInstanceName().c_str()
+ );
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ }
+ auto start = TimeUtil::GetTime(true);
+ rtSyncNJointControllerRobot(nJointCtrl);
+ nJointCtrl->rtRunIterationEnd(sensorValuesTimestamp, timeSinceLastIteration);
+ auto duration = TimeUtil::GetTime(true) - start;
+ if (nJointCtrl->rtGetErrorState())
{
- JointController& jointCtrl = *controlDevice.rtGetJointControllers().at(targIdx);
- jointCtrl.getControlTarget()->_copyTo(sc.control.at(ctrlIdx).at(targIdx));
+ ARMARX_RT_LOGF_ERROR(
+ "NJointControllerBase '%s' requested deactivation while running it",
+ nJointCtrl->rtGetInstanceName().c_str()
+ );
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ }
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ {
+ ARMARX_RT_LOGF_ERROR(
+ "The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds()
+ ).deactivateSpam(5);
+ }
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
+ {
+ ARMARX_RT_LOGF_WARNING(
+ "The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds()
+ ).deactivateSpam(5);
}
}
- _module<ControlThreadDataBuffer>().rtSensorAndControlBufferCommitWrite();
+ catch (...)
+ {
+ ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
+ << " threw an exception and is now deactivated: "
+ << GetHandledExceptionString();
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ }
+ }
+ // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
+ rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
+ }
+ void ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex)
+ {
+ const NJointControllerBasePtr& nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
+ NJointControllerAttorneyForControlThread::RtDeactivateControllerBecauseOfError(nJointCtrl);
+ for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
+ {
+ const ControlDevicePtr& controlDevice = rtGetControlDevices().at(ctrlDevIdx);
+ JointController* es = controlDevice->rtGetJointEmergencyStopController();
+
+ ARMARX_CHECK_EQUAL_W_HINT(
+ rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx),
+ nJointCtrlIndex,
+ VAROUT(ctrlDevIdx) << "\n"
+ << VAROUT(controlDevice->getDeviceName()) << "\n"
+ << dumpRtState()
+ );
- rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferEnd();
+ controlDevice->rtSetActiveJointController(es);
+ rtGetActivatedJointControllers().at(ctrlDevIdx) = es;
+ rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) = IndexSentinel();
}
-
- const std::vector<ControlDevicePtr>& ControlThread::rtGetControlDevices() const
+ rtGetActivatedNJointControllers().at(nJointCtrlIndex) = nullptr;
+ //check ControlDeviceHardwareControlModeGroups
{
- return DevicesAttorneyForControlThread::GetControlDevices(this);
+ for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
+ {
+ const auto& ctrlModeGroups = _module<Devices>().getControlModeGroups();
+ const auto groupIdx = ctrlModeGroups.groupIndices.at(ctrlDevIdx);
+ if (groupIdx == IndexSentinel())
+ {
+ continue;
+ }
+ ControlDevice* const dev = rtGetControlDevices().at(ctrlDevIdx).get();
+ JointController* const jointCtrl = dev->rtGetActiveJointController();
+ const auto hwModeHash = jointCtrl->rtGetHardwareControlModeHash();
+ //this device is in a group!
+ // -> check all other devices
+ for (const auto otherIdx : ctrlModeGroups.deviceIndices.at(groupIdx))
+ {
+ ControlDevice* const otherDev = rtGetControlDevices().at(otherIdx).get();
+ JointController* const otherJointCtrl = otherDev->rtGetActiveJointController();
+ const auto otherHwModeHash = otherJointCtrl->rtGetHardwareControlModeHash();
+ if (hwModeHash == otherHwModeHash)
+ {
+ //the assigend ctrl has the same hwMode -> don't do anything
+ continue;
+ }
+ const auto otherNJointCtrl1Idx = rtGetActivatedJointToNJointControllerAssignement().at(otherIdx);
+ if (otherNJointCtrl1Idx == IndexSentinel())
+ {
+ //the hwmodes are different! (hence the other ctrl must be in stop movement
+ ARMARX_CHECK_EXPRESSION(otherJointCtrl == otherDev->rtGetJointStopMovementController());
+ //we need to activate the es contoller
+ JointController* const es = otherDev->rtGetJointEmergencyStopController();
+ otherDev->rtSetActiveJointController(es);
+ ARMARX_CHECK_EXPRESSION(es->rtGetHardwareControlModeHash() == hwModeHash);
+ rtGetActivatedJointControllers().at(otherIdx) = es;
+ continue;
+ }
+ //the devs NJoint controller needs to be deactivated
+ rtDeactivateAssignedNJointControllerBecauseOfError(otherIdx);
+ }
+ }
}
+ rtDeactivatedNJointControllerBecauseOfError(nJointCtrl);
+ }
+
+ void ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex)
+ {
+ std::size_t nJointCtrlIndex = rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
+ ARMARX_CHECK_LESS_W_HINT(
+ nJointCtrlIndex, rtGetControlDevices().size(),
+ "no NJoint controller controls this device (name = "
+ << rtGetControlDevices().at(ctrlDevIndex)->rtGetDeviceName()
+ << ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
+ << "This means an invariant is violated! Dumping data for debugging:\n"
+ << VAROUT(ctrlDevIndex) << "\n"
+ << dumpRtState()
+ );
+
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
+ }
- const std::vector<SensorDevicePtr>& ControlThread::rtGetSensorDevices()
+ void ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferStart();
+ //commit all changes to activated controllers (joint, njoint, assignement)
{
- return DevicesAttorneyForControlThread::GetSensorDevices(this);
+ ControlThreadDataBufferAttorneyForControlThread::CommitActivatedControllers(this);
}
- RTThreadTimingsSensorDevice& ControlThread::rtGetThreadTimingsSensorDevice()
+ SensorAndControl& sc = _module<ControlThreadDataBuffer>().rtGetSensorAndControlBuffer();
+ sc.writeTimestamp = IceUtil::Time::now(); // this has to be in real time
+ sc.sensorValuesTimestamp = sensorValuesTimestamp;
+ sc.timeSinceLastIteration = timeSinceLastIteration;
+ ARMARX_CHECK_EQUAL(rtGetSensorDevices().size(), sc.sensors.size());
+ for (std::size_t sensIdx = 0; sensIdx < rtGetSensorDevices().size(); ++sensIdx)
{
- return DevicesAttorneyForControlThread::GetThreadTimingsSensorDevice(this);
+ rtGetSensorDevices().at(sensIdx)->getSensorValue()->_copyTo(sc.sensors.at(sensIdx));
}
- void ControlThread::rtSetEmergencyStopState(EmergencyStopState state)
+ ARMARX_CHECK_EQUAL(rtGetControlDevices().size(), sc.control.size());
+ for (std::size_t ctrlIdx = 0; ctrlIdx < rtGetControlDevices().size(); ++ctrlIdx)
{
- if (state == EmergencyStopState::eEmergencyStopActive)
+ ControlDevice& controlDevice = *rtGetControlDevices().at(ctrlIdx);
+ ARMARX_CHECK_EQUAL(controlDevice.rtGetJointControllers().size(), sc.control.at(ctrlIdx).size());
+ for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetJointControllers().size(); ++targIdx)
{
- emergencyStopStateRequest = EmergencyStopStateRequest::RequestActive;
- }
- else
- {
- emergencyStopStateRequest = EmergencyStopStateRequest::RequestInactive;
+ JointController& jointCtrl = *controlDevice.rtGetJointControllers().at(targIdx);
+ jointCtrl.getControlTarget()->_copyTo(sc.control.at(ctrlIdx).at(targIdx));
}
}
+ _module<ControlThreadDataBuffer>().rtSensorAndControlBufferCommitWrite();
- void ControlThread::_preFinishControlThreadInitialization()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- controlThreadId = std::this_thread::get_id();
-
- ARMARX_CHECK_EQUAL(rtGetActivatedJointControllers().size(), rtGetActivatedJointToNJointControllerAssignement().size());
- ARMARX_CHECK_EQUAL(rtGetActivatedJointControllers().size(), rtGetActivatedNJointControllers().size());
- //resize buffers used for error oputput
- preSwitchSetup_ActivatedJointControllers.resize(rtGetActivatedJointControllers().size());
- postSwitchSetup_ActivatedJointControllers.resize(rtGetActivatedJointControllers().size());
-
- preSwitchSetup_ActivatedJointToNJointControllerAssignement.resize(rtGetActivatedJointToNJointControllerAssignement().size());
- postSwitchSetup_ActivatedJointToNJointControllerAssignement.resize(rtGetActivatedJointToNJointControllerAssignement().size());
- preSwitchSetup_ActivatedNJointControllers.resize(rtGetActivatedNJointControllers().size());
- postSwitchSetup_ActivatedNJointControllers.resize(rtGetActivatedNJointControllers().size());
+ rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferEnd();
+ }
- // setup inverse dynamics
- if (getProperty<bool>("EnableInverseDynamics").getValue())
- {
- RobotUnit* robotUnit = dynamic_cast<RobotUnit*>(this);
- ARMARX_CHECK_EXPRESSION(robotUnit);
- std::shared_ptr<DynamicsHelper> dynamicsHelpers(new DynamicsHelper(robotUnit));
- auto bodySetName = getProperty<std::string>("InverseDynamicsRobotBodySet").getValue();
- auto rtRobotBodySet = rtGetRobot()->getRobotNodeSet(bodySetName);
- ARMARX_CHECK_EXPRESSION_W_HINT(rtRobotBodySet, "could not find robot node set with name: " << bodySetName << " - Check property InverseDynamicsRobotBodySet");
- // rtfilters::RTFilterBasePtr accFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
- // rtfilters::RTFilterBasePtr velFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
- rtfilters::RTFilterBasePtr accFilter(new rtfilters::AverageFilter(30));
- rtfilters::RTFilterBasePtr velFilter(new rtfilters::AverageFilter(30));
-
- auto setList = armarx::Split(getProperty<std::string>("InverseDynamicsRobotJointSets").getValue(), ",", true, true);
- for (auto& set : setList)
- {
- auto rns = rtGetRobot()->getRobotNodeSet(set);
- ARMARX_CHECK_EXPRESSION_W_HINT(rns, "could not find robot node set with name: " << set << " - Check property InverseDynamicsRobotJointSets");
- dynamicsHelpers->addRobotPart(rns, rtRobotBodySet, velFilter, accFilter);
- ARMARX_VERBOSE << "Added nodeset " << set << " for inverse dynamics";
- }
+ const std::vector<ControlDevicePtr>& ControlThread::rtGetControlDevices() const
+ {
+ return DevicesAttorneyForControlThread::GetControlDevices(this);
+ }
- this->dynamicsHelpers = dynamicsHelpers;
- }
+ const std::vector<SensorDevicePtr>& ControlThread::rtGetSensorDevices()
+ {
+ return DevicesAttorneyForControlThread::GetSensorDevices(this);
+ }
- }
+ RTThreadTimingsSensorDevice& ControlThread::rtGetThreadTimingsSensorDevice()
+ {
+ return DevicesAttorneyForControlThread::GetThreadTimingsSensorDevice(this);
+ }
- void ControlThread::_preOnInitRobotUnit()
+ void ControlThread::rtSetEmergencyStopState(EmergencyStopState state)
+ {
+ if (state == EmergencyStopState::eEmergencyStopActive)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- try
- {
- rtRobot = VirtualRobot::RobotIO::loadRobot(_module<RobotData>().getRobotFileName(), VirtualRobot::RobotIO::eStructure);
- rtRobot->setUpdateCollisionModel(false);
- rtRobot->setUpdateVisualization(false);
- rtRobot->setThreadsafe(false);
- rtRobotNodes = rtRobot->getRobotNodes();
- }
- catch (VirtualRobot::VirtualRobotException& e)
- {
- throw UserException(e.what());
- }
- usPerDevUntilWarn = getProperty<std::size_t>("NjointController_AllowedExecutionTimePerControlDeviceUntilWarning").getValue();
- usPerDevUntilError = getProperty<std::size_t>("NjointController_AllowedExecutionTimePerControlDeviceUntilError").getValue();
+ emergencyStopStateRequest = EmergencyStopStateRequest::RequestActive;
}
-
- void ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+ else
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
+ emergencyStopStateRequest = EmergencyStopStateRequest::RequestInactive;
}
+ }
- EmergencyStopState ControlThread::getEmergencyStopState(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return emergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
- }
+ void ControlThread::_preFinishControlThreadInitialization()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ controlThreadId = std::this_thread::get_id();
- EmergencyStopState ControlThread::getRtEmergencyStopState(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
- }
+ _maxControllerCount = rtGetActivatedJointControllers().size();
- void ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
- }
+ ARMARX_CHECK_EQUAL(_maxControllerCount, rtGetActivatedJointToNJointControllerAssignement().size());
+ ARMARX_CHECK_EQUAL(_maxControllerCount, rtGetActivatedNJointControllers().size());
+ //resize buffers used for error oputput
+ preSwitchSetup_ActivatedJointControllers.resize(_maxControllerCount);
+ postSwitchSetup_ActivatedJointControllers.resize(_maxControllerCount);
+
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement.resize(_maxControllerCount);
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement.resize(_maxControllerCount);
+
+ preSwitchSetup_ActivatedNJointControllers.resize(_maxControllerCount);
+ postSwitchSetup_ActivatedNJointControllers.resize(_maxControllerCount);
+
+ _activatedSynchronousNJointControllersIdx.resize(_maxControllerCount, _maxControllerCount);
+ _activatedAsynchronousNJointControllersIdx.resize(_maxControllerCount, _maxControllerCount);
- void ControlThread::processEmergencyStopRequest()
+ // setup inverse dynamics
+ if (getProperty<bool>("EnableInverseDynamics").getValue())
{
- const auto state = emergencyStopStateRequest.exchange(EmergencyStopStateRequest::RequestNone);
- switch (state)
+ RobotUnit* robotUnit = dynamic_cast<RobotUnit*>(this);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ std::shared_ptr<DynamicsHelper> dynamicsHelpers(new DynamicsHelper(robotUnit));
+ auto bodySetName = getProperty<std::string>("InverseDynamicsRobotBodySet").getValue();
+ auto rtRobotBodySet = rtGetRobot()->getRobotNodeSet(bodySetName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(rtRobotBodySet, "could not find robot node set with name: " << bodySetName << " - Check property InverseDynamicsRobotBodySet");
+ // rtfilters::RTFilterBasePtr accFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
+ // rtfilters::RTFilterBasePtr velFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
+ rtfilters::RTFilterBasePtr accFilter(new rtfilters::AverageFilter(30));
+ rtfilters::RTFilterBasePtr velFilter(new rtfilters::AverageFilter(30));
+
+ auto setList = armarx::Split(getProperty<std::string>("InverseDynamicsRobotJointSets").getValue(), ",", true, true);
+ for (auto& set : setList)
{
- case EmergencyStopStateRequest::RequestActive:
- setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- break;
- case EmergencyStopStateRequest::RequestInactive:
- setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
- break;
- case EmergencyStopStateRequest::RequestNone:
- break;
- default:
- ARMARX_CHECK_EXPRESSION_W_HINT(!static_cast<int>(state), "Unkown value for EmergencyStopStateRequest");
+ auto rns = rtGetRobot()->getRobotNodeSet(set);
+ ARMARX_CHECK_EXPRESSION_W_HINT(rns, "could not find robot node set with name: " << set << " - Check property InverseDynamicsRobotJointSets");
+ dynamicsHelpers->addRobotPart(rns, rtRobotBodySet, velFilter, accFilter);
+ ARMARX_VERBOSE << "Added nodeset " << set << " for inverse dynamics";
}
- }
- const std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers() const
- {
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
+ this->dynamicsHelpers = dynamicsHelpers;
}
- const std::vector<NJointController*>& ControlThread::rtGetActivatedNJointControllers() const
- {
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
- }
+ }
- const std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement() const
+ void ControlThread::_preOnInitRobotUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ try
{
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ rtRobot = VirtualRobot::RobotIO::loadRobot(_module<RobotData>().getRobotFileName(), VirtualRobot::RobotIO::eStructure);
+ rtRobot->setUpdateCollisionModel(false);
+ rtRobot->setUpdateVisualization(false);
+ rtRobot->setThreadsafe(false);
+ rtRobotNodes = rtRobot->getRobotNodes();
}
-
- std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers()
+ catch (VirtualRobot::VirtualRobotException& e)
{
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
+ throw UserException(e.what());
}
+ usPerDevUntilWarn = getProperty<std::size_t>("NjointController_AllowedExecutionTimePerControlDeviceUntilWarning").getValue();
+ usPerDevUntilError = getProperty<std::size_t>("NjointController_AllowedExecutionTimePerControlDeviceUntilError").getValue();
+ }
- std::vector<NJointController*>& ControlThread::rtGetActivatedNJointControllers()
- {
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
- }
+ void ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
+ }
- std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement()
- {
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
- }
+ EmergencyStopState ControlThread::getEmergencyStopState(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return emergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ }
- const std::vector<JointController*>& ControlThread::rtGetRequestedJointControllers() const
- {
- return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointControllers(this);
- }
+ EmergencyStopState ControlThread::getRtEmergencyStopState(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ }
- const std::vector<NJointController*>& ControlThread::rtGetRequestedNJointControllers() const
- {
- return ControlThreadDataBufferAttorneyForControlThread::GetRequestedNJointControllers(this);
- }
+ void ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
+ }
- const std::vector<std::size_t>& ControlThread::rtGetRequestedJointToNJointControllerAssignement() const
- {
- return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointToNJointControllerAssignement(this);
+ void ControlThread::processEmergencyStopRequest()
+ {
+ const auto state = emergencyStopStateRequest.exchange(EmergencyStopStateRequest::RequestNone);
+ switch (state)
+ {
+ case EmergencyStopStateRequest::RequestActive:
+ setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ break;
+ case EmergencyStopStateRequest::RequestInactive:
+ setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
+ break;
+ case EmergencyStopStateRequest::RequestNone:
+ break;
+ default:
+ ARMARX_CHECK_EXPRESSION_W_HINT(!static_cast<int>(state), "Unkown value for EmergencyStopStateRequest");
}
+ }
+
+ const std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
+ }
- void ControlThread::rtSyncNJointControllerRobot(NJointController* njctrl)
+ const std::vector<NJointControllerBase*>& ControlThread::rtGetActivatedNJointControllers() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
+ }
+
+ const std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ }
+
+ std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers()
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
+ }
+
+ std::vector<NJointControllerBase*>& ControlThread::rtGetActivatedNJointControllers()
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
+ }
+
+ std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement()
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ }
+
+ const std::vector<JointController*>& ControlThread::rtGetRequestedJointControllers() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointControllers(this);
+ }
+
+ const std::vector<NJointControllerBase*>& ControlThread::rtGetRequestedNJointControllers() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetRequestedNJointControllers(this);
+ }
+
+ const std::vector<std::size_t>& ControlThread::rtGetRequestedJointToNJointControllerAssignement() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointToNJointControllerAssignement(this);
+ }
+
+ void ControlThread::rtSyncNJointControllerRobot(NJointControllerBase* njctrl)
+ {
+ if (njctrl->rtGetRobot())
{
- if (njctrl->rtGetRobot())
+ auto& from = rtRobotNodes;
+ auto& to = njctrl->rtGetRobotNodes();
+ for (std::size_t i = 0; i < from.size(); ++i)
{
- auto& from = rtRobotNodes;
- auto& to = njctrl->rtGetRobotNodes();
- for (std::size_t i = 0; i < from.size(); ++i)
- {
- to.at(i)->copyPoseFrom(from.at(i));
- }
+ to.at(i)->copyPoseFrom(from.at(i));
}
}
+ }
- void ControlThread::dumpRtControllerSetup(
- std::ostream& out,
- const std::string& indent,
- const std::vector<JointController*>& activeJCtrls,
- const std::vector<std::size_t>& assignement,
- const std::vector<NJointController*>& activeNJCtrls) const
+ void ControlThread::dumpRtControllerSetup(
+ std::ostream& out,
+ const std::string& indent,
+ const std::vector<JointController*>& activeJCtrls,
+ const std::vector<std::size_t>& assignement,
+ const std::vector<NJointControllerBase*>& activeNJCtrls) const
+ {
+ out << indent << "JointControllers:\n";
+ {
+ const auto& cdevs = rtGetControlDevices();
+ for (std::size_t i = 0; i < cdevs.size(); ++i)
+ {
+ const JointController* jctrl = activeJCtrls.at(i);
+ out << indent << "\t(" << i << ")\t" << cdevs.at(i)->rtGetDeviceName() << ":\n"
+ << indent << "\t\t Controller: " << jctrl->getControlMode() << " (" << jctrl << ")\n"
+ << indent << "\t\t Assigned NJoint: " << assignement.at(i) << "\n";
+ }
+ }
+ out << indent << "NJointControllers:\n";
{
- out << indent << "JointControllers:\n";
+ for (std::size_t i = 0; i < activeNJCtrls.size(); ++i)
{
- const auto& cdevs = rtGetControlDevices();
- for (std::size_t i = 0; i < cdevs.size(); ++i)
+ const auto* njctrl = activeNJCtrls.at(i);
+ out << indent << "\t(" << i << ")\t";
+ if (njctrl)
{
- const JointController* jctrl = activeJCtrls.at(i);
- out << indent << "\t(" << i << ")\t" << cdevs.at(i)->rtGetDeviceName() << ":\n"
- << indent << "\t\t Controller: " << jctrl->getControlMode() << " (" << jctrl << ")\n"
- << indent << "\t\t Assigned NJoint: " << assignement.at(i) << "\n";
+ out << njctrl->rtGetInstanceName() << " (" << njctrl << "):"
+ << "\t Class: " << njctrl->rtGetClassName() << "\n";
}
- }
- out << indent << "NJointControllers:\n";
- {
- for (std::size_t i = 0; i < activeNJCtrls.size(); ++i)
+ else
{
- const auto* njctrl = activeNJCtrls.at(i);
- out << indent << "\t(" << i << ")\t";
- if (njctrl)
- {
- out << njctrl->rtGetInstanceName() << " (" << njctrl << "):"
- << "\t Class: " << njctrl->rtGetClassName() << "\n";
- }
- else
- {
- out << " (" << njctrl << ")\n";
- }
+ out << " (" << njctrl << ")\n";
}
}
-
}
- std::string ControlThread::dumpRtState() const
- {
- std::stringstream str;
- str << "state requested\n";
- dumpRtControllerSetup(
- str, "\t",
- rtGetRequestedJointControllers(),
- rtGetRequestedJointToNJointControllerAssignement(),
- rtGetRequestedNJointControllers());
-
- str << "state before rtSwitchControllerSetup() was called\n";
- dumpRtControllerSetup(
- str, "\t",
- preSwitchSetup_ActivatedJointControllers,
- preSwitchSetup_ActivatedJointToNJointControllerAssignement,
- preSwitchSetup_ActivatedNJointControllers);
-
- str << "state after rtSwitchControllerSetup() was called\n";
- dumpRtControllerSetup(
- str, "\t",
- postSwitchSetup_ActivatedJointControllers,
- postSwitchSetup_ActivatedJointToNJointControllerAssignement,
- postSwitchSetup_ActivatedNJointControllers);
-
- str << "state now\n";
- dumpRtControllerSetup(
- str, "\t",
- rtGetActivatedJointControllers(),
- rtGetActivatedJointToNJointControllerAssignement(),
- rtGetActivatedNJointControllers());
+ }
- str << VAROUT(rtSwitchControllerSetupChangedControllers) << "\n";
- str << VAROUT(numberOfInvalidTargetsInThisIteration) << "\n";
- return str.str();
- }
+ std::string ControlThread::dumpRtState() const
+ {
+ std::stringstream str;
+ str << "state requested\n";
+ dumpRtControllerSetup(
+ str, "\t",
+ rtGetRequestedJointControllers(),
+ rtGetRequestedJointToNJointControllerAssignement(),
+ rtGetRequestedNJointControllers());
+
+ str << "state before rtSwitchControllerSetup() was called\n";
+ dumpRtControllerSetup(
+ str, "\t",
+ preSwitchSetup_ActivatedJointControllers,
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement,
+ preSwitchSetup_ActivatedNJointControllers);
+
+ str << "state after rtSwitchControllerSetup() was called\n";
+ dumpRtControllerSetup(
+ str, "\t",
+ postSwitchSetup_ActivatedJointControllers,
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement,
+ postSwitchSetup_ActivatedNJointControllers);
+
+ str << "state now\n";
+ dumpRtControllerSetup(
+ str, "\t",
+ rtGetActivatedJointControllers(),
+ rtGetActivatedJointToNJointControllerAssignement(),
+ rtGetActivatedNJointControllers());
+
+ str << VAROUT(rtSwitchControllerSetupChangedControllers) << "\n";
+ str << VAROUT(numberOfInvalidTargetsInThisIteration) << "\n";
+ return str.str();
+ }
- void ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
- {
- rtRobot->setGlobalPose(gp, updateRobot);
- }
+ void ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
+ {
+ rtRobot->setGlobalPose(gp, updateRobot);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index 59644ac61b37fd7e2b303fddc109e59fc2d430cd..f1adb85fd4e0e2422ee9b06ed6a6880104742e5e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -35,340 +35,346 @@ namespace armarx
{
class DynamicsHelper;
class RTThreadTimingsSensorDevice;
- namespace RobotUnitModule
+}
+
+namespace armarx::RobotUnitModule
+{
+ class ControlThreadPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ public:
+ ControlThreadPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::size_t>(
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning", 2,
+ "A Warning will be printed, If the execution time in micro seconds of a NJointControllerBase "
+ "exceeds this parameter times the number of ControlDevices."
+ );
+ defineOptionalProperty<std::size_t>(
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilError", 20,
+ "An Error will be printed, If the execution time in micro seconds of a NJointControllerBase "
+ "exceeds this parameter times the number of ControlDevices."
+ );
+ defineOptionalProperty<bool>("EnableInverseDynamics", false,
+ "If true, inverse dynamics are calculated for all joints given in the "
+ "InverseDynamicsRobotJointSets property during each loop of "
+ "the rt control thread.");
+ defineOptionalProperty<std::string>(
+ "InverseDynamicsRobotJointSets", "LeftArm,RightArm",
+ "Comma separated list of robot nodesets for which the inverse dynamics should be calculcated."
+ );
+ defineOptionalProperty<std::string>(
+ "InverseDynamicsRobotBodySet", "RobotCol",
+ "Robot nodeset of which the masses should be used for the inverse dynamics"
+ );
+ }
+ };
+
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" manages the \ref ControlThread.
+ *
+ * It offers the basic functions called in the \ref ControlThread.
+ *
+ * @see ModuleBase
+ */
+ class ControlThread : virtual public ModuleBase, virtual public RobotUnitControlThreadInterface
{
- class ControlThreadPropertyDefinitions: public ModuleBasePropertyDefinitions
+ friend class ModuleBase;
+ enum class EmergencyStopStateRequest
{
- public:
- ControlThreadPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::size_t>(
- "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning", 2,
- "A Warning will be printed, If the execution time in micro seconds of a NJointController "
- "exceeds this parameter times the number of ControlDevices."
- );
- defineOptionalProperty<std::size_t>(
- "NjointController_AllowedExecutionTimePerControlDeviceUntilError", 20,
- "An Error will be printed, If the execution time in micro seconds of a NJointController "
- "exceeds this parameter times the number of ControlDevices."
- );
- defineOptionalProperty<bool>("EnableInverseDynamics", false,
- "If true, inverse dynamics are calculated for all joints given in the "
- "InverseDynamicsRobotJointSets property during each loop of "
- "the rt control thread.");
- defineOptionalProperty<std::string>(
- "InverseDynamicsRobotJointSets", "LeftArm,RightArm",
- "Comma separated list of robot nodesets for which the inverse dynamics should be calculcated."
- );
- defineOptionalProperty<std::string>(
- "InverseDynamicsRobotBodySet", "RobotCol",
- "Robot nodeset of which the masses should be used for the inverse dynamics"
- );
- }
+ RequestActive,
+ RequestInactive,
+ RequestNone,
};
+ public:
+ /**
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
+ */
+ static ControlThread& Instance()
+ {
+ return ModuleBase::Instance<ControlThread>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ ///@see \ref ModuleBase::_preFinishControlThreadInitialization
+ void _preFinishControlThreadInitialization();
+ ///@see \ref ModuleBase::_preOnInitRobotUnit
+ void _preOnInitRobotUnit();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Sets the \ref EmergencyStopState
+ * @param state The \ref EmergencyStopState to set
+ */
+ void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Returns the \ref ControlThread's target \ref EmergencyStopState
+ * @return The \ref EmergencyStopState
+ */
+ EmergencyStopState getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the \ref ControlThread's \ref EmergencyStopState
+ * @return The \ref EmergencyStopState
+ */
+ EmergencyStopState getRtEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// Module interface /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Teturns he \ref ControlThread's thread id
+ * @return The \ref ControlThread's thread id
+ * @see controlThreadId
+ */
+ std::thread::id getControlThreadId() const
+ {
+ return controlThreadId;
+ }
+ private:
+ /**
+ * @brief Sets the \ref EmergencyStopState without updating the topic.
+ * For use in \ref RobotUnitEmergencyStopMaster.
+ * @param state The \ref EmergencyStopState to set
+ */
+ void setEmergencyStopStateNoReportToTopic(EmergencyStopState state);
+ /**
+ * @brief Sets the emergency stop state to the request from rt (if there was a request).
+ * For use in \ref Publisher.
+ */
+ void processEmergencyStopRequest();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// rt interface ///////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" manages the \ref ControlThread.
- *
- * It offers the basic functions called in the \ref ControlThread.
- *
- * @see ModuleBase
+ * @brief Returns the simox robot used in the control thread
+ * @return The simox robot used in the control thread
*/
- class ControlThread : virtual public ModuleBase, virtual public RobotUnitControlThreadInterface
+ const VirtualRobot::RobotPtr& rtGetRobot() const
{
- friend class ModuleBase;
- enum class EmergencyStopStateRequest
- {
- RequestActive,
- RequestInactive,
- RequestNone,
- };
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static ControlThread& Instance()
- {
- return ModuleBase::Instance<ControlThread>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- ///@see \ref ModuleBase::_preFinishControlThreadInitialization
- void _preFinishControlThreadInitialization();
- ///@see \ref ModuleBase::_preOnInitRobotUnit
- void _preOnInitRobotUnit();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// ice interface //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Sets the \ref EmergencyStopState
- * @param state The \ref EmergencyStopState to set
- */
- void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Returns the \ref ControlThread's target \ref EmergencyStopState
- * @return The \ref EmergencyStopState
- */
- EmergencyStopState getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the \ref ControlThread's \ref EmergencyStopState
- * @return The \ref EmergencyStopState
- */
- EmergencyStopState getRtEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////// Module interface /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Teturns he \ref ControlThread's thread id
- * @return The \ref ControlThread's thread id
- * @see controlThreadId
- */
- std::thread::id getControlThreadId() const
- {
- return controlThreadId;
- }
- private:
- /**
- * @brief Sets the \ref EmergencyStopState without updating the topic.
- * For use in \ref RobotUnitEmergencyStopMaster.
- * @param state The \ref EmergencyStopState to set
- */
- void setEmergencyStopStateNoReportToTopic(EmergencyStopState state);
- /**
- * @brief Sets the emergency stop state to the request from rt (if there was a request).
- * For use in \ref Publisher.
- */
- void processEmergencyStopRequest();
+ return rtRobot;
+ }
+ /**
+ * @brief Changes the set of active controllers.
+ * Changes can be caused by a new set of requested controllers or emergency stop
+ * @return Whether active controllers were changed
+ */
+ bool rtSwitchControllerSetup();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// rt interface ///////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- /**
- * @brief Returns the simox robot used in the control thread
- * @return The simox robot used in the control thread
- */
- const VirtualRobot::RobotPtr& rtGetRobot() const
- {
- return rtRobot;
- }
- /**
- * @brief Changes the set of active controllers.
- * Changes can be caused by a new set of requested controllers or emergency stop
- * @return Whether active controllers were changed
- */
- bool rtSwitchControllerSetup();
+ /**
+ * @brief Searches for the \ref NJointControllerBase responsible for the given \ref JointController and deactivates it.
+ * Does not commit the new set of activated controllers)
+ * @param ctrlDevIndex The control device index causing the problem.
+ */
+ void rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex);
+ /**
+ * @brief Deactivates a NJointControllerBase and sets all used ControlDevices to EmergencyStop.
+ * Does not commit the new set of activated controllers)
+ * @param nJointCtrlIndex The NJointControllerBase to deactivate (index in controllersActivated)
+ */
+ void rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex);
+ /**
+ * @brief Runs NJoint controllers
+ * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
+ * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
+ */
+ void rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ /**
+ * @brief Runs Joint controllers and writes target values to ControlDevices
+ * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
+ * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
+ */
+ void rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ /**
+ * @brief Calls \ref SensorDevice::rtReadSensorValues for all \ref SensorDevice "SensorDevices".
+ * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
+ * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
+ */
+ void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ /**
+ * @brief Execute code after updating the sensor values. Default implementation calculates the inverse dynamics.
+ * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
+ * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
+ */
+ void virtual rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ /// @brief Deactivates all NJointControllers generating invalid targets.
+ void rtHandleInvalidTargets();
+ /// @brief Calls \ref JointController::rtResetTarget for all active Joint controllers
+ void rtResetAllTargets();
+ /**
+ * @brief Hook for deriving classes (called AFTER a NJointControllerBase is deactivated due to an error)
+ *
+ * This is useful when two Control devices can be controlled by different NJointControllers but either both or none
+ * have to be controlled.
+ *
+ * Since this hook is called after the controller with an error was deactivated an other call to
+ * \ref rtDeactivateAssignedNJointControllerBecauseOfError can't lead to a cyclic execution.
+ */
+ virtual void rtDeactivatedNJointControllerBecauseOfError(const NJointControllerBasePtr& /*nJointCtrl*/) {}
+ /**
+ * @brief Updates the current \ref SensorValueBase "SensorValues" and \ref ControlTargetBase "ControlTargets"
+ * for code running outside of the \ref ControlThread
+ * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
+ * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
+ */
+ void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- /**
- * @brief Searches for the \ref NJointController responsible for the given \ref JointController and deactivates it.
- * Does not commit the new set of activated controllers)
- * @param ctrlDevIndex The control device index causing the problem.
- */
- void rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex);
- /**
- * @brief Deactivates a NJointController and sets all used ControlDevices to EmergencyStop.
- * Does not commit the new set of activated controllers)
- * @param nJointCtrlIndex The NJointController to deactivate (index in controllersActivated)
- */
- void rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex);
- /**
- * @brief Runs NJoint controllers
- * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
- * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
- */
- void rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- /**
- * @brief Runs Joint controllers and writes target values to ControlDevices
- * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
- * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
- */
- void rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- /**
- * @brief Calls \ref SensorDevice::rtReadSensorValues for all \ref SensorDevice "SensorDevices".
- * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
- * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
- */
- void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- /**
- * @brief Execute code after updating the sensor values. Default implementation calculates the inverse dynamics.
- * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
- * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
- */
- void virtual rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- /// @brief Deactivates all NJointControllers generating invalid targets.
- void rtHandleInvalidTargets();
- /// @brief Calls \ref JointController::rtResetTarget for all active Joint controllers
- void rtResetAllTargets();
- /**
- * @brief Hook for deriving classes (called AFTER a NJointController is deactivated due to an error)
- *
- * This is useful when two Control devices can be controlled by different NJointControllers but either both or none
- * have to be controlled.
- *
- * Since this hook is called after the controller with an error was deactivated an other call to
- * \ref rtDeactivateAssignedNJointControllerBecauseOfError can't lead to a cyclic execution.
- */
- virtual void rtDeactivatedNJointControllerBecauseOfError(const NJointControllerPtr& /*nJointCtrl*/) {}
- /**
- * @brief Updates the current \ref SensorValueBase "SensorValues" and \ref ControlTargetBase "ControlTargets"
- * for code running outside of the \ref ControlThread
- * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
- * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
- */
- void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ /**
+ * @brief Returns all \ref ControlDevice "ControlDevices"
+ * @return All \ref ControlDevice "ControlDevices"
+ */
+ const std::vector<ControlDevicePtr>& rtGetControlDevices() const;
+ /**
+ * @brief Returns all \ref SensorDevice "SensorDevices"
+ * @return All \ref SensorDevice "SensorDevices"
+ */
+ const std::vector<SensorDevicePtr>& rtGetSensorDevices();
+ /**
+ * @brief Returns the \ref RTThreadTimingsSensorDevice
+ * @return The \ref RTThreadTimingsSensorDevice
+ */
+ RTThreadTimingsSensorDevice& rtGetThreadTimingsSensorDevice();
- /**
- * @brief Returns all \ref ControlDevice "ControlDevices"
- * @return All \ref ControlDevice "ControlDevices"
- */
- const std::vector<ControlDevicePtr>& rtGetControlDevices() const;
- /**
- * @brief Returns all \ref SensorDevice "SensorDevices"
- * @return All \ref SensorDevice "SensorDevices"
- */
- const std::vector<SensorDevicePtr>& rtGetSensorDevices();
- /**
- * @brief Returns the \ref RTThreadTimingsSensorDevice
- * @return The \ref RTThreadTimingsSensorDevice
- */
- RTThreadTimingsSensorDevice& rtGetThreadTimingsSensorDevice();
+ /**
+ * @brief Returns whether the rt thread is in emergency stop. (This function is for use in the \ref ControlThread)
+ * @return Whether the rt thread is in emergency stop.
+ */
+ bool rtIsInEmergencyStop() const
+ {
+ return rtIsInEmergencyStop_;
+ }
+ /**
+ * @brief Returns the \ref ControlThread's emergency stop state. (This function is for use in the \ref ControlThread)
+ * @return The \ref ControlThread's emergency stop state
+ */
+ EmergencyStopState rtGetEmergencyStopState() const
+ {
+ return rtIsInEmergencyStop_ ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ }
- /**
- * @brief Returns whether the rt thread is in emergency stop. (This function is for use in the \ref ControlThread)
- * @return Whether the rt thread is in emergency stop.
- */
- bool rtIsInEmergencyStop() const
- {
- return rtIsInEmergencyStop_;
- }
- /**
- * @brief Returns the \ref ControlThread's emergency stop state. (This function is for use in the \ref ControlThread)
- * @return The \ref ControlThread's emergency stop state
- */
- EmergencyStopState rtGetEmergencyStopState() const
- {
- return rtIsInEmergencyStop_ ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
- }
+ /**
+ * @brief Set an \ref EmergencyStopState request. This request will be executed by the \ref Publisher
+ * @param state The \ref EmergencyStopState to set
+ */
+ void rtSetEmergencyStopState(EmergencyStopState state);
- /**
- * @brief Set an \ref EmergencyStopState request. This request will be executed by the \ref Publisher
- * @param state The \ref EmergencyStopState to set
- */
- void rtSetEmergencyStopState(EmergencyStopState state);
+ void rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot = true);
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * @brief Returns the activated \ref JointController "JointControllers"
+ * @return The activated \ref JointController "JointControllers"
+ */
+ std::vector<JointController*>& rtGetActivatedJointControllers();
+ const std::vector<JointController*>& rtGetActivatedJointControllers() const;
+ /**
+ * @brief Returns the activated \ref NJointControllerBase "NJointControllers"
+ * @return The activated \ref NJointControllerBase "NJointControllers"
+ */
+ std::vector<NJointControllerBase*>& rtGetActivatedNJointControllers();
+ const std::vector<NJointControllerBase*>& rtGetActivatedNJointControllers() const;
- void rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot = true);
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////// implementation //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * @brief Returns the activated \ref JointController "JointControllers"
- * @return The activated \ref JointController "JointControllers"
- */
- std::vector<JointController*>& rtGetActivatedJointControllers();
- const std::vector<JointController*>& rtGetActivatedJointControllers() const;
- /**
- * @brief Returns the activated \ref NJointController "NJointControllers"
- * @return The activated \ref NJointController "NJointControllers"
- */
- std::vector<NJointController*>& rtGetActivatedNJointControllers();
- const std::vector<NJointController*>& rtGetActivatedNJointControllers() const;
+ /**
+ * @brief Returns a vector containing the index of the activated \ref NJointControllerBase
+ * for each \ref JointController
+ * @return A vector containing the index of the activated \ref NJointControllerBase
+ * for each \ref JointController
+ */
+ std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement();
+ const std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement() const;
- /**
- * @brief Returns a vector containing the index of the activated \ref NJointController
- * for each \ref JointController
- * @return A vector containing the index of the activated \ref NJointController
- * for each \ref JointController
- */
- std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement();
- const std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement() const;
+ /**
+ * @brief Returns the requested \ref JointController "JointControllers"
+ * @return The requested \ref JointController "JointControllers"
+ */
+ const std::vector<JointController*>& rtGetRequestedJointControllers() const;
+ /**
+ * @brief Returns the requested \ref NJointControllerBase "NJointControllers"
+ * @return The requested \ref NJointControllerBase "NJointControllers"
+ */
+ const std::vector<NJointControllerBase*>& rtGetRequestedNJointControllers() const;
- /**
- * @brief Returns the requested \ref JointController "JointControllers"
- * @return The requested \ref JointController "JointControllers"
- */
- const std::vector<JointController*>& rtGetRequestedJointControllers() const;
- /**
- * @brief Returns the requested \ref NJointController "NJointControllers"
- * @return The requested \ref NJointController "NJointControllers"
- */
- const std::vector<NJointController*>& rtGetRequestedNJointControllers() const;
+ /**
+ * @brief Returns a vector containing the index of the requested \ref NJointControllerBase
+ * for each \ref JointController
+ * @return A vector containing the index of the requested \ref NJointControllerBase
+ * for each \ref JointController
+ */
+ const std::vector<std::size_t>& rtGetRequestedJointToNJointControllerAssignement() const;
- /**
- * @brief Returns a vector containing the index of the requested \ref NJointController
- * for each \ref JointController
- * @return A vector containing the index of the requested \ref NJointController
- * for each \ref JointController
- */
- const std::vector<std::size_t>& rtGetRequestedJointToNJointControllerAssignement() const;
+ void rtSyncNJointControllerRobot(NJointControllerBase* njctrl);
- void rtSyncNJointControllerRobot(NJointController* njctrl);
+ void dumpRtControllerSetup(
+ std::ostream& out,
+ const std::string& indent,
+ const std::vector<JointController*>& activeCdevs,
+ const std::vector<std::size_t>& activeJCtrls,
+ const std::vector<NJointControllerBase*>& assignement) const;
+ std::string dumpRtState() const;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ ///@brief Whether the ControlThread is in emergency stop
+ std::atomic_bool rtIsInEmergencyStop_ {false};
+ ///@brief Whether the ControlThread is supposed to be in emergency stop
+ std::atomic_bool emergencyStop {false};
- void dumpRtControllerSetup(
- std::ostream& out,
- const std::string& indent,
- const std::vector<JointController*>& activeCdevs,
- const std::vector<std::size_t>& activeJCtrls,
- const std::vector<NJointController*>& assignement) const;
- std::string dumpRtState() const;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- ///@brief Whether the ControlThread is in emergency stop
- std::atomic_bool rtIsInEmergencyStop_ {false};
- ///@brief Whether the ControlThread is supposed to be in emergency stop
- std::atomic_bool emergencyStop {false};
+ ///@brief The \ref VirtualRobot::Robot used in the \ref ControlThread
+ VirtualRobot::RobotPtr rtRobot;
+ ///@brief The \ref VirtualRobot::Robot's \ref VirtualRobot::RobotNode "VirtualRobot::RobotNode" used in the \ref ControlThread
+ std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
- ///@brief The \ref VirtualRobot::Robot used in the \ref ControlThread
- VirtualRobot::RobotPtr rtRobot;
- ///@brief The \ref VirtualRobot::Robot's \ref VirtualRobot::RobotNode "VirtualRobot::RobotNode" used in the \ref ControlThread
- std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
+ ///@brief The \ref ControlThread's thread id
+ std::atomic<std::thread::id> controlThreadId;
- ///@brief The \ref ControlThread's thread id
- std::atomic<std::thread::id> controlThreadId;
+ /// @brief A Warning will be printed, if the execution time per ControlDev of a NJointControllerBase exceeds this parameter
+ std::size_t usPerDevUntilWarn;
+ /// @brief An Error will be printed, if the execution time per ControlDev of a NJointControllerBase exceeds this parameter
+ std::size_t usPerDevUntilError;
- /// @brief A Warning will be printed, if the execution time per ControlDev of a NJointController exceeds this parameter
- std::size_t usPerDevUntilWarn;
- /// @brief An Error will be printed, if the execution time per ControlDev of a NJointController exceeds this parameter
- std::size_t usPerDevUntilError;
+ bool rtSwitchControllerSetupChangedControllers {false};
+ std::size_t numberOfInvalidTargetsInThisIteration {0};
- bool rtSwitchControllerSetupChangedControllers {false};
- std::size_t numberOfInvalidTargetsInThisIteration {0};
+ std::vector<JointController*> preSwitchSetup_ActivatedJointControllers;
+ std::vector<std::size_t> preSwitchSetup_ActivatedJointToNJointControllerAssignement;
+ std::vector<NJointControllerBase*> preSwitchSetup_ActivatedNJointControllers;
- std::vector<JointController*> preSwitchSetup_ActivatedJointControllers;
- std::vector<std::size_t> preSwitchSetup_ActivatedJointToNJointControllerAssignement;
- std::vector<NJointController*> preSwitchSetup_ActivatedNJointControllers;
+ std::vector<JointController*> postSwitchSetup_ActivatedJointControllers;
+ std::vector<std::size_t> postSwitchSetup_ActivatedJointToNJointControllerAssignement;
+ std::vector<NJointControllerBase*> postSwitchSetup_ActivatedNJointControllers;
- std::vector<JointController*> postSwitchSetup_ActivatedJointControllers;
- std::vector<std::size_t> postSwitchSetup_ActivatedJointToNJointControllerAssignement;
- std::vector<NJointController*> postSwitchSetup_ActivatedNJointControllers;
+ std::vector<std::size_t> _activatedSynchronousNJointControllersIdx;
+ std::vector<std::size_t> _activatedAsynchronousNJointControllersIdx;
- std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest {EmergencyStopStateRequest::RequestNone};
+ std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest {EmergencyStopStateRequest::RequestNone};
- std::shared_ptr<DynamicsHelper> dynamicsHelpers;
+ std::shared_ptr<DynamicsHelper> dynamicsHelpers;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class ControlThreadAttorneyForPublisher;
- /**
- * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref RobotUnitEmergencyStopMaster.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class ControlThreadAttorneyForRobotUnitEmergencyStopMaster;
- };
- }
+ std::size_t _maxControllerCount = 0;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class ControlThreadAttorneyForPublisher;
+ /**
+ * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref RobotUnitEmergencyStopMaster.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class ControlThreadAttorneyForRobotUnitEmergencyStopMaster;
+ };
}
+
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
index 36e7c7a9a3fc78e3529fe7f4af00a8974a0e61f0..b310e7f57ea397470b683e40b3f9606d2885185e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
@@ -27,315 +27,309 @@
#include "../NJointControllers/NJointController.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ /**
+ * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThreadDataBuffer.
+ * \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 DevicesAttorneyForControlThreadDataBuffer
{
- /**
- * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThreadDataBuffer.
- * \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 DevicesAttorneyForControlThreadDataBuffer
+ friend class ControlThreadDataBuffer;
+ static std::vector<JointController*> GetStopMovementJointControllers(ControlThreadDataBuffer* p)
{
- friend class ControlThreadDataBuffer;
- static std::vector<JointController*> GetStopMovementJointControllers(ControlThreadDataBuffer* p)
- {
- return p->_module<Devices>().getStopMovementJointControllers();
- }
- };
- /**
- * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref ControlThreadDataBuffer.
- * \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 ControllerManagementAttorneyForControlThreadDataBuffer
- {
- friend class ControlThreadDataBuffer;
+ return p->_module<Devices>().getStopMovementJointControllers();
+ }
+ };
+ /**
+ * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref ControlThreadDataBuffer.
+ * \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 ControllerManagementAttorneyForControlThreadDataBuffer
+ {
+ friend class ControlThreadDataBuffer;
- static void UpdateNJointControllerRequestedState(ControlThreadDataBuffer* p, const std::set<NJointControllerPtr>& request)
- {
- p->_module<ControllerManagement>().updateNJointControllerRequestedState(request);
- }
- };
- }
+ static void UpdateNJointControllerRequestedState(ControlThreadDataBuffer* p, const std::set<NJointControllerBasePtr>& request)
+ {
+ p->_module<ControllerManagement>().updateNJointControllerRequestedState(request);
+ }
+ };
}
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ JointAndNJointControllers ControlThreadDataBuffer::getActivatedControllers() const
{
- JointAndNJointControllers ControlThreadDataBuffer::getActivatedControllers() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
- return controllersActivated.getReadBuffer();
- }
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ return controllersActivated.getReadBuffer();
+ }
- std::vector<JointController*> ControlThreadDataBuffer::getActivatedJointControllers() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
- return controllersActivated.getReadBuffer().jointControllers;
- }
+ std::vector<JointController*> ControlThreadDataBuffer::getActivatedJointControllers() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ return controllersActivated.getReadBuffer().jointControllers;
+ }
- std::vector<NJointController*> ControlThreadDataBuffer::getActivatedNJointControllers() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
- return controllersActivated.getReadBuffer().nJointControllers;
- }
- bool ControlThreadDataBuffer::activatedControllersChanged() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
- return controllersActivated.updateReadBuffer();
- }
+ std::vector<NJointControllerBase*> ControlThreadDataBuffer::getActivatedNJointControllers() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ return controllersActivated.getReadBuffer().nJointControllers;
+ }
+ bool ControlThreadDataBuffer::activatedControllersChanged() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ return controllersActivated.updateReadBuffer();
+ }
- JointAndNJointControllers ControlThreadDataBuffer::copyRequestedControllers() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
- return controllersRequested.getWriteBuffer();
- }
+ JointAndNJointControllers ControlThreadDataBuffer::copyRequestedControllers() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ return controllersRequested.getWriteBuffer();
+ }
- std::vector<JointController*> ControlThreadDataBuffer::copyRequestedJointControllers() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
- return controllersRequested.getWriteBuffer().jointControllers;
- }
+ std::vector<JointController*> ControlThreadDataBuffer::copyRequestedJointControllers() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ return controllersRequested.getWriteBuffer().jointControllers;
+ }
- std::vector<NJointController*> ControlThreadDataBuffer::copyRequestedNJointControllers() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
- return controllersRequested.getWriteBuffer().nJointControllers;
- }
+ std::vector<NJointControllerBase*> ControlThreadDataBuffer::copyRequestedNJointControllers() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ return controllersRequested.getWriteBuffer().nJointControllers;
+ }
- bool ControlThreadDataBuffer::sensorAndControlBufferChanged() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- return controlThreadOutputBuffer.updateReadBuffer();
- }
+ bool ControlThreadDataBuffer::sensorAndControlBufferChanged() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlThreadOutputBuffer.updateReadBuffer();
+ }
- const SensorAndControl& ControlThreadDataBuffer::getSensorAndControlBuffer() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- return controlThreadOutputBuffer.getReadBuffer();
- }
+ const SensorAndControl& ControlThreadDataBuffer::getSensorAndControlBuffer() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlThreadOutputBuffer.getReadBuffer();
+ }
- void ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
+ void ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ //check NJoint
+ const auto& nJointCtrls = setOfControllers.nJointControllers;
+ std::set<NJointControllerBasePtr> nJointSet {nJointCtrls.begin(), nJointCtrls.end()};
+ nJointSet.erase(nullptr);
+ //# NJoint
+ const std::size_t nNJointCtrls = std::count_if(nJointCtrls.begin(), nJointCtrls.end(), [](const NJointControllerBasePtr & p)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- //check NJoint
- const auto& nJointCtrls = setOfControllers.nJointControllers;
- std::set<NJointControllerPtr> nJointSet {nJointCtrls.begin(), nJointCtrls.end()};
- nJointSet.erase(nullptr);
- //# NJoint
- const std::size_t nNJointCtrls = std::count_if(nJointCtrls.begin(), nJointCtrls.end(), [](const NJointControllerPtr & p)
- {
- return p;
- });
+ return p;
+ });
- ARMARX_CHECK_EXPRESSION(nNJointCtrls == nJointSet.size());
- ARMARX_CHECK_EXPRESSION(nJointCtrls.size() == _module<Devices>().getNumberOfControlDevices());
- //first nNJointCtrls not null
- ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls, [](NJointController * p)
- {
- return p;
- }));
- //last getNumberOfControlDevices()-nNJointCtrls null
- ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin() + nNJointCtrls, nJointCtrls.end(), [](NJointController * p)
- {
- return !p;
- }));
- //conflict free and sorted
- ARMARX_CHECK_EXPRESSION(std::is_sorted(nJointCtrls.begin(), nJointCtrls.end(), std::greater<NJointController*> {}));
- ARMARX_CHECK_EXPRESSION(NJointController::AreNotInConflict(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls));
+ ARMARX_CHECK_EXPRESSION(nNJointCtrls == nJointSet.size());
+ ARMARX_CHECK_EXPRESSION(nJointCtrls.size() == _module<Devices>().getNumberOfControlDevices());
+ //first nNJointCtrls not null
+ ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls, [](NJointControllerBase * p)
+ {
+ return p;
+ }));
+ //last getNumberOfControlDevices()-nNJointCtrls null
+ ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin() + nNJointCtrls, nJointCtrls.end(), [](NJointControllerBase * p)
+ {
+ return !p;
+ }));
+ //conflict free and sorted
+ ARMARX_CHECK_EXPRESSION(std::is_sorted(nJointCtrls.begin(), nJointCtrls.end(), std::greater<NJointControllerBase*> {}));
+ ARMARX_CHECK_EXPRESSION(NJointControllerBase::AreNotInConflict(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls));
- //check JointCtrl
- const auto& jointCtrls = setOfControllers.jointControllers;
- ARMARX_CHECK_EXPRESSION(jointCtrls.size() == _module<Devices>().getNumberOfControlDevices());
- ARMARX_CHECK_EXPRESSION(std::all_of(jointCtrls.begin(), jointCtrls.end(), [](JointController * p)
- {
- return p;
- }));
+ //check JointCtrl
+ const auto& jointCtrls = setOfControllers.jointControllers;
+ ARMARX_CHECK_EXPRESSION(jointCtrls.size() == _module<Devices>().getNumberOfControlDevices());
+ ARMARX_CHECK_EXPRESSION(std::all_of(jointCtrls.begin(), jointCtrls.end(), [](JointController * p)
+ {
+ return p;
+ }));
- //check groups
+ //check groups
+ {
+ ARMARX_DEBUG << "check groups";
+ std::size_t grpIdx = 0;
+ for (const auto& group : _module<Devices>().getControlModeGroups().groups)
{
- ARMARX_DEBUG << "check groups";
- std::size_t grpIdx = 0;
- for (const auto& group : _module<Devices>().getControlModeGroups().groups)
+ ARMARX_CHECK_EXPRESSION(!group.empty());
+ const auto hwMode = setOfControllers.jointControllers.at(_module<Devices>().getControlDeviceIndex(*group.begin()))->getHardwareControlMode();
+ ARMARX_DEBUG << "---- group " << grpIdx << " mode '" << hwMode << "'";
+ for (const auto& other : group)
{
- ARMARX_CHECK_EXPRESSION(!group.empty());
- const auto hwMode = setOfControllers.jointControllers.at(_module<Devices>().getControlDeviceIndex(*group.begin()))->getHardwareControlMode();
- ARMARX_DEBUG << "---- group " << grpIdx << " mode '" << hwMode << "'";
- for (const auto& other : group)
- {
- const auto& dev = _module<Devices>().getControlDeviceIndex(other);
- ARMARX_CHECK_EXPRESSION(dev);
- const auto otherHwMode = setOfControllers.jointControllers.at(dev)->getHardwareControlMode();
- ARMARX_DEBUG << "-------- '" << other << "'' mode '" << otherHwMode << "'";
- ARMARX_CHECK_EXPRESSION(otherHwMode == hwMode);
- }
- ++grpIdx;
+ const auto& dev = _module<Devices>().getControlDeviceIndex(other);
+ ARMARX_CHECK_EXPRESSION(dev);
+ const auto otherHwMode = setOfControllers.jointControllers.at(dev)->getHardwareControlMode();
+ ARMARX_DEBUG << "-------- '" << other << "'' mode '" << otherHwMode << "'";
+ ARMARX_CHECK_EXPRESSION(otherHwMode == hwMode);
}
+ ++grpIdx;
}
- //setup assignement
+ }
+ //setup assignement
+ {
+ ARMARX_DEBUG << "setup assignement index";
+ setOfControllers.resetAssignement();
+ for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < setOfControllers.nJointControllers.size(); ++nJointCtrlIndex)
{
- ARMARX_DEBUG << "setup assignement index";
- setOfControllers.resetAssignement();
- for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < setOfControllers.nJointControllers.size(); ++nJointCtrlIndex)
+ const NJointControllerBase* nJoint = setOfControllers.nJointControllers.at(nJointCtrlIndex);
+ if (!nJoint)
{
- const NJointController* nJoint = setOfControllers.nJointControllers.at(nJointCtrlIndex);
- if (!nJoint)
- {
- break;
- }
- ARMARX_DEBUG << "---- " << nJoint->getInstanceName();
- for (std::size_t jointIndex : nJoint->getControlDeviceUsedIndices())
- {
- ARMARX_DEBUG << "-------- Index " << jointIndex << ": " << setOfControllers.jointToNJointControllerAssignement.at(jointIndex) << "-> " << nJointCtrlIndex;
- ARMARX_CHECK_EXPRESSION(setOfControllers.jointToNJointControllerAssignement.at(jointIndex) == IndexSentinel());
- setOfControllers.jointToNJointControllerAssignement.at(jointIndex) = nJointCtrlIndex;
- }
+ break;
+ }
+ ARMARX_DEBUG << "---- " << nJoint->getInstanceName();
+ for (std::size_t jointIndex : nJoint->getControlDeviceUsedIndices())
+ {
+ ARMARX_DEBUG << "-------- Index " << jointIndex << ": " << setOfControllers.jointToNJointControllerAssignement.at(jointIndex) << "-> " << nJointCtrlIndex;
+ ARMARX_CHECK_EXPRESSION(setOfControllers.jointToNJointControllerAssignement.at(jointIndex) == IndexSentinel());
+ setOfControllers.jointToNJointControllerAssignement.at(jointIndex) = nJointCtrlIndex;
}
}
+ }
- {
- ARMARX_DEBUG << "call checkSetOfControllersToActivate";
- checkSetOfControllersToActivate(setOfControllers);
- }
- //set requested state
- ControllerManagementAttorneyForControlThreadDataBuffer::UpdateNJointControllerRequestedState(this, nJointSet);
-
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.jointToNJointControllerAssignement.size());
- {
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
- controllersRequested.getWriteBuffer() = std::move(setOfControllers);
- controllersRequested.commitWrite();
- }
- ARMARX_DEBUG << "writeRequestedControllers(JointAndNJointControllers&& setOfControllers)...done!";
+ {
+ ARMARX_DEBUG << "call checkSetOfControllersToActivate";
+ checkSetOfControllersToActivate(setOfControllers);
}
+ //set requested state
+ ControllerManagementAttorneyForControlThreadDataBuffer::UpdateNJointControllerRequestedState(this, nJointSet);
- void ControlThreadDataBuffer::setActivateControllersRequest(std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> ctrls)
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.jointToNJointControllerAssignement.size());
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::recursive_mutex> guardRequested {controllersRequestedMutex};
- throwIfDevicesNotReady(__FUNCTION__);
- //erase nullptr
- ctrls.erase(nullptr);
- ARMARX_CHECK_EXPRESSION(ctrls.size() <= _module<Devices>().getNumberOfControlDevices());
+ std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ controllersRequested.getWriteBuffer() = std::move(setOfControllers);
+ controllersRequested.commitWrite();
+ }
+ ARMARX_DEBUG << "writeRequestedControllers(JointAndNJointControllers&& setOfControllers)...done!";
+ }
- const std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> setOfRequestedCtrls
- {
- controllersRequested.getWriteBuffer().nJointControllers.begin(),
- controllersRequested.getWriteBuffer().nJointControllers.end()
- };
- if (setOfRequestedCtrls == ctrls)
- {
- //redirty the flag to swap in the same set again
- controllersRequested.commitWrite();
- return;
- }
- JointAndNJointControllers request {_module<Devices>().getNumberOfControlDevices()};
- std::size_t idx = 0;
- for (const NJointControllerPtr& nJointCtrl : ctrls)
+ void ControlThreadDataBuffer::setActivateControllersRequest(std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::recursive_mutex> guardRequested {controllersRequestedMutex};
+ throwIfDevicesNotReady(__FUNCTION__);
+ //erase nullptr
+ ctrls.erase(nullptr);
+ ARMARX_CHECK_EXPRESSION(ctrls.size() <= _module<Devices>().getNumberOfControlDevices());
+
+ const std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> setOfRequestedCtrls
+ {
+ controllersRequested.getWriteBuffer().nJointControllers.begin(),
+ controllersRequested.getWriteBuffer().nJointControllers.end()
+ };
+ if (setOfRequestedCtrls == ctrls)
+ {
+ //redirty the flag to swap in the same set again
+ controllersRequested.commitWrite();
+ return;
+ }
+ JointAndNJointControllers request {_module<Devices>().getNumberOfControlDevices()};
+ std::size_t idx = 0;
+ for (const NJointControllerBasePtr& nJointCtrl : ctrls)
+ {
+ request.nJointControllers.at(idx++) = nJointCtrl.get();
+ //add Joint for this ctrl
+ for (const auto& cd2cm : nJointCtrl->getControlDeviceUsedControlModeMap())
{
- request.nJointControllers.at(idx++) = nJointCtrl.get();
- //add Joint for this ctrl
- for (const auto& cd2cm : nJointCtrl->getControlDeviceUsedControlModeMap())
+ std::size_t jointCtrlIndex = _module<Devices>().getControlDevices().index(cd2cm.first);
+ if (request.jointControllers.at(jointCtrlIndex))
{
- std::size_t jointCtrlIndex = _module<Devices>().getControlDevices().index(cd2cm.first);
- if (request.jointControllers.at(jointCtrlIndex))
+ std::stringstream ss;
+ ss << "RobotUnit:setActivateControllersRequest controllers to activate are in conflict!\ncontrollers:";
+ for (const auto& ctrl : ctrls)
{
- std::stringstream ss;
- ss << "RobotUnit:setActivateControllersRequest controllers to activate are in conflict!\ncontrollers:";
- for (const auto& ctrl : ctrls)
- {
- ss << "\n" << ctrl->getInstanceName();
- }
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ ss << "\n" << ctrl->getInstanceName();
}
- request.jointControllers.at(jointCtrlIndex) = _module<Devices>().getControlDevices().at(jointCtrlIndex)->getJointController(cd2cm.second);
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
}
+ request.jointControllers.at(jointCtrlIndex) = _module<Devices>().getControlDevices().at(jointCtrlIndex)->getJointController(cd2cm.second);
}
- for (auto i : getIndices(request.jointControllers))
+ }
+ for (auto i : getIndices(request.jointControllers))
+ {
+ JointController*& jointCtrl = request.jointControllers.at(i);
+ if (!jointCtrl)
{
- JointController*& jointCtrl = request.jointControllers.at(i);
- if (!jointCtrl)
+ JointController* jointCtrlOld = controllersRequested.getWriteBuffer().jointControllers.at(i);
+ if (jointCtrlOld == _module<Devices>().getControlDevices().at(i)->getJointStopMovementController())
{
- JointController* jointCtrlOld = controllersRequested.getWriteBuffer().jointControllers.at(i);
- if (jointCtrlOld == _module<Devices>().getControlDevices().at(i)->getJointStopMovementController())
- {
- //don't replace this controller with emergency stop
- jointCtrl = jointCtrlOld;
- }
- else
- {
- //no one controls this device -> emergency stop
- jointCtrl = _module<Devices>().getControlDevices().at(i)->getJointEmergencyStopController();
- }
+ //don't replace this controller with emergency stop
+ jointCtrl = jointCtrlOld;
+ }
+ else
+ {
+ //no one controls this device -> emergency stop
+ jointCtrl = _module<Devices>().getControlDevices().at(i)->getJointEmergencyStopController();
}
}
- ARMARX_DEBUG << "wrote requested controllers";
- ARMARX_INFO << "requested controllers:\n" << ARMARX_STREAM_PRINTER
+ }
+ ARMARX_DEBUG << "wrote requested controllers";
+ ARMARX_INFO << "requested controllers:\n" << ARMARX_STREAM_PRINTER
+ {
+ for (std::size_t i = 0; i < request.nJointControllers.size(); ++i)
{
- for (std::size_t i = 0; i < request.nJointControllers.size(); ++i)
+ const auto& ctrl = request.nJointControllers.at(i);
+ if (ctrl)
{
- const auto& ctrl = request.nJointControllers.at(i);
- if (ctrl)
- {
- out << i << "\t'" << ctrl->getInstanceName() << "' \t of class '" << ctrl->getClassName() << "'\n";
- }
+ out << i << "\t'" << ctrl->getInstanceName() << "' \t of class '" << ctrl->getClassName() << "'\n";
}
- };
- writeRequestedControllers(std::move(request));
- }
+ }
+ };
+ writeRequestedControllers(std::move(request));
+ }
- void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- _module<Devices>().updateVirtualRobotFromSensorValues(robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
- }
+ void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ _module<Devices>().updateVirtualRobotFromSensorValues(robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
+ }
- void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- _module<Devices>().updateVirtualRobotFromSensorValues(robot, controlThreadOutputBuffer.getReadBuffer().sensors);
- }
- void ControlThreadDataBuffer::_postFinishDeviceInitialization()
+ void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ _module<Devices>().updateVirtualRobotFromSensorValues(robot, controlThreadOutputBuffer.getReadBuffer().sensors);
+ }
+ void ControlThreadDataBuffer::_postFinishDeviceInitialization()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_DEBUG << "initializing buffers:";
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "initializing buffers:";
- {
- ARMARX_DEBUG << "----initializing controller buffers for " << _module<Devices>().getNumberOfControlDevices() << " control devices";
+ ARMARX_DEBUG << "----initializing controller buffers for " << _module<Devices>().getNumberOfControlDevices() << " control devices";
- JointAndNJointControllers ctrlinitReq(_module<Devices>().getNumberOfControlDevices());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointToNJointControllerAssignement.size());
- controllersActivated.reinitAllBuffers(ctrlinitReq);
+ JointAndNJointControllers ctrlinitReq(_module<Devices>().getNumberOfControlDevices());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointToNJointControllerAssignement.size());
+ controllersActivated.reinitAllBuffers(ctrlinitReq);
- ctrlinitReq.jointControllers = DevicesAttorneyForControlThreadDataBuffer::GetStopMovementJointControllers(this);
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointToNJointControllerAssignement.size());
- controllersRequested.reinitAllBuffers(ctrlinitReq);
- controllersRequested.commitWrite();//to make sure the diry bit is activated (to trigger rt switch)
- }
+ ctrlinitReq.jointControllers = DevicesAttorneyForControlThreadDataBuffer::GetStopMovementJointControllers(this);
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointToNJointControllerAssignement.size());
+ controllersRequested.reinitAllBuffers(ctrlinitReq);
+ controllersRequested.commitWrite();//to make sure the diry bit is activated (to trigger rt switch)
}
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
index bb05c426cc1766dc9caa872d8ccb3cee941306c5..66597aee2f9b7d14f597fa4fcaf0b2e533179db7 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
@@ -37,246 +37,247 @@
namespace armarx
{
class SensorValue1DoFActuatorPosition;
+}
- namespace RobotUnitModule
+namespace armarx::RobotUnitModule
+{
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" manages all communication into and out of the \ref ControlThread.
+ *
+ * \section buffers Data Buffers
+ * This class holds three data buffers
+ *
+ * \subsection bufferreq Buffer for requested controllers
+ * \note This buffer's direction is ArmarX -> \ref ControlThread
+ *
+ * This buffer contains all \ref NJointControllerBase "NJointControllers"
+ * requested by the user (e.g.: via \ref NJointControllerBase::activate) and
+ * the set of \ref JointController "JointControllers" implicitly defined by this.
+ * These controllers will be activated by \ref ControlThread::rtSwitchControllerSetup.
+ *
+ *
+ * \subsection bufferact Buffer for activated controllers
+ * \note This buffer's direction is \ref ControlThread -> ArmarX
+ *
+ * This buffer contains all \ref JointController "Joint-" and
+ * \ref NJointControllerBase "NJointControllers" executed by the \ref ControlThread.
+ *
+ *
+ * \subsection bufferout Buffer for iteration data
+ * \note This buffer's direction is \ref ControlThread -> ArmarX
+ *
+ * This buffer contains all \ref SensorValueBase "SensorValues" and \ref ControlTargetBase "ControlTargets"
+ * for each iteration.
+ * It also contains messages logged via \ref ARMARX_RT_LOGF.
+ *
+ * This data is used to update \ref Units and is reported to topics (see \ref Publish).
+ *
+ * This buffer is organized as a ringbuffer and holds several old iterations.
+ *
+ * @see ModuleBase
+ */
+ class ControlThreadDataBuffer : virtual public ModuleBase
{
+ friend class ModuleBase;
+ public:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" manages all communication into and out of the \ref ControlThread.
- *
- * \section buffers Data Buffers
- * This class holds three data buffers
- *
- * \subsection bufferreq Buffer for requested controllers
- * \note This buffer's direction is ArmarX -> \ref ControlThread
- *
- * This buffer contains all \ref NJointController "NJointControllers"
- * requested by the user (e.g.: via \ref NJointController::activate) and
- * the set of \ref JointController "JointControllers" implicitly defined by this.
- * These controllers will be activated by \ref ControlThread::rtSwitchControllerSetup.
- *
- *
- * \subsection bufferact Buffer for activated controllers
- * \note This buffer's direction is \ref ControlThread -> ArmarX
- *
- * This buffer contains all \ref JointController "Joint-" and
- * \ref NJointController "NJointControllers" executed by the \ref ControlThread.
- *
- *
- * \subsection bufferout Buffer for iteration data
- * \note This buffer's direction is \ref ControlThread -> ArmarX
- *
- * This buffer contains all \ref SensorValueBase "SensorValues" and \ref ControlTargetBase "ControlTargets"
- * for each iteration.
- * It also contains messages logged via \ref ARMARX_RT_LOGF.
- *
- * This data is used to update \ref Units and is reported to topics (see \ref Publish).
- *
- * This buffer is organized as a ringbuffer and holds several old iterations.
- *
- * @see ModuleBase
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
*/
- class ControlThreadDataBuffer : virtual public ModuleBase
+ static ControlThreadDataBuffer& Instance()
{
- friend class ModuleBase;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static ControlThreadDataBuffer& Instance()
- {
- return ModuleBase::Instance<ControlThreadDataBuffer>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_postFinishDeviceInitialization
- void _postFinishDeviceInitialization();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////// Module interface /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Updates the given VirtualRobot with the current joint values
- * @param robot The robot to update
- */
- void updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const;
- /**
- * @brief Updates the given VirtualRobot with the current joint values
- * @param robot The robot to update
- * @param nodes The robot's nodes
- */
- void updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const;
+ return ModuleBase::Instance<ControlThreadDataBuffer>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_postFinishDeviceInitialization
+ void _postFinishDeviceInitialization();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// Module interface /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Updates the given VirtualRobot with the current joint values
+ * @param robot The robot to update
+ */
+ void updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const;
+ /**
+ * @brief Updates the given VirtualRobot with the current joint values
+ * @param robot The robot to update
+ * @param nodes The robot's nodes
+ */
+ void updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const;
- /**
- * @brief Returns whether the set of activated \ref JointController "Joint-" and
- * \ref NJointController "NJointControllers" changed.
- * @return Whether the set of activated \ref JointController "Joint-" and
- * \ref NJointController "NJointControllers" changed.
- * @see ControlThread
- * @see ControlThread::rtSwitchControllerSetup
- */
- bool activatedControllersChanged() const; ///TODO private + attorney for publish
- /**
- * @brief Returns the set of activated \ref JointController "Joint-" and
- * \ref NJointController "NJointControllers".
- * @return The set of activated \ref JointController "Joint-" and
- * \ref NJointController "NJointControllers".
- * @see ControlThread
- * @see ControlThread::rtSwitchControllerSetup
- */
- JointAndNJointControllers getActivatedControllers() const;
- /**
- * @brief Returns the set of activated \ref JointController "JointControllers".
- * @return The set of activated \ref JointController "JointControllers".
- * @see ControlThread
- * @see ControlThread::rtSwitchControllerSetup
- */
- std::vector<JointController*> getActivatedJointControllers() const;
- /**
- * @brief Returns the set of activated \ref NJointController "NJointControllers".
- * @return The set of activated \ref NJointController "NJointControllers".
- * @see ControlThread
- * @see ControlThread::rtSwitchControllerSetup
- */
- std::vector<NJointController*> getActivatedNJointControllers() const;
+ /**
+ * @brief Returns whether the set of activated \ref JointController "Joint-" and
+ * \ref NJointControllerBase "NJointControllers" changed.
+ * @return Whether the set of activated \ref JointController "Joint-" and
+ * \ref NJointControllerBase "NJointControllers" changed.
+ * @see ControlThread
+ * @see ControlThread::rtSwitchControllerSetup
+ */
+ bool activatedControllersChanged() const; ///TODO private + attorney for publish
+ /**
+ * @brief Returns the set of activated \ref JointController "Joint-" and
+ * \ref NJointControllerBase "NJointControllers".
+ * @return The set of activated \ref JointController "Joint-" and
+ * \ref NJointControllerBase "NJointControllers".
+ * @see ControlThread
+ * @see ControlThread::rtSwitchControllerSetup
+ */
+ JointAndNJointControllers getActivatedControllers() const;
+ /**
+ * @brief Returns the set of activated \ref JointController "JointControllers".
+ * @return The set of activated \ref JointController "JointControllers".
+ * @see ControlThread
+ * @see ControlThread::rtSwitchControllerSetup
+ */
+ std::vector<JointController*> getActivatedJointControllers() const;
+ /**
+ * @brief Returns the set of activated \ref NJointControllerBase "NJointControllers".
+ * @return The set of activated \ref NJointControllerBase "NJointControllers".
+ * @see ControlThread
+ * @see ControlThread::rtSwitchControllerSetup
+ */
+ std::vector<NJointControllerBase*> getActivatedNJointControllers() const;
- /**
- * @brief Returns the set of requsted \ref JointController "Joint-" and
- * \ref NJointController "NJointControllers".
- * @return The set of requested \ref JointController "Joint-" and
- * \ref NJointController "NJointControllers".
- * @see ControllerManagement
- * @see ControllerManagement::activateNJointController
- * @see ControllerManagement::deactivateNJointController
- * @see ControllerManagement::switchNJointControllerSetup
- * @see NJointController::activateController
- * @see NJointController::deactivateController
- * @see setActivateControllersRequest
- */
- JointAndNJointControllers copyRequestedControllers() const;
- /**
- * @brief Returns the set of requsted \ref JointController "JointControllers".
- * @return The set of requested \ref JointController "JointControllers".
- * @see ControllerManagement
- * @see ControllerManagement::activateNJointController
- * @see ControllerManagement::deactivateNJointController
- * @see ControllerManagement::switchNJointControllerSetup
- * @see NJointController::activateController
- * @see NJointController::deactivateController
- * @see setActivateControllersRequest
- */
- std::vector<JointController*> copyRequestedJointControllers() const;
- /**
- * @brief Returns the set of requsted \ref NJointController "NJointControllers".
- * @return The set of requested \ref NJointController "NJointControllers".
- * @see ControllerManagement
- * @see ControllerManagement::activateNJointController
- * @see ControllerManagement::deactivateNJointController
- * @see ControllerManagement::switchNJointControllerSetup
- * @see NJointController::activateController
- * @see NJointController::deactivateController
- * @see setActivateControllersRequest
- */
- std::vector<NJointController*> copyRequestedNJointControllers() const;
+ /**
+ * @brief Returns the set of requsted \ref JointController "Joint-" and
+ * \ref NJointControllerBase "NJointControllers".
+ * @return The set of requested \ref JointController "Joint-" and
+ * \ref NJointControllerBase "NJointControllers".
+ * @see ControllerManagement
+ * @see ControllerManagement::activateNJointController
+ * @see ControllerManagement::deactivateNJointController
+ * @see ControllerManagement::switchNJointControllerSetup
+ * @see NJointControllerBase::activateController
+ * @see NJointControllerBase::deactivateController
+ * @see setActivateControllersRequest
+ */
+ JointAndNJointControllers copyRequestedControllers() const;
+ /**
+ * @brief Returns the set of requsted \ref JointController "JointControllers".
+ * @return The set of requested \ref JointController "JointControllers".
+ * @see ControllerManagement
+ * @see ControllerManagement::activateNJointController
+ * @see ControllerManagement::deactivateNJointController
+ * @see ControllerManagement::switchNJointControllerSetup
+ * @see NJointControllerBase::activateController
+ * @see NJointControllerBase::deactivateController
+ * @see setActivateControllersRequest
+ */
+ std::vector<JointController*> copyRequestedJointControllers() const;
+ /**
+ * @brief Returns the set of requsted \ref NJointControllerBase "NJointControllers".
+ * @return The set of requested \ref NJointControllerBase "NJointControllers".
+ * @see ControllerManagement
+ * @see ControllerManagement::activateNJointController
+ * @see ControllerManagement::deactivateNJointController
+ * @see ControllerManagement::switchNJointControllerSetup
+ * @see NJointControllerBase::activateController
+ * @see NJointControllerBase::deactivateController
+ * @see setActivateControllersRequest
+ */
+ std::vector<NJointControllerBase*> copyRequestedNJointControllers() const;
- /**
- * @brief Updates the sensor and control buffer and returns whether the buffer was updated.
- * @return Whether the buffer was updated.
- */
- bool sensorAndControlBufferChanged() const; /// TODO private + attorney for publish
- /**
- * @brief Returns the last updated sensor and control buffer.
- * @return The last updated sensor and control buffer.
- */
- const SensorAndControl& getSensorAndControlBuffer() const;
+ /**
+ * @brief Updates the sensor and control buffer and returns whether the buffer was updated.
+ * @return Whether the buffer was updated.
+ */
+ bool sensorAndControlBufferChanged() const; /// TODO private + attorney for publish
+ /**
+ * @brief Returns the last updated sensor and control buffer.
+ * @return The last updated sensor and control buffer.
+ */
+ const SensorAndControl& getSensorAndControlBuffer() const;
- /**
- * @brief Returns the \ref ControlThreadOutputBuffer
- * \warning This function is dangerous to use.
- * @return The \ref ControlThreadOutputBuffer
- */
- ControlThreadOutputBuffer& getControlThreadOutputBuffer() ///TODO refactor logging and remove this function
- {
- return controlThreadOutputBuffer;
- }
+ /**
+ * @brief Returns the \ref ControlThreadOutputBuffer
+ * \warning This function is dangerous to use.
+ * @return The \ref ControlThreadOutputBuffer
+ */
+ ControlThreadOutputBuffer& getControlThreadOutputBuffer() ///TODO refactor logging and remove this function
+ {
+ return controlThreadOutputBuffer;
+ }
- /**
- * @brief Returns the writer buffer for sensor and control values.
- * @return The writer buffer for sensor and control values.
- */
- SensorAndControl& rtGetSensorAndControlBuffer() ///TODO move to attorney
- {
- return controlThreadOutputBuffer.getWriteBuffer();
- }
- /**
- * @brief Commits the writer buffer for sensor and control values.
- */
- void rtSensorAndControlBufferCommitWrite() ///TODO move to attorney
- {
- return controlThreadOutputBuffer.commitWrite();
- }
+ /**
+ * @brief Returns the writer buffer for sensor and control values.
+ * @return The writer buffer for sensor and control values.
+ */
+ SensorAndControl& rtGetSensorAndControlBuffer() ///TODO move to attorney
+ {
+ return controlThreadOutputBuffer.getWriteBuffer();
+ }
+ /**
+ * @brief Commits the writer buffer for sensor and control values.
+ */
+ void rtSensorAndControlBufferCommitWrite() ///TODO move to attorney
+ {
+ return controlThreadOutputBuffer.commitWrite();
+ }
- /**
- * @brief Sets the set of requested \ref NJointController "NJointControllers" to \param ctrls.
- * @param ctrls \ref NJointController "NJointControllers" requested.
- * @see ControllerManagement
- * @see ControllerManagement::activateNJointController
- * @see ControllerManagement::deactivateNJointController
- * @see ControllerManagement::switchNJointControllerSetup
- * @see NJointController::activateController
- * @see NJointController::deactivateController
- */
- void setActivateControllersRequest(std::set<NJointControllerPtr, std::greater<NJointControllerPtr> > ctrls);
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////// implementation //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- /**
- * @brief This hook is called prior to writing the set of requested controllers.
- * A deriving class can override this function to perform some additional checks
- * (e.g. two controll devices may always need to have the same control mode, this can be checked here)
- * If any check fails, throw an exception with a descriptive message.
- * @param controllers The controllers to activate
- */
- virtual void checkSetOfControllersToActivate(const JointAndNJointControllers& /*controllers*/) const {}
- private:
- void writeRequestedControllers(JointAndNJointControllers&& setOfControllers);
+ /**
+ * @brief Sets the set of requested \ref NJointControllerBase "NJointControllers" to \param ctrls.
+ * @param ctrls \ref NJointControllerBase "NJointControllers" requested.
+ * @see ControllerManagement
+ * @see ControllerManagement::activateNJointController
+ * @see ControllerManagement::deactivateNJointController
+ * @see ControllerManagement::switchNJointControllerSetup
+ * @see NJointControllerBase::activateController
+ * @see NJointControllerBase::deactivateController
+ */
+ void setActivateControllersRequest(std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr> > ctrls);
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ /**
+ * @brief This hook is called prior to writing the set of requested controllers.
+ * A deriving class can override this function to perform some additional checks
+ * (e.g. two controll devices may always need to have the same control mode, this can be checked here)
+ * If any check fails, throw an exception with a descriptive message.
+ * @param controllers The controllers to activate
+ */
+ virtual void checkSetOfControllersToActivate(const JointAndNJointControllers& /*controllers*/) const {}
+ private:
+ void writeRequestedControllers(JointAndNJointControllers&& setOfControllers);
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some NJoint controllers may be null)
- /// data may only be used after State::InitializingDevices
- /// all NJointController have to be sorted by id (null controllers are at the end)
- WriteBufferedTripleBuffer<JointAndNJointControllers> controllersRequested;
- /// @brief Mutex guarding \ref controllersRequested
- mutable std::recursive_mutex controllersRequestedMutex;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some NJoint controllers may be null)
+ /// data may only be used after State::InitializingDevices
+ /// all NJointControllerBase have to be sorted by id (null controllers are at the end)
+ WriteBufferedTripleBuffer<JointAndNJointControllers> controllersRequested;
+ /// @brief Mutex guarding \ref controllersRequested
+ mutable std::recursive_mutex controllersRequestedMutex;
- /// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some NJoint controllers may be null)
- /// data may only be used after State::InitializingDevices
- /// all NJointController are sorted by id (null controllers may be anywhere)
- WriteBufferedTripleBuffer<JointAndNJointControllers> controllersActivated;
- /// @brief Mutex guarding \ref controllersActivated
- mutable std::recursive_mutex controllersActivatedMutex;
+ /// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some NJoint controllers may be null)
+ /// data may only be used after State::InitializingDevices
+ /// all NJointControllerBase are sorted by id (null controllers may be anywhere)
+ WriteBufferedTripleBuffer<JointAndNJointControllers> controllersActivated;
+ /// @brief Mutex guarding \ref controllersActivated
+ mutable std::recursive_mutex controllersActivatedMutex;
- /// @brief Transfers Data (current sensor values and control targets) from the control thread to other threads (direction RT -> nonRT)
- /// \warning DO NOT ACCESS EXCEPT YOU KNOW WHAT YOU ARE DOING!
- ControlThreadOutputBuffer controlThreadOutputBuffer;
- /// @brief Mutex guarding \ref controlThreadOutputBuffer
- mutable std::recursive_mutex controlThreadOutputBufferMutex; /// TODO use this mutex instead of the global one
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer in a sane fashion for \ref ControlThread.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class ControlThreadDataBufferAttorneyForControlThread;
- };
- }
+ /// @brief Transfers Data (current sensor values and control targets) from the control thread to other threads (direction RT -> nonRT)
+ /// \warning DO NOT ACCESS EXCEPT YOU KNOW WHAT YOU ARE DOING!
+ ControlThreadOutputBuffer controlThreadOutputBuffer;
+ /// @brief Mutex guarding \ref controlThreadOutputBuffer
+ mutable std::recursive_mutex controlThreadOutputBufferMutex; /// TODO use this mutex instead of the global one
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer in a sane fashion for \ref ControlThread.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class ControlThreadDataBufferAttorneyForControlThread;
+ };
}
+
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index d528147fd9aa58d42da2f25bbcfb3a6f41ad1f80..1b7d7f76a00bd84be656b5a3bd29bad5ad8d5e4c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -25,719 +25,713 @@
#include "RobotUnitModuleControllerManagement.h"
#include "../NJointControllers/NJointController.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ template<class Cont>
+ static Ice::StringSeq GetNonNullNames(const Cont& c)
{
- template<class Cont>
- static Ice::StringSeq GetNonNullNames(const Cont& c)
+ Ice::StringSeq result;
+ result.reserve(c.size());
+ for (const auto& e : c)
{
- Ice::StringSeq result;
- result.reserve(c.size());
- for (const auto& e : c)
+ if (e)
{
- if (e)
- {
- result.emplace_back(e->getName());
- }
+ result.emplace_back(e->getName());
}
- return result;
}
+ return result;
+ }
- /**
- * \brief This class allows minimal access to private members of \ref NJointController 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
+ /**
+ * \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)
{
- friend class ControllerManagement;
- static void SetRequested(const NJointControllerPtr& nJointCtrl, bool requested)
- {
- nJointCtrl->isRequested = requested;
- }
- };
- }
+ nJointCtrl->isRequested = requested;
+ }
+ };
}
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ Ice::StringSeq ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
{
- Ice::StringSeq ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return GetNonNullNames(_module<ControlThreadDataBuffer>().copyRequestedNJointControllers());
- }
+ 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());
- }
+ 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
+ void ControllerManagement::checkNJointControllerClassName(const std::string& className) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (!NJointControllerRegistry::has(className))
{
- 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 via loadLibFromPath(path) or loadLibFromPackage(package, lib))";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
+ std::stringstream ss;
+ ss << "Requested controller class '" << className << "' unknown! Known classes:" << NJointControllerRegistry::getKeys()
+ << " (If this class exists in a different lib then load it via loadLibFromPath(path) or loadLibFromPackage(package, lib))";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
}
+ }
- std::vector<NJointControllerPtr> ControllerManagement::getNJointControllersNotNull(const std::vector<std::string>& names) const
+ 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)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- std::vector<NJointControllerPtr> ctrl;
- ctrl.reserve(names.size());
- for (const auto& name : names)
- {
- ctrl.emplace_back(getNJointControllerNotNull(name));
- }
- return ctrl;
- }
-
- const NJointControllerPtr& 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 NJointController with name '" << name
- << "'. Existing NJointControllers are: " << getNJointControllerNames();
- throw InvalidArgumentException {ss.str()};
- }
- if (!it->second)
- {
- std::stringstream ss;
- ss << "RobotUnit: The NJointController 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;
+ ctrl.emplace_back(getNJointControllerNotNull(name));
}
+ return ctrl;
+ }
- void ControllerManagement::deleteNJointController(const NJointControllerPtr& ctrl)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deleteNJointControllers(std::vector<NJointControllerPtr> {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;
+ }
- StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::map<std::string, NJointControllerPtr> 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;
- }
+ void ControllerManagement::deleteNJointController(const NJointControllerBasePtr& ctrl)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ deleteNJointControllers(std::vector<NJointControllerBasePtr> {ctrl});
+ }
- NJointControllerInterfacePrx ControllerManagement::createNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- const Ice::Current&)
+ StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //no lock required
- return NJointControllerInterfacePrx::uncheckedCast(
- createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
+ auto guard = getGuard();
+ //copy to keep lock retention time low
+ nJointControllersCopy = nJointControllers;
}
-
- NJointControllerInterfacePrx ControllerManagement::createNJointControllerFromVariantConfig(
- const std::string& className,
- const std::string& instanceName,
- const StringVariantBaseMap& variants,
- const Ice::Current&)
+ StringNJointControllerPrxDictionary result;
+ for (const auto& pair : nJointControllersCopy)
{
- 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*/);
+ result[pair.first] = NJointControllerInterfacePrx::uncheckedCast(pair.second->getProxy(-1, true));
}
+ return result;
+ }
- const NJointControllerPtr& 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);
+ 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));
+ }
- //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()};
- }
+ 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*/);
+ }
- //create the controller
- ARMARX_CHECK_EXPRESSION(factory);
- NJointControllerPtr nJointCtrl = factory->create(
- this,
- config,
- controllerCreateRobot,
- deletable, internal,
- instanceName
+ 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_W_HINT(
- nJointCtrl->getControlDeviceUsedIndices().size(), 0,
- "The NJointController '" << 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);
- }
+ ARMARX_CHECK_NOT_EQUAL_W_HINT(
+ 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);
- const bool result = getArmarXManager()->loadLibFromPath(path);
- ARMARX_INFO << "loadLibFromPath('" << path << "') -> " << result;
- return result;
- }
+ bool ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ 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);
- const bool result = getArmarXManager()->loadLibFromPackage(package, lib);
- ARMARX_INFO << "loadLibFromPackage('" << package << "', '" << lib << "') -> " << result;
- return result;
- }
+ bool ControllerManagement::loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current&)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ 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::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<NJointControllerPtr>& ctrls) const
+ 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)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::vector<std::string> result;
- result.reserve(ctrls.size());
- for (const auto& ctrl : ctrls)
+ if (ctrl)
{
- if (ctrl)
- {
- result.emplace_back(ctrl->getInstanceName());
- }
+ result.emplace_back(ctrl->getInstanceName());
}
- return result;
}
+ 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 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();
}
- void ControllerManagement::activateNJointController(const NJointControllerPtr& ctrl)
+ )
+ )
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- activateNJointControllers(std::vector<NJointControllerPtr> {ctrl});
+ return;
}
- void ControllerManagement::activateNJointControllers(const std::vector<NJointControllerPtr>& ctrlsToActVec)
+ //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
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- if (ctrlsToActVec.empty())
+ auto r = NJointControllerBase::AreNotInConflict(ctrlsToAct.begin(), ctrlsToAct.end());
+ if (!r)
{
- return;
- }
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- //if not activate them
- std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> 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 NJointControllerPtr & ctrl)
- {
- return ctrl->isControllerActive();
+ 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.get());
+ }
+ ARMARX_DEBUG << "all requested controllers are conflict free" << std::flush;
+ auto printInUse = ARMARX_STREAM_PRINTER
+ {
+ for (const auto c : inuse)
{
- return;
+ out << (c ? 1 : 0);
}
- //get already requested
- const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
- std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> ctrlsAlreadyRequested {ctrlVector.begin(), ctrlVector.end()};
- ctrlsAlreadyRequested.erase(nullptr);
- //check for conflict
- std::vector<char> inuse;
- //check requested controllers
+ };
+ ARMARX_DEBUG << "inuse field (request)\n" << printInUse;
+ //add already active controllers if they are conflict free
+ {
+ if (ctrlsAlreadyRequested.empty())
{
- auto r = NJointController::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.get());
+ ARMARX_DEBUG << "no already requested NJointControllers";
}
- ARMARX_DEBUG << "all requested controllers are conflict free" << std::flush;
- auto printInUse = ARMARX_STREAM_PRINTER
+ for (const NJointControllerBasePtr& nJointCtrl : ctrlsAlreadyRequested)
{
- for (const auto c : inuse)
+ if (ctrlsToAct.count(nJointCtrl))
{
- out << (c ? 1 : 0);
+ continue;
}
- };
- ARMARX_DEBUG << "inuse field (request)\n" << printInUse;
- //add already active controllers if they are conflict free
- {
- if (ctrlsAlreadyRequested.empty())
+ auto r = nJointCtrl->isNotInConflictWith(inuse);
+ if (r)
{
- ARMARX_DEBUG << "no already requested NJointControllers";
+ ARMARX_DEBUG << "keeping already requested NJointControllerBase '"
+ << nJointCtrl->getInstanceName() << "' in list of requested controllers";
+ ctrlsToAct.insert(nJointCtrl);
+ inuse = std::move(r.get());
}
- for (const NJointControllerPtr& nJointCtrl : ctrlsAlreadyRequested)
+ else
{
- if (ctrlsToAct.count(nJointCtrl))
- {
- continue;
- }
- auto r = nJointCtrl->isNotInConflictWith(inuse);
- if (r)
- {
- ARMARX_DEBUG << "keeping already requested NJointController '"
- << nJointCtrl->getInstanceName() << "' in list of requested controllers";
- ctrlsToAct.insert(nJointCtrl);
- inuse = std::move(r.get());
- }
- else
- {
- ARMARX_INFO << "removing already requested NJointController '"
- << nJointCtrl->getInstanceName() << "' from list of requested controllers";
- }
+ ARMARX_INFO << "removing already requested NJointControllerBase '"
+ << nJointCtrl->getInstanceName() << "' from list of requested controllers";
}
- ARMARX_DEBUG << "inuse field (all)\n" << printInUse;
}
- _module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrlsToAct);
+ ARMARX_DEBUG << "inuse field (all)\n" << printInUse;
}
+ _module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrlsToAct);
+ }
- void ControllerManagement::deactivateNJointController(const std::string& name, const Ice::Current&)
+ 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())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateNJointControllers(getNJointControllersNotNull({name}));
+ return;
}
- void ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+ 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)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateNJointControllers(getNJointControllersNotNull(names));
+ ctrls.erase(nJointCtrlToDeactivate);
}
- void ControllerManagement::deactivateNJointController(const NJointControllerPtr& ctrl)
+ if (ctrls.size() == ctrlsNum)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateNJointControllers(std::vector<NJointControllerPtr> {ctrl});
- }
- void ControllerManagement::deactivateNJointControllers(const std::vector<NJointControllerPtr>& ctrlsDeacVec)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- if (ctrlsDeacVec.empty())
- {
- return;
- }
- const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
- std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> 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);
+ 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&)
+ 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())
{
- 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()});
+ return;
}
- void ControllerManagement::deleteNJointControllers(const std::vector<NJointControllerPtr>& ctrlsToDelVec)
+ //check if all can be deleted
+ for (const auto& nJointCtrl : 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())
{
- if (!nJointCtrl->isDeletable())
+ throw LogicError
{
- throw LogicError
- {
- "The NJointController '" + nJointCtrl->getInstanceName() +
- "' can't be deleted since this operation is not allowed for this controller! (no NJointController was deleted)"
- };
- }
- if (nJointCtrl->isControllerActive() || nJointCtrl->isControllerRequested())
- {
- throw LogicError
- {
- "The NJointController '" + nJointCtrl->getInstanceName() +
- "' can't be deleted since it is active or requested! (no NJointController was deleted)"
- };
- }
+ "The NJointControllerBase '" + nJointCtrl->getInstanceName() +
+ "' can't be deleted since this operation is not allowed for this controller! (no NJointControllerBase was deleted)"
+ };
}
- for (const auto& nJointCtrl : ctrlsToDelVec)
+ if (nJointCtrl->isControllerActive() || nJointCtrl->isControllerRequested())
{
- 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 NJointController '" << name << "' to be deleted";
+ throw LogicError
+ {
+ "The NJointControllerBase '" + nJointCtrl->getInstanceName() +
+ "' can't be deleted since it is active or requested! (no NJointControllerBase was deleted)"
+ };
}
}
-
- void ControllerManagement::deactivateAndDeleteNJointController(const std::string& name, const Ice::Current&)
+ for (const auto& nJointCtrl : ctrlsToDelVec)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateAndDeleteNJointControllers(getNJointControllersNotNull({name}));
+ 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::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateAndDeleteNJointControllers(getNJointControllersNotNull(names));
- }
- void ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerPtr& ctrl)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateAndDeleteNJointControllers({ctrl});
+ }
+
+ 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();
}
- void ControllerManagement::deactivateAndDeleteNJointControllers(const std::vector<NJointControllerPtr>& ctrlsToDelVec)
+ )
+ )
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- if (ctrlsToDelVec.empty())
+ if (isShuttingDown())
{
return;
}
- deactivateNJointControllers(ctrlsToDelVec);
- while (
- std::any_of(
- ctrlsToDelVec.begin(),
- ctrlsToDelVec.end(),
- [](const NJointControllerPtr & ctrl)
- {
- return ctrl->isControllerActive();
- }
- )
- )
- {
- if (isShuttingDown())
- {
- return;
- }
- std::this_thread::sleep_for(std::chrono::microseconds {100});
- }
- deleteNJointControllers(ctrlsToDelVec);
+ std::this_thread::sleep_for(std::chrono::microseconds {100});
}
+ deleteNJointControllers(ctrlsToDelVec);
+ }
- NJointControllerClassDescription ControllerManagement::getNJointControllerClassDescription(
- const std::string& className, const Ice::Current&) const
+ 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)
{
- 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())
+ //this phase should only last short so busy waiting is ok
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ if (! --tries)
{
- data.configDescription =
- NJointControllerRegistry::get(className)->GenerateConfigDescription(
- controllerCreateRobot,
- _module<Devices>().getControlDevicesConstPtr(),
- _module<Devices>().getSensorDevicesConstPtr()
- );
+ throw RuntimeError {"RobotUnit::getNJointControllerClassDescriptions: it took too long to for the unit to get in a valid state"};
}
- return data;
}
+ auto guard = getGuard();
+ NJointControllerClassDescriptionSeq r;
+ r.reserve(NJointControllerRegistry::getKeys().size());
+ for (const auto& key : NJointControllerRegistry::getKeys())
+ {
+ r.emplace_back(getNJointControllerClassDescription(key));
+ }
+ return r;
+ }
- NJointControllerClassDescriptionSeq ControllerManagement::getNJointControllerClassDescriptions(const Ice::Current&) const
+ NJointControllerInterfacePrx ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ NJointControllerBasePtr ctrl;
{
- 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())
+ auto it = nJointControllers.find(name);
+ if (it == nJointControllers.end())
{
- r.emplace_back(getNJointControllerClassDescription(key));
+ return nullptr;
}
- return r;
+ 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();
+ }
- NJointControllerInterfacePrx ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
+ NJointControllerStatusSeq ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ auto guard = getGuard();
+ if (!areDevicesReady())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- NJointControllerPtr 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));
+ return {};
+ }
+ NJointControllerStatusSeq r;
+ r.reserve(nJointControllers.size());
+ for (const auto& nJointCtrl : nJointControllers)
+ {
+ r.emplace_back(nJointCtrl.second->getControllerStatus());
}
+ return r;
+ }
- NJointControllerStatus ControllerManagement::getNJointControllerStatus(const std::string& name, const Ice::Current&) const
+ NJointControllerDescription ControllerManagement::getNJointControllerDescription(const std::string& name, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ NJointControllerBasePtr nJointCtrl;
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
- return getNJointControllerNotNull(name)->getControllerStatus();
+ nJointCtrl = getNJointControllerNotNull(name);
}
+ return nJointCtrl->getControllerDescription();
+ }
- NJointControllerStatusSeq ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
+ NJointControllerDescriptionSeq ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
{
- 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;
+ nJointControllersCopy = nJointControllers;
}
-
- NJointControllerDescription ControllerManagement::getNJointControllerDescription(const std::string& name, const Ice::Current&) const
+ NJointControllerDescriptionSeq r;
+ r.reserve(nJointControllersCopy.size());
+ for (const auto& nJointCtrl : nJointControllersCopy)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- NJointControllerPtr nJointCtrl;
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- nJointCtrl = getNJointControllerNotNull(name);
- }
- return nJointCtrl->getControllerDescription();
+ r.emplace_back(nJointCtrl.second->getControllerDescription());
}
+ return r;
+ }
- NJointControllerDescriptionSeq ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
+ NJointControllerDescriptionWithStatus ControllerManagement::getNJointControllerDescriptionWithStatus(
+ const std::string& name, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ NJointControllerBasePtr nJointCtrl;
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::map<std::string, NJointControllerPtr> 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;
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ nJointCtrl = getNJointControllerNotNull(name);
}
+ return nJointCtrl->getControllerDescriptionWithStatus();
+ }
- NJointControllerDescriptionWithStatus ControllerManagement::getNJointControllerDescriptionWithStatus(
- const std::string& name, const Ice::Current&) const
+ NJointControllerDescriptionWithStatusSeq ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- NJointControllerPtr nJointCtrl;
+ auto guard = getGuard();
+ if (!areDevicesReady())
{
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- nJointCtrl = getNJointControllerNotNull(name);
+ return {};
}
- return nJointCtrl->getControllerDescriptionWithStatus();
+ nJointControllersCopy = nJointControllers;
}
+ NJointControllerDescriptionWithStatusSeq r;
+ r.reserve(nJointControllersCopy.size());
+ for (const auto& nJointCtrl : nJointControllersCopy)
+ {
+ r.emplace_back(nJointCtrl.second->getControllerDescriptionWithStatus());
+ }
+ return r;
+ }
- NJointControllerDescriptionWithStatusSeq ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
+ void ControllerManagement::removeNJointControllers(std::map<std::string, NJointControllerBasePtr>& ctrls, bool blocking, RobotUnitListenerPrx l)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ for (auto& n2NJointCtrl : ctrls)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::map<std::string, NJointControllerPtr> nJointControllersCopy;
+ NJointControllerBasePtr& nJointCtrl = n2NJointCtrl.second;
+ if (blocking)
{
- auto guard = getGuard();
- if (!areDevicesReady())
- {
- return {};
- }
- nJointControllersCopy = nJointControllers;
+ ARMARX_VERBOSE << "deleted NJointControllerBase " << n2NJointCtrl.first;
+ getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
}
- NJointControllerDescriptionWithStatusSeq r;
- r.reserve(nJointControllersCopy.size());
- for (const auto& nJointCtrl : nJointControllersCopy)
+ else
{
- r.emplace_back(nJointCtrl.second->getControllerDescriptionWithStatus());
+ ARMARX_VERBOSE << "deleted NJointControllerBase " << n2NJointCtrl.first;
+ getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
}
- return r;
- }
-
- void ControllerManagement::removeNJointControllers(std::map<std::string, NJointControllerPtr>& ctrls, bool blocking, RobotUnitListenerPrx l)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- for (auto& n2NJointCtrl : ctrls)
+ if (l)
{
- NJointControllerPtr& nJointCtrl = n2NJointCtrl.second;
- if (blocking)
- {
- ARMARX_VERBOSE << "deleted NJointController " << n2NJointCtrl.first;
- getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
- }
- else
- {
- ARMARX_VERBOSE << "deleted NJointController " << n2NJointCtrl.first;
- getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
- }
- if (l)
- {
- l->nJointControllerDeleted(n2NJointCtrl.first);
- }
+ l->nJointControllerDeleted(n2NJointCtrl.first);
}
- ctrls.clear();
}
+ ctrls.clear();
+ }
- void ControllerManagement::removeNJointControllersToBeDeleted(bool blocking, RobotUnitListenerPrx l)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- removeNJointControllers(nJointControllersToBeDeleted, blocking, l);
- }
+ 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::_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();
- }
+ void ControllerManagement::_postFinishRunning()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ nJointControllers.clear();
+ }
- void ControllerManagement::_preOnInitRobotUnit()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- controllerCreateRobot = _module<RobotData>().cloneRobot();
- }
+ void ControllerManagement::_preOnInitRobotUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ controllerCreateRobot = _module<RobotData>().cloneRobot();
+ }
- void ControllerManagement::updateNJointControllerRequestedState(const std::set<NJointControllerPtr>& request)
+ 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)
{
- 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));
- }
+ NJointControllerAttorneyForControllerManagement::SetRequested(name2NJoint.second, request.count(name2NJoint.second));
}
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
index 421e037962cffeb5a950cdae7c85d6cd732523c6..5df121b158490cfa80cc42f28ae87fb9302fb185 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
@@ -28,424 +28,421 @@
#include "RobotUnitModuleBase.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" manages \ref NJointControllerBase "NJointControllers".
+ *
+ * @see ModuleBase
+ */
+ class ControllerManagement : virtual public ModuleBase, virtual public RobotUnitControllerManagementInterface
{
+ friend class ModuleBase;
+ public:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" manages \ref NJointController "NJointControllers".
- *
- * @see ModuleBase
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
*/
- class ControllerManagement : virtual public ModuleBase, virtual public RobotUnitControllerManagementInterface
+ static ControllerManagement& Instance()
{
- friend class ModuleBase;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static ControllerManagement& Instance()
- {
- return ModuleBase::Instance<ControllerManagement>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_preOnInitRobotUnit
- void _preOnInitRobotUnit();
- /// @see ModuleBase::_preFinishRunning
- void _preFinishRunning();
- /// @see ModuleBase::_postFinishRunning
- void _postFinishRunning();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// ice interface //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Returns a proxy to the \ref NJointController.
- * @param name The \ref NJointController's name.
- * @return A proxy to the \ref NJointController.
- * @see getAllNJointControllers
- */
- NJointControllerInterfacePrx getNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns proxies to all \ref NJointController "NJointControllers"
- * @return Proxies to all \ref NJointController "NJointControllers"
- * @see getNJointController
- */
- StringNJointControllerPrxDictionary getAllNJointControllers(const Ice::Current& = Ice::emptyCurrent) const override;
+ return ModuleBase::Instance<ControllerManagement>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_preOnInitRobotUnit
+ void _preOnInitRobotUnit();
+ /// @see ModuleBase::_preFinishRunning
+ void _preFinishRunning();
+ /// @see ModuleBase::_postFinishRunning
+ void _postFinishRunning();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns a proxy to the \ref NJointControllerBase.
+ * @param name The \ref NJointControllerBase's name.
+ * @return A proxy to the \ref NJointControllerBase.
+ * @see getAllNJointControllers
+ */
+ NJointControllerInterfacePrx getNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns proxies to all \ref NJointControllerBase "NJointControllers"
+ * @return Proxies to all \ref NJointControllerBase "NJointControllers"
+ * @see getNJointController
+ */
+ StringNJointControllerPrxDictionary getAllNJointControllers(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the status of the \ref NJointController.
- * @param name The \ref NJointController's name.
- * @return The status of the \ref NJointController.
- * @see NJointControllerStatus
- * @see getNJointControllerStatuses
- * @see getNJointControllerDescriptionWithStatus
- * @see getNJointControllerDescriptionsWithStatuses
- */
- NJointControllerStatus getNJointControllerStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the status of all \ref NJointController "NJointControllers".
- * @return The status of all \ref NJointController "NJointControllers".
- * @see NJointControllerStatus
- * @see getNJointControllerStatus
- * @see getNJointControllerDescriptionWithStatus
- * @see getNJointControllerDescriptionsWithStatuses
- */
- NJointControllerStatusSeq getNJointControllerStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the status of the \ref NJointControllerBase.
+ * @param name The \ref NJointControllerBase's name.
+ * @return The status of the \ref NJointControllerBase.
+ * @see NJointControllerStatus
+ * @see getNJointControllerStatuses
+ * @see getNJointControllerDescriptionWithStatus
+ * @see getNJointControllerDescriptionsWithStatuses
+ */
+ NJointControllerStatus getNJointControllerStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the status of all \ref NJointControllerBase "NJointControllers".
+ * @return The status of all \ref NJointControllerBase "NJointControllers".
+ * @see NJointControllerStatus
+ * @see getNJointControllerStatus
+ * @see getNJointControllerDescriptionWithStatus
+ * @see getNJointControllerDescriptionsWithStatuses
+ */
+ NJointControllerStatusSeq getNJointControllerStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the description of the \ref NJointController.
- * @param name The \ref NJointController's name.
- * @return The description of the \ref NJointController.
- * @see NJointControllerDescription
- * @see getNJointControllerDescriptions
- * @see getNJointControllerDescriptionWithStatus
- * @see getNJointControllerDescriptionsWithStatuses
- */
- NJointControllerDescription getNJointControllerDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the description of all \ref NJointController "NJointControllers".
- * @return The description of all \ref NJointController "NJointControllers".
- * @see NJointControllerDescription
- * @see getNJointControllerDescription
- * @see getNJointControllerDescriptionWithStatus
- * @see getNJointControllerDescriptionsWithStatuses
- */
+ /**
+ * @brief Returns the description of the \ref NJointControllerBase.
+ * @param name The \ref NJointControllerBase's name.
+ * @return The description of the \ref NJointControllerBase.
+ * @see NJointControllerDescription
+ * @see getNJointControllerDescriptions
+ * @see getNJointControllerDescriptionWithStatus
+ * @see getNJointControllerDescriptionsWithStatuses
+ */
+ NJointControllerDescription getNJointControllerDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the description of all \ref NJointControllerBase "NJointControllers".
+ * @return The description of all \ref NJointControllerBase "NJointControllers".
+ * @see NJointControllerDescription
+ * @see getNJointControllerDescription
+ * @see getNJointControllerDescriptionWithStatus
+ * @see getNJointControllerDescriptionsWithStatuses
+ */
- NJointControllerDescriptionSeq getNJointControllerDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerDescriptionSeq getNJointControllerDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the status and description of the \ref NJointController.
- * @param name The \ref NJointController's name.
- * @return The status and description of the \ref NJointController.
- * @see getNJointControllerDescriptionsWithStatuses
- * @see NJointControllerStatus
- * @see getNJointControllerStatus
- * @see getNJointControllerStatuses
- * @see NJointControllerDescription
- * @see getNJointControllerDescription
- * @see getNJointControllerDescriptions
- */
- NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(
- const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the status and description of all \ref NJointController "NJointControllers".
- * @return The status and description of all \ref NJointController "NJointControllers".
- * @see getNJointControllerDescriptionsWithStatus
- * @see NJointControllerStatus
- * @see getNJointControllerStatus
- * @see getNJointControllerStatuses
- * @see NJointControllerDescription
- * @see getNJointControllerDescription
- * @see getNJointControllerDescriptions
- */
- NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the status and description of the \ref NJointControllerBase.
+ * @param name The \ref NJointControllerBase's name.
+ * @return The status and description of the \ref NJointControllerBase.
+ * @see getNJointControllerDescriptionsWithStatuses
+ * @see NJointControllerStatus
+ * @see getNJointControllerStatus
+ * @see getNJointControllerStatuses
+ * @see NJointControllerDescription
+ * @see getNJointControllerDescription
+ * @see getNJointControllerDescriptions
+ */
+ NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(
+ const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the status and description of all \ref NJointControllerBase "NJointControllers".
+ * @return The status and description of all \ref NJointControllerBase "NJointControllers".
+ * @see getNJointControllerDescriptionsWithStatus
+ * @see NJointControllerStatus
+ * @see getNJointControllerStatus
+ * @see getNJointControllerStatuses
+ * @see NJointControllerDescription
+ * @see getNJointControllerDescription
+ * @see getNJointControllerDescriptions
+ */
+ NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief getNJointControllerClassDescription
- * @param className
- * @return
- */
- NJointControllerClassDescription getNJointControllerClassDescription(
- const std::string& className, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief getNJointControllerClassDescriptions
- * @return
- */
- NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(path)`)
- * @param path Path to the lib to load.
- * @return Whether loading the lib was successful.
- * @see ArmarXManager::loadLibFromPath
- */
- bool loadLibFromPath(const std::string& path, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(package, lib)`)
- * @param package The armarx package containing the lib
- * @param lib The lib name to load.
- * @return Whether loading the lib was successful.
- * @see ArmarXManager::loadLibFromPackage
- */
- bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Returns the names of all available classes of \ref NJointController.
- * @return The names of all available classes of \ref NJointController.
- */
- Ice::StringSeq getNJointControllerClassNames(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the names of all \ref NJointController "NJointControllers"
- * @return The names of all \ref NJointController "NJointControllers"
- */
- Ice::StringSeq getNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the names of all requested \ref NJointController "NJointControllers"
- * @return The names of all requested \ref NJointController "NJointControllers"
- */
- Ice::StringSeq getRequestedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the names of all activated \ref NJointController "NJointControllers"
- * @return The names of all activated \ref NJointController "NJointControllers"
- */
- Ice::StringSeq getActivatedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief getNJointControllerClassDescription
+ * @param className
+ * @return
+ */
+ NJointControllerClassDescription getNJointControllerClassDescription(
+ const std::string& className, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief getNJointControllerClassDescriptions
+ * @return
+ */
+ NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(path)`)
+ * @param path Path to the lib to load.
+ * @return Whether loading the lib was successful.
+ * @see ArmarXManager::loadLibFromPath
+ */
+ bool loadLibFromPath(const std::string& path, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(package, lib)`)
+ * @param package The armarx package containing the lib
+ * @param lib The lib name to load.
+ * @return Whether loading the lib was successful.
+ * @see ArmarXManager::loadLibFromPackage
+ */
+ bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Returns the names of all available classes of \ref NJointControllerBase.
+ * @return The names of all available classes of \ref NJointControllerBase.
+ */
+ Ice::StringSeq getNJointControllerClassNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the names of all \ref NJointControllerBase "NJointControllers"
+ * @return The names of all \ref NJointControllerBase "NJointControllers"
+ */
+ Ice::StringSeq getNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the names of all requested \ref NJointControllerBase "NJointControllers"
+ * @return The names of all requested \ref NJointControllerBase "NJointControllers"
+ */
+ Ice::StringSeq getRequestedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the names of all activated \ref NJointControllerBase "NJointControllers"
+ * @return The names of all activated \ref NJointControllerBase "NJointControllers"
+ */
+ Ice::StringSeq getActivatedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Queues the given \ref NJointController for deletion.
- * @param name The \ref NJointController to delete.
- * @see removeNJointControllersToBeDeleted
- * @see nJointControllersToBeDeleted
- * @see deleteNJointControllers
- */
- void deleteNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Queues the given \ref NJointController "NJointControllers" for deletion.
- * @param names The \ref NJointController "NJointControllers" to delete.
- * @see removeNJointControllersToBeDeleted
- * @see nJointControllersToBeDeleted
- * @see deleteNJointController
- */
- void deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Queues the given \ref NJointController for deletion and deactivates it if necessary.
- * @param name The \ref NJointController to delete.
- * @see removeNJointControllersToBeDeleted
- * @see nJointControllersToBeDeleted
- * @see deleteNJointController
- * @see deleteNJointControllers
- * @see deactivateAnddeleteNJointControllers
- */
- void deactivateAndDeleteNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Queues the given \ref NJointController "NJointControllers" for deletion and deactivates them if necessary.
- * @param names The \ref NJointController "NJointControllers" to delete.
- * @see removeNJointControllersToBeDeleted
- * @see nJointControllersToBeDeleted
- * @see deleteNJointController
- * @see deleteNJointControllers
- * @see deactivateAnddeleteNJointController
- */
- void deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override;
+ /**
+ * @brief Queues the given \ref NJointControllerBase for deletion.
+ * @param name The \ref NJointControllerBase to delete.
+ * @see removeNJointControllersToBeDeleted
+ * @see nJointControllersToBeDeleted
+ * @see deleteNJointControllers
+ */
+ void deleteNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Queues the given \ref NJointControllerBase "NJointControllers" for deletion.
+ * @param names The \ref NJointControllerBase "NJointControllers" to delete.
+ * @see removeNJointControllersToBeDeleted
+ * @see nJointControllersToBeDeleted
+ * @see deleteNJointController
+ */
+ void deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Queues the given \ref NJointControllerBase for deletion and deactivates it if necessary.
+ * @param name The \ref NJointControllerBase to delete.
+ * @see removeNJointControllersToBeDeleted
+ * @see nJointControllersToBeDeleted
+ * @see deleteNJointController
+ * @see deleteNJointControllers
+ * @see deactivateAnddeleteNJointControllers
+ */
+ void deactivateAndDeleteNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Queues the given \ref NJointControllerBase "NJointControllers" for deletion and deactivates them if necessary.
+ * @param names The \ref NJointControllerBase "NJointControllers" to delete.
+ * @see removeNJointControllersToBeDeleted
+ * @see nJointControllersToBeDeleted
+ * @see deleteNJointController
+ * @see deleteNJointControllers
+ * @see deactivateAnddeleteNJointController
+ */
+ void deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override;
- /**
- * @brief Requests activation for the given \ref NJointController.
- * @param name The requested \ref NJointController.
- * @see activateNJointControllers
- */
- void activateNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Requests activation for the given \ref NJointController "NJointControllers".
- * @param names The requested \ref NJointController "NJointControllers".
- * @see activateNJointController
- */
- void activateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Requests activation for the given \ref NJointControllerBase.
+ * @param name The requested \ref NJointControllerBase.
+ * @see activateNJointControllers
+ */
+ void activateNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Requests activation for the given \ref NJointControllerBase "NJointControllers".
+ * @param names The requested \ref NJointControllerBase "NJointControllers".
+ * @see activateNJointController
+ */
+ void activateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Requests deactivation for the given \ref NJointController.
- * @param name The \ref NJointController to be deactivated.
- * @see deactivateNJointControllers
- */
- void deactivateNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Requests deactivation for the given \ref NJointController "NJointControllers".
- * @param names The \ref NJointController "NJointControllers" to be deactivated.
- * @see deactivateNJointController
- */
- void deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Requests deactivation for the given \ref NJointControllerBase.
+ * @param name The \ref NJointControllerBase to be deactivated.
+ * @see deactivateNJointControllers
+ */
+ void deactivateNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Requests deactivation for the given \ref NJointControllerBase "NJointControllers".
+ * @param names The \ref NJointControllerBase "NJointControllers" to be deactivated.
+ * @see deactivateNJointController
+ */
+ void deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Cretes a \ref NJointController.
- * @param className The \ref NJointController's class.
- * @param instanceName The \ref NJointController's name.
- * @param config A config passed to the \ref NJointController's ctor.
- * @return A proxy to the created \ref NJointController.
- */
- NJointControllerInterfacePrx createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Cretes a \ref NJointController.
- * @param className The \ref NJointController's class.
- * @param instanceName The \ref NJointController's name.
- * @param variants A map of \ref Variant "Variants" passed to the \ref NJointController's ctor.
- * @return A proxy to the created \ref NJointController.
- */
- NJointControllerInterfacePrx createNJointControllerFromVariantConfig(
- const std::string& className, const std::string& instanceName,
- const StringVariantBaseMap& variants, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Cretes a \ref NJointControllerBase.
+ * @param className The \ref NJointControllerBase's class.
+ * @param instanceName The \ref NJointControllerBase's name.
+ * @param config A config passed to the \ref NJointControllerBase's ctor.
+ * @return A proxy to the created \ref NJointControllerBase.
+ */
+ NJointControllerInterfacePrx createNJointController(
+ const std::string& className, const std::string& instanceName,
+ const NJointControllerConfigPtr& config, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Cretes a \ref NJointControllerBase.
+ * @param className The \ref NJointControllerBase's class.
+ * @param instanceName The \ref NJointControllerBase's name.
+ * @param variants A map of \ref Variant "Variants" passed to the \ref NJointControllerBase's ctor.
+ * @return A proxy to the created \ref NJointControllerBase.
+ */
+ NJointControllerInterfacePrx createNJointControllerFromVariantConfig(
+ const std::string& className, const std::string& instanceName,
+ const StringVariantBaseMap& variants, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Changes the set of requested \ref NJointController "NJointControllers" to the given set.
- * @param newSetup The new set of requested \ref NJointController "NJointControllers"
- */
- void switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current& = Ice::emptyCurrent) override;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////// Module interface /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Cretes a \ref NJointController.
- * @param className The \ref NJointController's class.
- * @param instanceName The \ref NJointController's name.
- * @param config A config passed to the \ref NJointController's ctor.
- * @param deletable Whether the \ref NJointController can be deleted.
- * @param internal Whether the \ref NJointController should be tagged as internal.
- * @return A pointer to the created \ref NJointController
- */
- const NJointControllerPtr& createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, bool deletable, bool internal);
+ /**
+ * @brief Changes the set of requested \ref NJointControllerBase "NJointControllers" to the given set.
+ * @param newSetup The new set of requested \ref NJointControllerBase "NJointControllers"
+ */
+ void switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current& = Ice::emptyCurrent) override;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// Module interface /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Cretes a \ref NJointControllerBase.
+ * @param className The \ref NJointControllerBase's class.
+ * @param instanceName The \ref NJointControllerBase's name.
+ * @param config A config passed to the \ref NJointControllerBase's ctor.
+ * @param deletable Whether the \ref NJointControllerBase can be deleted.
+ * @param internal Whether the \ref NJointControllerBase should be tagged as internal.
+ * @return A pointer to the created \ref NJointControllerBase
+ */
+ const NJointControllerBasePtr& createNJointController(
+ const std::string& className, const std::string& instanceName,
+ const NJointControllerConfigPtr& config, bool deletable, bool internal);
- /**
- * @brief Returns pointers to the \ref NJointController "NJointControllers". (If one does not exist, an exception is thrown.)
- * @param names The \ref NJointController "NJointControllers" names.
- * @return Pointers to the \ref NJointController "NJointControllers".
- * @throw If there is no \ref NJointcontroller for \param name
- */
- std::vector<armarx::NJointControllerPtr> getNJointControllersNotNull(const std::vector<std::string>& names) const;
- /**
- * @brief Returns a pointer to the \ref NJointController. (If it does not exist, an exception is thrown.)
- * @param name The \ref NJointController
- * @return A pointer to the \ref NJointController.
- * @throw If there is no \ref NJointcontroller for \param name
- */
- const NJointControllerPtr& getNJointControllerNotNull(const std::string& name) const;
+ /**
+ * @brief Returns pointers to the \ref NJointControllerBase "NJointControllers". (If one does not exist, an exception is thrown.)
+ * @param names The \ref NJointControllerBase "NJointControllers" names.
+ * @return Pointers to the \ref NJointControllerBase "NJointControllers".
+ * @throw If there is no \ref NJointControllerBase for \param name
+ */
+ std::vector<armarx::NJointControllerBasePtr> getNJointControllersNotNull(const std::vector<std::string>& names) const;
+ /**
+ * @brief Returns a pointer to the \ref NJointControllerBase. (If it does not exist, an exception is thrown.)
+ * @param name The \ref NJointControllerBase
+ * @return A pointer to the \ref NJointControllerBase.
+ * @throw If there is no \ref NJointControllerBase for \param name
+ */
+ const NJointControllerBasePtr& getNJointControllerNotNull(const std::string& name) const;
- /**
- * @brief Returns the names of given \ref NJointController "NJointControllers"
- * @param ctrls The \ref NJointController "NJointControllers"
- * @return The names of given \ref NJointController "NJointControllers"
- */
- std::vector<std::string> getNJointControllerNames(const std::vector<armarx::NJointControllerPtr>& ctrls) const;
+ /**
+ * @brief Returns the names of given \ref NJointControllerBase "NJointControllers"
+ * @param ctrls The \ref NJointControllerBase "NJointControllers"
+ * @return The names of given \ref NJointControllerBase "NJointControllers"
+ */
+ std::vector<std::string> getNJointControllerNames(const std::vector<armarx::NJointControllerBasePtr>& ctrls) const;
- /**
- * @brief Queues the given \ref NJointController for deletion.
- * @param ctrl The \ref NJointController to delete.
- * @see removeNJointControllersToBeDeleted
- * @see nJointControllersToBeDeleted
- * @see deleteNJointControllers
- */
- void deleteNJointController(const NJointControllerPtr& ctrl);
- /**
- * @brief Queues the given \ref NJointController "NJointControllers" for deletion.
- * @param ctrls The \ref NJointController "NJointControllers" to delete.
- * @see removeNJointControllersToBeDeleted
- * @see nJointControllersToBeDeleted
- * @see deleteNJointController
- */
- void deleteNJointControllers(const std::vector<NJointControllerPtr>& ctrls);
- /**
- * @brief Queues the given \ref NJointController for deletion and deactivates it if necessary.
- * @param ctrl The \ref NJointController to delete.
- * @see removeNJointControllersToBeDeleted
- * @see nJointControllersToBeDeleted
- * @see deleteNJointController
- * @see deleteNJointControllers
- * @see deactivateAnddeleteNJointControllers
- */
- void deactivateAndDeleteNJointController(const NJointControllerPtr& ctrl);
- /**
- * @brief Queues the given \ref NJointController "NJointControllers" for deletion and deactivates them if necessary.
- * @param ctrls The \ref NJointController "NJointControllers" to delete.
- * @see removeNJointControllersToBeDeleted
- * @see nJointControllersToBeDeleted
- * @see deleteNJointController
- * @see deleteNJointControllers
- * @see deactivateAnddeleteNJointController
- */
- void deactivateAndDeleteNJointControllers(const std::vector<NJointControllerPtr>& ctrls);
+ /**
+ * @brief Queues the given \ref NJointControllerBase for deletion.
+ * @param ctrl The \ref NJointControllerBase to delete.
+ * @see removeNJointControllersToBeDeleted
+ * @see nJointControllersToBeDeleted
+ * @see deleteNJointControllers
+ */
+ void deleteNJointController(const NJointControllerBasePtr& ctrl);
+ /**
+ * @brief Queues the given \ref NJointControllerBase "NJointControllers" for deletion.
+ * @param ctrls The \ref NJointControllerBase "NJointControllers" to delete.
+ * @see removeNJointControllersToBeDeleted
+ * @see nJointControllersToBeDeleted
+ * @see deleteNJointController
+ */
+ void deleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrls);
+ /**
+ * @brief Queues the given \ref NJointControllerBase for deletion and deactivates it if necessary.
+ * @param ctrl The \ref NJointControllerBase to delete.
+ * @see removeNJointControllersToBeDeleted
+ * @see nJointControllersToBeDeleted
+ * @see deleteNJointController
+ * @see deleteNJointControllers
+ * @see deactivateAnddeleteNJointControllers
+ */
+ void deactivateAndDeleteNJointController(const NJointControllerBasePtr& ctrl);
+ /**
+ * @brief Queues the given \ref NJointControllerBase "NJointControllers" for deletion and deactivates them if necessary.
+ * @param ctrls The \ref NJointControllerBase "NJointControllers" to delete.
+ * @see removeNJointControllersToBeDeleted
+ * @see nJointControllersToBeDeleted
+ * @see deleteNJointController
+ * @see deleteNJointControllers
+ * @see deactivateAnddeleteNJointController
+ */
+ void deactivateAndDeleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrls);
- /**
- * @brief Requests activation for the given \ref NJointController.
- * @param ctrl The requested \ref NJointController.
- * @see activateNJointControllers
- */
- void activateNJointController(const NJointControllerPtr& ctrl);
- /**
- * @brief Requests activation for the given \ref NJointController "NJointControllers".
- * @param ctrls The requested \ref NJointController "NJointControllers".
- * @see activateNJointController
- */
- void activateNJointControllers(const std::vector<NJointControllerPtr>& ctrls);
+ /**
+ * @brief Requests activation for the given \ref NJointControllerBase.
+ * @param ctrl The requested \ref NJointControllerBase.
+ * @see activateNJointControllers
+ */
+ void activateNJointController(const NJointControllerBasePtr& ctrl);
+ /**
+ * @brief Requests activation for the given \ref NJointControllerBase "NJointControllers".
+ * @param ctrls The requested \ref NJointControllerBase "NJointControllers".
+ * @see activateNJointController
+ */
+ void activateNJointControllers(const std::vector<NJointControllerBasePtr>& ctrls);
- /**
- * @brief Requests deactivation for the given \ref NJointController.
- * @param ctrl The \ref NJointController to be deactivated.
- * @see deactivateNJointControllers
- */
- void deactivateNJointController(const NJointControllerPtr& ctrl);
- /**
- * @brief Requests deactivation for the given \ref NJointController "NJointControllers".
- * @param ctrls The \ref NJointController "NJointControllers" to be deactivated.
- * @see deactivateNJointController
- */
- void deactivateNJointControllers(const std::vector<NJointControllerPtr>& ctrls);
+ /**
+ * @brief Requests deactivation for the given \ref NJointControllerBase.
+ * @param ctrl The \ref NJointControllerBase to be deactivated.
+ * @see deactivateNJointControllers
+ */
+ void deactivateNJointController(const NJointControllerBasePtr& ctrl);
+ /**
+ * @brief Requests deactivation for the given \ref NJointControllerBase "NJointControllers".
+ * @param ctrls The \ref NJointControllerBase "NJointControllers" to be deactivated.
+ * @see deactivateNJointController
+ */
+ void deactivateNJointControllers(const std::vector<NJointControllerBasePtr>& ctrls);
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////// implementation //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * @brief Deletes the given \ref NJointController "NJointControllers" and removes them
- * from the \ref ArmarXManager
- * @param ctrls \ref NJointController "NJointControllers" to delete.
- * @param blocking Whether removal from the \ref ArmarXManager should be blocking.
- * @param l Proxy to the listener topic all removal events should be sent to.
- */
- void removeNJointControllers(std::map<std::string, NJointControllerPtr>& ctrls, bool blocking = true, RobotUnitListenerPrx l = nullptr);
- /**
- * @brief Calls \ref removeNJointControllers for all \ref NJointController "NJointControllers"
- * queued for deletion (\ref nJointControllersToBeDeleted).
- * @param blocking Whether removal from the \ref ArmarXManager should be blocking.
- * @param l Proxy to the listener topic all removal events should be sent to.
- * @see removeNJointControllers
- * @see nJointControllersToBeDeleted
- */
- void removeNJointControllersToBeDeleted(bool blocking = true, RobotUnitListenerPrx l = nullptr);
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * @brief Deletes the given \ref NJointControllerBase "NJointControllers" and removes them
+ * from the \ref ArmarXManager
+ * @param ctrls \ref NJointControllerBase "NJointControllers" to delete.
+ * @param blocking Whether removal from the \ref ArmarXManager should be blocking.
+ * @param l Proxy to the listener topic all removal events should be sent to.
+ */
+ void removeNJointControllers(std::map<std::string, NJointControllerBasePtr>& ctrls, bool blocking = true, RobotUnitListenerPrx l = nullptr);
+ /**
+ * @brief Calls \ref removeNJointControllers for all \ref NJointControllerBase "NJointControllers"
+ * queued for deletion (\ref nJointControllersToBeDeleted).
+ * @param blocking Whether removal from the \ref ArmarXManager should be blocking.
+ * @param l Proxy to the listener topic all removal events should be sent to.
+ * @see removeNJointControllers
+ * @see nJointControllersToBeDeleted
+ */
+ void removeNJointControllersToBeDeleted(bool blocking = true, RobotUnitListenerPrx l = nullptr);
- /**
- * @brief Sets the requested flag for all given \ref NJointController "NJointControllers" and unsets it for the rest.
- * @param request The \ref NJointController "NJointControllers" where the requested flag should be set.
- */
- void updateNJointControllerRequestedState(const std::set<NJointControllerPtr>& request);
- /**
- * @brief Throws if there is no factory for \ref NJointController "NJointControllers" for the given class name.
- * @param className The class to check for.
- * @throw If there is no factory for \ref NJointController "NJointControllers" for the given class name.
- */
- void checkNJointControllerClassName(const std::string& className) const;
+ /**
+ * @brief Sets the requested flag for all given \ref NJointControllerBase "NJointControllers" and unsets it for the rest.
+ * @param request The \ref NJointControllerBase "NJointControllers" where the requested flag should be set.
+ */
+ void updateNJointControllerRequestedState(const std::set<NJointControllerBasePtr>& request);
+ /**
+ * @brief Throws if there is no factory for \ref NJointControllerBase "NJointControllers" for the given class name.
+ * @param className The class to check for.
+ * @throw If there is no factory for \ref NJointControllerBase "NJointControllers" for the given class name.
+ */
+ void checkNJointControllerClassName(const std::string& className) const;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief Holds all currently loaded NJoint controllers (index: [instancename])(May not be accessed in rt.)
- std::map<std::string, NJointControllerPtr> nJointControllers;
- /// @brief These controllers will be deleted in the next iteration of publish
- std::map<std::string, NJointControllerPtr> nJointControllersToBeDeleted;
- /// @brief VirtualRobot used when creating controllers / calling other functions in this module
- VirtualRobot::RobotPtr controllerCreateRobot;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief Holds all currently loaded NJoint controllers (index: [instancename])(May not be accessed in rt.)
+ std::map<std::string, NJointControllerBasePtr> nJointControllers;
+ /// @brief These controllers will be deleted in the next iteration of publish
+ std::map<std::string, NJointControllerBasePtr> nJointControllersToBeDeleted;
+ /// @brief VirtualRobot used when creating controllers / calling other functions in this module
+ VirtualRobot::RobotPtr controllerCreateRobot;
- std::recursive_mutex controllerMutex;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref Publisher.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class ControllerManagementAttorneyForPublisher;
- /**
- * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref ControlThreadDataBuffer.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class ControllerManagementAttorneyForControlThreadDataBuffer;
- };
- }
+ std::recursive_mutex controllerMutex;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref Publisher.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class ControllerManagementAttorneyForPublisher;
+ /**
+ * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref ControlThreadDataBuffer.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class ControllerManagementAttorneyForControlThreadDataBuffer;
+ };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index 68c6254f177655c6d1064b2227da664c194125fe..526db6aa7e1771f8dc234a8bc890be2f3003e6be 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -24,698 +24,694 @@
#include "RobotUnitModuleControlThreadDataBuffer.h"
#include "RobotUnitModuleRobotData.h"
-
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
- {
- const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
+ const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
- ControlDeviceDescription Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
+ ControlDeviceDescription Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ if (!controlDevices.has(name))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- if (!controlDevices.has(name))
- {
- std::stringstream ss;
- ss << "getControlDeviceDescription: There is no ControlDevice '" << name
- << "'. There are these ControlDevices: " << controlDevices.keys();
- throw InvalidArgumentException {ss.str()};
- }
- return getControlDeviceDescription(controlDevices.index(name));
+ std::stringstream ss;
+ ss << "getControlDeviceDescription: There is no ControlDevice '" << name
+ << "'. There are these ControlDevices: " << controlDevices.keys();
+ throw InvalidArgumentException {ss.str()};
}
+ return getControlDeviceDescription(controlDevices.index(name));
+ }
- ControlDeviceDescriptionSeq Devices::getControlDeviceDescriptions(const Ice::Current&) const
+ ControlDeviceDescriptionSeq Devices::getControlDeviceDescriptions(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (!areDevicesReady())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- if (!areDevicesReady())
- {
- return {};
- }
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- ControlDeviceDescriptionSeq r;
- r.reserve(getNumberOfControlDevices());
- for (auto idx : getIndices(controlDevices.values()))
- {
- r.emplace_back(getControlDeviceDescription(idx));
- }
- throwIfDevicesNotReady(__FUNCTION__);
- return r;
+ return {};
}
-
- std::size_t Devices::getNumberOfControlDevices() const
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ ControlDeviceDescriptionSeq r;
+ r.reserve(getNumberOfControlDevices());
+ for (auto idx : getIndices(controlDevices.values()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- return controlDevices.size();
+ r.emplace_back(getControlDeviceDescription(idx));
}
+ throwIfDevicesNotReady(__FUNCTION__);
+ return r;
+ }
- std::size_t Devices::getNumberOfSensorDevices() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- return sensorDevices.size();
- }
+ std::size_t Devices::getNumberOfControlDevices() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlDevices.size();
+ }
- std::size_t Devices::getSensorDeviceIndex(const std::string& deviceName) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
- return sensorDevices.index(deviceName);
- }
- std::size_t Devices::getControlDeviceIndex(const std::string& deviceName) const
+ std::size_t Devices::getNumberOfSensorDevices() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorDevices.size();
+ }
+
+ std::size_t Devices::getSensorDeviceIndex(const std::string& deviceName) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
+ return sensorDevices.index(deviceName);
+ }
+ std::size_t Devices::getControlDeviceIndex(const std::string& deviceName) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
+ return controlDevices.index(deviceName);
+ }
+
+ ConstSensorDevicePtr Devices::getSensorDevice(const std::string& sensorDeviceName) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
+ }
+
+ ConstControlDevicePtr Devices::getControlDevice(const std::string& deviceName) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ return controlDevices.at(deviceName, ControlDevice::NullPtr);
+ }
+
+ Ice::StringSeq Devices::getControlDeviceNames(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ return controlDevices.keys();
+ }
+
+ ControlDeviceDescription Devices::getControlDeviceDescription(std::size_t idx) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ const ControlDevicePtr& controlDevice = controlDevices.at(idx);
+ ControlDeviceDescription data;
+ data.deviceName = controlDevice->getDeviceName();
+ data.tags.assign(controlDevice->getTags().begin(), controlDevice->getTags().end());
+ for (const auto& jointCtrl : controlDevice->getJointControllers())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
- return controlDevices.index(deviceName);
+ data.contolModeToTargetType[jointCtrl->getControlMode()].targetType = jointCtrl->getControlTarget()->getControlTargetType();
+ data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode = jointCtrl->getHardwareControlMode();
}
+ throwIfDevicesNotReady(__FUNCTION__);
+ return data;
+ }
- ConstSensorDevicePtr Devices::getSensorDevice(const std::string& sensorDeviceName) const
+ ControlDeviceStatus Devices::getControlDeviceStatus(std::size_t idx) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ const ControlDevicePtr& controlDevice = controlDevices.at(idx);
+ ControlDeviceStatus status;
+ const auto activeJointCtrl = _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
+ status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
+ status.deviceName = controlDevice->getDeviceName();
+ const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
+ ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(idx));
+ status.requestedControlMode = requestedJointControllers.at(idx)->getControlMode();
+ for (const auto& targ : _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().control.at(idx))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
+ status.controlTargetValues[targ->getControlMode()] = targ->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
}
+ status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return status;
+ }
- ConstControlDevicePtr Devices::getControlDevice(const std::string& deviceName) const
+ ControlDeviceStatus Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ if (!controlDevices.has(name))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- return controlDevices.at(deviceName, ControlDevice::NullPtr);
+ std::stringstream ss;
+ ss << "getControlDeviceStatus: There is no ControlDevice '" << name
+ << "'. There are these ControlDevices: " << controlDevices.keys();
+ throw InvalidArgumentException {ss.str()};
}
+ return getControlDeviceStatus(controlDevices.index(name));
+ }
- Ice::StringSeq Devices::getControlDeviceNames(const Ice::Current&) const
+ ControlDeviceStatusSeq Devices::getControlDeviceStatuses(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (!areDevicesReady())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- return controlDevices.keys();
+ return {};
}
-
- ControlDeviceDescription Devices::getControlDeviceDescription(std::size_t idx) const
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ ControlDeviceStatusSeq r;
+ r.reserve(getNumberOfControlDevices());
+ for (auto idx : getIndices(controlDevices.values()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- const ControlDevicePtr& controlDevice = controlDevices.at(idx);
- ControlDeviceDescription data;
- data.deviceName = controlDevice->getDeviceName();
- data.tags.assign(controlDevice->getTags().begin(), controlDevice->getTags().end());
- for (const auto& jointCtrl : controlDevice->getJointControllers())
- {
- data.contolModeToTargetType[jointCtrl->getControlMode()].targetType = jointCtrl->getControlTarget()->getControlTargetType();
- data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode = jointCtrl->getHardwareControlMode();
- }
- throwIfDevicesNotReady(__FUNCTION__);
- return data;
+ r.emplace_back(getControlDeviceStatus(idx));
}
+ throwIfDevicesNotReady(__FUNCTION__);
+ return r;
+ }
- ControlDeviceStatus Devices::getControlDeviceStatus(std::size_t idx) const
+ Ice::StringSeq Devices::getSensorDeviceNames(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ return sensorDevices.keys();
+ }
+
+ SensorDeviceDescription Devices::getSensorDeviceDescription(std::size_t idx) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
+ SensorDeviceDescription data;
+ data.deviceName = sensorDevice->getDeviceName();
+ data.tags.assign(sensorDevice->getTags().begin(), sensorDevice->getTags().end());
+ data.sensorValueType = sensorDevice->getSensorValueType();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return data;
+ }
+
+ SensorDeviceStatus Devices::getSensorDeviceStatus(std::size_t idx) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
+ SensorDeviceStatus status;
+ status.deviceName = sensorDevice->getDeviceName();
+ status.sensorValue = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensors.at(idx)->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
+ status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return status;
+ }
+
+ SensorDeviceDescription Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ if (!sensorDevices.has(name))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- const ControlDevicePtr& controlDevice = controlDevices.at(idx);
- ControlDeviceStatus status;
- const auto activeJointCtrl = _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
- status.deviceName = controlDevice->getDeviceName();
- const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
- ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(idx));
- status.requestedControlMode = requestedJointControllers.at(idx)->getControlMode();
- for (const auto& targ : _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().control.at(idx))
- {
- status.controlTargetValues[targ->getControlMode()] = targ->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
- }
- status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
- throwIfDevicesNotReady(__FUNCTION__);
- return status;
+ std::stringstream ss;
+ ss << "getSensorDeviceDescription: There is no SensorDevice '" << name
+ << "'. There are these SensorDevices: " << sensorDevices.keys();
+ throw InvalidArgumentException {ss.str()};
}
+ return getSensorDeviceDescription(sensorDevices.index(name));
+ }
- ControlDeviceStatus Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
+ SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ if (!areDevicesReady())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- if (!controlDevices.has(name))
- {
- std::stringstream ss;
- ss << "getControlDeviceStatus: There is no ControlDevice '" << name
- << "'. There are these ControlDevices: " << controlDevices.keys();
- throw InvalidArgumentException {ss.str()};
- }
- return getControlDeviceStatus(controlDevices.index(name));
+ return {};
}
-
- ControlDeviceStatusSeq Devices::getControlDeviceStatuses(const Ice::Current&) const
+ SensorDeviceDescriptionSeq r;
+ r.reserve(getNumberOfSensorDevices());
+ for (auto idx : getIndices(sensorDevices.values()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- if (!areDevicesReady())
- {
- return {};
- }
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- ControlDeviceStatusSeq r;
- r.reserve(getNumberOfControlDevices());
- for (auto idx : getIndices(controlDevices.values()))
- {
- r.emplace_back(getControlDeviceStatus(idx));
- }
- throwIfDevicesNotReady(__FUNCTION__);
- return r;
+ r.emplace_back(getSensorDeviceDescription(idx));
}
+ return r;
+ }
- Ice::StringSeq Devices::getSensorDeviceNames(const Ice::Current&) const
+ SensorDeviceStatus Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ if (!sensorDevices.has(name))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- return sensorDevices.keys();
+ std::stringstream ss;
+ ss << "getSensorDeviceStatus: There is no SensorDevice '" << name
+ << "'. There are these SensorDevices: " << sensorDevices.keys();
+ throw InvalidArgumentException {ss.str()};
}
+ return getSensorDeviceStatus(sensorDevices.index(name));
+ }
- SensorDeviceDescription Devices::getSensorDeviceDescription(std::size_t idx) const
+ SensorDeviceStatusSeq Devices::getSensorDeviceStatuses(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (!areDevicesReady())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
- SensorDeviceDescription data;
- data.deviceName = sensorDevice->getDeviceName();
- data.tags.assign(sensorDevice->getTags().begin(), sensorDevice->getTags().end());
- data.sensorValueType = sensorDevice->getSensorValueType();
- throwIfDevicesNotReady(__FUNCTION__);
- return data;
+ return {};
}
-
- SensorDeviceStatus Devices::getSensorDeviceStatus(std::size_t idx) const
+ std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ SensorDeviceStatusSeq r;
+ r.reserve(getNumberOfSensorDevices());
+ for (auto idx : getIndices(sensorDevices.values()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
- SensorDeviceStatus status;
- status.deviceName = sensorDevice->getDeviceName();
- status.sensorValue = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensors.at(idx)->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
- status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
- throwIfDevicesNotReady(__FUNCTION__);
- return status;
+ r.emplace_back(getSensorDeviceStatus(idx));
}
+ throwIfDevicesNotReady(__FUNCTION__);
+ return r;
+ }
- SensorDeviceDescription Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
+ void Devices::addControlDevice(const ControlDevicePtr& cd)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_DEBUG << "ControlDevice " << &cd;
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- if (!sensorDevices.has(name))
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ //check it
+ if (!cd)
{
std::stringstream ss;
- ss << "getSensorDeviceDescription: There is no SensorDevice '" << name
- << "'. There are these SensorDevices: " << sensorDevices.keys();
+ ss << "armarx::RobotUnit::addControlDevice: ControlDevice is nullptr";
+ ARMARX_ERROR << ss.str();
throw InvalidArgumentException {ss.str()};
}
- return getSensorDeviceDescription(sensorDevices.index(name));
- }
-
- SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- if (!areDevicesReady())
+ if (!cd->getJointEmergencyStopController())
{
- return {};
+ std::stringstream ss;
+ ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
+ << " has null JointEmergencyStopController (this is not allowed)";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
}
- SensorDeviceDescriptionSeq r;
- r.reserve(getNumberOfSensorDevices());
- for (auto idx : getIndices(sensorDevices.values()))
+ if (!cd->getJointStopMovementController())
{
- r.emplace_back(getSensorDeviceDescription(idx));
+ std::stringstream ss;
+ ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
+ << " has null getJointStopMovementController (this is not allowed)";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
}
- return r;
+ //add it
+ ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd ;
+ controlDevices.add(cd->getDeviceName(), cd);
+ cd->owner = this;
+ ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
}
+ ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ }
- SensorDeviceStatus Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
+ void Devices::addSensorDevice(const SensorDevicePtr& sd)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_DEBUG << "SensorDevice " << &sd;
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
std::lock_guard<MutexType> guard {sensorDevicesMutex};
- if (!sensorDevices.has(name))
+ //check it
+ if (!sd)
{
std::stringstream ss;
- ss << "getSensorDeviceStatus: There is no SensorDevice '" << name
- << "'. There are these SensorDevices: " << sensorDevices.keys();
+ ss << "armarx::RobotUnit::addSensorDevice: SensorDevice is nullptr";
+ ARMARX_ERROR << ss.str();
throw InvalidArgumentException {ss.str()};
}
- return getSensorDeviceStatus(sensorDevices.index(name));
- }
-
- SensorDeviceStatusSeq Devices::getSensorDeviceStatuses(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- if (!areDevicesReady())
+ if (!sd->getSensorValue())
{
- return {};
+ std::stringstream ss;
+ ss << "armarx::RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName()
+ << " has null SensorValue (this is not allowed)";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
}
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- SensorDeviceStatusSeq r;
- r.reserve(getNumberOfSensorDevices());
- for (auto idx : getIndices(sensorDevices.values()))
+ //add it
+ if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
+ {
+ ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
+ if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
+ {
+ throw InvalidArgumentException
+ {
+ "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
+ " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
+ };
+ }
+ //this checks if we already added such a device (do this before setting timingSensorDevice)
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ sensorDevices.add(sd->getDeviceName(), sd);
+ sd->owner = this;
+ rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
+ }
+ else
{
- r.emplace_back(getSensorDeviceStatus(idx));
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ sensorDevices.add(sd->getDeviceName(), sd);
+ sd->owner = this;
}
- throwIfDevicesNotReady(__FUNCTION__);
- return r;
}
+ ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ }
+
+ RTThreadTimingsSensorDevicePtr Devices::createRTThreadTimingSensorDevice() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
+ }
- void Devices::addControlDevice(const ControlDevicePtr& cd)
+ void Devices::_postFinishRunning()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<MutexType> guardS {sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ controlDevicesConstPtr.clear();
+ sensorDevicesConstPtr.clear();
+ sensorDevices.clear();
+ controlDevices.clear();
+ }
+
+ std::vector<JointController*> Devices::getStopMovementJointControllers() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::vector<JointController*> controllers;
+ controllers.reserve(controlDevices.values().size());
+ for (const ControlDevicePtr& dev : controlDevices.values())
+ {
+ ARMARX_CHECK_NOT_NULL(dev);
+ controllers.emplace_back(dev->rtGetJointStopMovementController());
+ ARMARX_CHECK_NOT_NULL(controllers.back());
+ }
+ ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controllers;
+ }
+
+ std::vector<JointController*> Devices::getEmergencyStopJointControllers() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::vector<JointController*> controllers;
+ controllers.reserve(controlDevices.values().size());
+ for (const ControlDevicePtr& dev : controlDevices.values())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "ControlDevice " << &cd;
- throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ ARMARX_CHECK_NOT_NULL(dev);
+ controllers.emplace_back(dev->rtGetJointEmergencyStopController());
+ ARMARX_CHECK_NOT_NULL(controllers.back());
+ }
+ ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controllers;
+ }
+
+ void Devices::_preFinishDeviceInitialization()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<MutexType> guardS {sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
+ {
+ addSensorDevice(createRTThreadTimingSensorDevice());
+ }
+ }
+
+ void Devices::_postFinishDeviceInitialization()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<MutexType> guardS {sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
+ {
+ for (const ControlDevicePtr& controlDevice : controlDevices.values())
{
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- //check it
- if (!cd)
+ ARMARX_CHECK_EXPRESSION(controlDevice);
+ ARMARX_DEBUG << "----" << controlDevice->getDeviceName();
+ if (!controlDevice->hasJointController(ControlModes::EmergencyStop))
{
- std::stringstream ss;
- ss << "armarx::RobotUnit::addControlDevice: ControlDevice is nullptr";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- if (!cd->getJointEmergencyStopController())
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
- << " has null JointEmergencyStopController (this is not allowed)";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ std::stringstream s;
+ s << "ControlDevice " << controlDevice->getDeviceName()
+ << " has no JointController with ControlMode " << ControlModes::EmergencyStop
+ << " (to fix this, add a JointController with this ControlMode to the ControlDevice).\nAvailable controllers: "
+ << controlDevice->getControlModes();
+ ARMARX_ERROR << "--------" << s.str();
+ throw LogicError {s.str()};
+
}
- if (!cd->getJointStopMovementController())
+ if (!controlDevice->hasJointController(ControlModes::StopMovement))
{
- std::stringstream ss;
- ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
- << " has null getJointStopMovementController (this is not allowed)";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ std::stringstream s;
+ s << "ControlDevice " << controlDevice->getDeviceName()
+ << " has no JointController with ControlMode \"" << ControlModes::StopMovement
+ << "\" (to fix this, add a JointController with this ControlMode to the ControlDevice) \nAvailable controllers: "
+ << controlDevice->getControlModes();
+ ARMARX_ERROR << "--------" << s.str();
+ throw LogicError {s.str()};
}
- //add it
- ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd ;
- controlDevices.add(cd->getDeviceName(), cd);
- cd->owner = this;
- ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
}
- ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
- throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
}
-
- void Devices::addSensorDevice(const SensorDevicePtr& sd)
+ ARMARX_DEBUG << "checking " << controlDevices.size() << " SensorDevices:";
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "SensorDevice " << &sd;
- throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ for (const SensorDevicePtr& sensorDevice : sensorDevices.values())
{
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
- //check it
- if (!sd)
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::addSensorDevice: SensorDevice is nullptr";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- if (!sd->getSensorValue())
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName()
- << " has null SensorValue (this is not allowed)";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- //add it
- if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
+ ARMARX_CHECK_EXPRESSION(sensorDevice);
+ ARMARX_DEBUG << "----" << sensorDevice->getDeviceName();
+ if (!sensorDevice->getSensorValue())
{
- ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
- if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
- {
- throw InvalidArgumentException
- {
- "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
- " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
- };
- }
- //this checks if we already added such a device (do this before setting timingSensorDevice)
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
- sensorDevices.add(sd->getDeviceName(), sd);
- sd->owner = this;
- rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
- }
- else
- {
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
- sensorDevices.add(sd->getDeviceName(), sd);
- sd->owner = this;
+ std::stringstream s;
+ s << "SensorDevice " << sensorDevice->getSensorValue() << " has null SensorValue";
+ ARMARX_ERROR << "--------" << s.str();
+ throw LogicError {s.str()};
}
+ ARMARX_CHECK_EXPRESSION(sensorDevice);
}
- ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";
- throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
}
-
- RTThreadTimingsSensorDevicePtr Devices::createRTThreadTimingSensorDevice() const
+ ARMARX_DEBUG << "copying device ptrs to const ptr map";
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
- }
-
- void Devices::_postFinishRunning()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
- controlDevicesConstPtr.clear();
- sensorDevicesConstPtr.clear();
- sensorDevices.clear();
- controlDevices.clear();
+ for (const auto& dev : controlDevices.values())
+ {
+ controlDevicesConstPtr[dev->getDeviceName()] = dev;
+ }
+ for (const auto& dev : sensorDevices.values())
+ {
+ sensorDevicesConstPtr[dev->getDeviceName()] = dev;
+ }
}
-
- std::vector<JointController*> Devices::getStopMovementJointControllers() const
+ ARMARX_DEBUG << "copy sensor values";
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- std::vector<JointController*> controllers;
- controllers.reserve(controlDevices.values().size());
- for (const ControlDevicePtr& dev : controlDevices.values())
+ ARMARX_CHECK_EXPRESSION(sensorValues.empty());
+ sensorValues.reserve(sensorDevices.values().size());
+ for (const SensorDevicePtr& dev : sensorDevices.values())
{
ARMARX_CHECK_NOT_NULL(dev);
- controllers.emplace_back(dev->rtGetJointStopMovementController());
- ARMARX_CHECK_NOT_NULL(controllers.back());
+ sensorValues.emplace_back(dev->getSensorValue());
+ ARMARX_CHECK_NOT_NULL(sensorValues.back());
}
- ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
- throwIfDevicesNotReady(__FUNCTION__);
- return controllers;
}
-
- std::vector<JointController*> Devices::getEmergencyStopJointControllers() const
+ ARMARX_DEBUG << "copy control targets";
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
- std::vector<JointController*> controllers;
- controllers.reserve(controlDevices.values().size());
+ ARMARX_CHECK_EXPRESSION(controlTargets.empty());
+ controlTargets.reserve(controlDevices.values().size());
for (const ControlDevicePtr& dev : controlDevices.values())
{
ARMARX_CHECK_NOT_NULL(dev);
- controllers.emplace_back(dev->rtGetJointEmergencyStopController());
- ARMARX_CHECK_NOT_NULL(controllers.back());
+ controlTargets.emplace_back();
+ controlTargets.back().reserve(dev->rtGetJointControllers().size());
+ for (JointController* ctrl : dev->rtGetJointControllers())
+ {
+ ARMARX_CHECK_NOT_NULL(ctrl);
+ controlTargets.back().emplace_back(ctrl->getControlTarget());
+ ARMARX_CHECK_NOT_NULL(controlTargets.back().back().get());
+ }
}
- ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
- throwIfDevicesNotReady(__FUNCTION__);
- return controllers;
}
-
- void Devices::_preFinishDeviceInitialization()
+ ARMARX_DEBUG << "setup ControlDeviceHardwareControlModeGroups";
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
- if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
- {
- addSensorDevice(createRTThreadTimingSensorDevice());
- }
- }
+ ctrlModeGroups.groupIndices.assign(getNumberOfControlDevices(), IndexSentinel());
- void Devices::_postFinishDeviceInitialization()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
- ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
+ if (!ctrlModeGroups.groupsMerged.empty())
{
- for (const ControlDevicePtr& controlDevice : controlDevices.values())
+ ARMARX_DEBUG << "Remove control devs from ControlDeviceHardwareControlModeGroups ("
+ << ctrlModeGroups.groups.size() << ")";
+ const auto groupsMerged = ctrlModeGroups.groupsMerged;
+ for (const auto& dev : groupsMerged)
{
- ARMARX_CHECK_EXPRESSION(controlDevice);
- ARMARX_DEBUG << "----" << controlDevice->getDeviceName();
- if (!controlDevice->hasJointController(ControlModes::EmergencyStop))
+ if (controlDevices.has(dev))
{
- std::stringstream s;
- s << "ControlDevice " << controlDevice->getDeviceName()
- << " has no JointController with ControlMode " << ControlModes::EmergencyStop
- << " (to fix this, add a JointController with this ControlMode to the ControlDevice).\nAvailable controllers: "
- << controlDevice->getControlModes();
- ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
-
+ continue;
}
- if (!controlDevice->hasJointController(ControlModes::StopMovement))
+ ctrlModeGroups.groupsMerged.erase(dev);
+ for (auto& group : ctrlModeGroups.groups)
{
- std::stringstream s;
- s << "ControlDevice " << controlDevice->getDeviceName()
- << " has no JointController with ControlMode \"" << ControlModes::StopMovement
- << "\" (to fix this, add a JointController with this ControlMode to the ControlDevice) \nAvailable controllers: "
- << controlDevice->getControlModes();
- ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
+ group.erase(dev);
}
+ ARMARX_DEBUG << "----removing nonexistent device: " << dev;
}
- }
- ARMARX_DEBUG << "checking " << controlDevices.size() << " SensorDevices:";
- {
- for (const SensorDevicePtr& sensorDevice : sensorDevices.values())
+ //remove empty groups
+ std::vector<std::set<std::string>> cleanedGroups;
+ cleanedGroups.reserve(ctrlModeGroups.groups.size());
+ for (auto& group : ctrlModeGroups.groups)
{
- ARMARX_CHECK_EXPRESSION(sensorDevice);
- ARMARX_DEBUG << "----" << sensorDevice->getDeviceName();
- if (!sensorDevice->getSensorValue())
+ const auto sz = group.size();
+ if (sz == 1)
{
- std::stringstream s;
- s << "SensorDevice " << sensorDevice->getSensorValue() << " has null SensorValue";
- ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
+ ARMARX_DEBUG << "----removing group with one dev: " << *group.begin();
}
- ARMARX_CHECK_EXPRESSION(sensorDevice);
- }
- }
- ARMARX_DEBUG << "copying device ptrs to const ptr map";
- {
- for (const auto& dev : controlDevices.values())
- {
- controlDevicesConstPtr[dev->getDeviceName()] = dev;
- }
- for (const auto& dev : sensorDevices.values())
- {
- sensorDevicesConstPtr[dev->getDeviceName()] = dev;
- }
- }
- ARMARX_DEBUG << "copy sensor values";
- {
- ARMARX_CHECK_EXPRESSION(sensorValues.empty());
- sensorValues.reserve(sensorDevices.values().size());
- for (const SensorDevicePtr& dev : sensorDevices.values())
- {
- ARMARX_CHECK_NOT_NULL(dev);
- sensorValues.emplace_back(dev->getSensorValue());
- ARMARX_CHECK_NOT_NULL(sensorValues.back());
- }
- }
- ARMARX_DEBUG << "copy control targets";
- {
- ARMARX_CHECK_EXPRESSION(controlTargets.empty());
- controlTargets.reserve(controlDevices.values().size());
- for (const ControlDevicePtr& dev : controlDevices.values())
- {
- ARMARX_CHECK_NOT_NULL(dev);
- controlTargets.emplace_back();
- controlTargets.back().reserve(dev->rtGetJointControllers().size());
- for (JointController* ctrl : dev->rtGetJointControllers())
+ else if (sz > 1)
{
- ARMARX_CHECK_NOT_NULL(ctrl);
- controlTargets.back().emplace_back(ctrl->getControlTarget());
- ARMARX_CHECK_NOT_NULL(controlTargets.back().back().get());
+ cleanedGroups.emplace_back(std::move(group));
}
}
+ ctrlModeGroups.groups = cleanedGroups;
+ ARMARX_DEBUG << "----number of groups left: " << ctrlModeGroups.groups.size();
}
- ARMARX_DEBUG << "setup ControlDeviceHardwareControlModeGroups";
+ if (!ctrlModeGroups.groupsMerged.empty())
{
- ctrlModeGroups.groupIndices.assign(getNumberOfControlDevices(), IndexSentinel());
-
- if (!ctrlModeGroups.groupsMerged.empty())
+ ARMARX_DEBUG << "Checking control modes for ControlDeviceHardwareControlModeGroups ("
+ << ctrlModeGroups.groups.size() << ")";
+ ctrlModeGroups.deviceIndices.resize(ctrlModeGroups.groups.size());
+ //iterate over groups
+ for (std::size_t groupIdx = 0; groupIdx < ctrlModeGroups.groups.size(); ++groupIdx)
{
- ARMARX_DEBUG << "Remove control devs from ControlDeviceHardwareControlModeGroups ("
- << ctrlModeGroups.groups.size() << ")";
- const auto groupsMerged = ctrlModeGroups.groupsMerged;
- for (const auto& dev : groupsMerged)
+ const std::set<std::string>& group = ctrlModeGroups.groups.at(groupIdx);
+ ctrlModeGroups.deviceIndices.at(groupIdx).reserve(group.size());
+ ARMARX_CHECK_EXPRESSION(!group.empty());
+ ARMARX_DEBUG << "----Group " << groupIdx << " size: " << group.size();
+ //gets a map of ControlMode->HardwareControlMode for the given device
+ const auto getControlModeToHWControlMode = [&](const std::string & devname)
{
- if (controlDevices.has(dev))
- {
- continue;
- }
- ctrlModeGroups.groupsMerged.erase(dev);
- for (auto& group : ctrlModeGroups.groups)
+ std::map<std::string, std::string> controlModeToHWControlMode;
+ const ControlDevicePtr& cd = controlDevices.at(devname);
+ for (const auto& jointCtrl : cd->getJointControllers())
{
- group.erase(dev);
+ controlModeToHWControlMode[jointCtrl->getControlMode()] = jointCtrl->getHardwareControlMode();
}
- ARMARX_DEBUG << "----removing nonexistent device: " << dev;
- }
- //remove empty groups
- std::vector<std::set<std::string>> cleanedGroups;
- cleanedGroups.reserve(ctrlModeGroups.groups.size());
- for (auto& group : ctrlModeGroups.groups)
+ return controlModeToHWControlMode;
+ };
+ //get modes of first dev
+ const auto controlModeToHWControlMode = getControlModeToHWControlMode(*group.begin());
+ //check other devs
+ for (const auto& devname : group)
{
- const auto sz = group.size();
- if (sz == 1)
- {
- ARMARX_DEBUG << "----removing group with one dev: " << *group.begin();
- }
- else if (sz > 1)
- {
- cleanedGroups.emplace_back(std::move(group));
- }
- }
- ctrlModeGroups.groups = cleanedGroups;
- ARMARX_DEBUG << "----number of groups left: " << ctrlModeGroups.groups.size();
- }
- if (!ctrlModeGroups.groupsMerged.empty())
- {
- ARMARX_DEBUG << "Checking control modes for ControlDeviceHardwareControlModeGroups ("
- << ctrlModeGroups.groups.size() << ")";
- ctrlModeGroups.deviceIndices.resize(ctrlModeGroups.groups.size());
- //iterate over groups
- for (std::size_t groupIdx = 0; groupIdx < ctrlModeGroups.groups.size(); ++groupIdx)
- {
- const std::set<std::string>& group = ctrlModeGroups.groups.at(groupIdx);
- ctrlModeGroups.deviceIndices.at(groupIdx).reserve(group.size());
- ARMARX_CHECK_EXPRESSION(!group.empty());
- ARMARX_DEBUG << "----Group " << groupIdx << " size: " << group.size();
- //gets a map of ControlMode->HardwareControlMode for the given device
- const auto getControlModeToHWControlMode = [&](const std::string & devname)
- {
- std::map<std::string, std::string> controlModeToHWControlMode;
- const ControlDevicePtr& cd = controlDevices.at(devname);
- for (const auto& jointCtrl : cd->getJointControllers())
- {
- controlModeToHWControlMode[jointCtrl->getControlMode()] = jointCtrl->getHardwareControlMode();
- }
- return controlModeToHWControlMode;
- };
- //get modes of first dev
- const auto controlModeToHWControlMode = getControlModeToHWControlMode(*group.begin());
- //check other devs
- for (const auto& devname : group)
- {
- ARMARX_CHECK_EXPRESSION_W_HINT(
- controlDevices.has(devname),
- "The ControlDeviceHardwareControlModeGroups property contains device names not existent in the robot: "
- << devname << "\navailable:\n" << controlDevices.keys()
- );
- //Assert all devices in a group have the same control modes with the same hw controle modes
- const auto controlModeToHWControlModeForDevice = getControlModeToHWControlMode(devname);
- ARMARX_CHECK_EXPRESSION_W_HINT(
- controlModeToHWControlModeForDevice == controlModeToHWControlMode,
- "Error for control modes of device '" << devname << "'\n"
- << "it has the modes: " << controlModeToHWControlModeForDevice
- << "\n but should have the modes: " << controlModeToHWControlMode
- );
- //insert the device index into the device indices
- const auto devIdx = controlDevices.index(devname);
- ctrlModeGroups.deviceIndices.at(groupIdx).emplace_back(devIdx);
- //insert the group index into the group indices (+ check the current group index is the sentinel
- ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.size() > devIdx);
- ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.at(devIdx) == IndexSentinel());
- ctrlModeGroups.groupIndices.at(devIdx) = groupIdx;
- ARMARX_DEBUG << "------- " << devname;
- }
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ controlDevices.has(devname),
+ "The ControlDeviceHardwareControlModeGroups property contains device names not existent in the robot: "
+ << devname << "\navailable:\n" << controlDevices.keys()
+ );
+ //Assert all devices in a group have the same control modes with the same hw controle modes
+ const auto controlModeToHWControlModeForDevice = getControlModeToHWControlMode(devname);
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ controlModeToHWControlModeForDevice == controlModeToHWControlMode,
+ "Error for control modes of device '" << devname << "'\n"
+ << "it has the modes: " << controlModeToHWControlModeForDevice
+ << "\n but should have the modes: " << controlModeToHWControlMode
+ );
+ //insert the device index into the device indices
+ const auto devIdx = controlDevices.index(devname);
+ ctrlModeGroups.deviceIndices.at(groupIdx).emplace_back(devIdx);
+ //insert the group index into the group indices (+ check the current group index is the sentinel
+ ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.size() > devIdx);
+ ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.at(devIdx) == IndexSentinel());
+ ctrlModeGroups.groupIndices.at(devIdx) = groupIdx;
+ ARMARX_DEBUG << "------- " << devname;
}
}
}
- ARMARX_DEBUG << "create mapping from sensor values to robot nodes";
+ }
+ ARMARX_DEBUG << "create mapping from sensor values to robot nodes";
+ {
+ ARMARX_CHECK_EXPRESSION(simoxRobotSensorValueMapping.empty());
+ VirtualRobot::RobotPtr r = _module<RobotData>().cloneRobot();
+ const auto nodes = r->getRobotNodes();
+ for (std::size_t idxRobot = 0; idxRobot < nodes.size(); ++idxRobot)
{
- ARMARX_CHECK_EXPRESSION(simoxRobotSensorValueMapping.empty());
- VirtualRobot::RobotPtr r = _module<RobotData>().cloneRobot();
- const auto nodes = r->getRobotNodes();
- for (std::size_t idxRobot = 0; idxRobot < nodes.size(); ++idxRobot)
+ const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
+ if (node->isRotationalJoint() || node->isTranslationalJoint())
{
- const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
- if (node->isRotationalJoint() || node->isTranslationalJoint())
+ const auto& name = node->getName();
+ if (sensorDevices.has(name))
{
- const auto& name = node->getName();
- if (sensorDevices.has(name))
+ const auto& dev = sensorDevices.at(name);
+ if (dev->getSensorValue()->isA<SensorValue1DoFActuatorPosition>())
{
- const auto& dev = sensorDevices.at(name);
- if (dev->getSensorValue()->isA<SensorValue1DoFActuatorPosition>())
- {
- SimoxRobotSensorValueMapping m;
- m.idxRobot = idxRobot;
- m.idxSens = sensorDevices.index(name);
- simoxRobotSensorValueMapping.emplace_back(m);
- }
- else
- {
- ARMARX_WARNING << "SensorValue for SensorDevice " << name << " has no position data";
- }
+ SimoxRobotSensorValueMapping m;
+ m.idxRobot = idxRobot;
+ m.idxSens = sensorDevices.index(name);
+ simoxRobotSensorValueMapping.emplace_back(m);
}
else
{
- ARMARX_INFO << "No SensorDevice for RobotNode: " << name;
+ ARMARX_WARNING << "SensorValue for SensorDevice " << name << " has no position data";
}
}
+ else
+ {
+ ARMARX_INFO << "No SensorDevice for RobotNode: " << name;
+ }
}
}
-
- ARMARX_VERBOSE << "ControlDevices:\n" << controlDevices.keys();
- ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
}
- void Devices::_preOnInitRobotUnit()
+ ARMARX_VERBOSE << "ControlDevices:\n" << controlDevices.keys();
+ ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
+ }
+
+ void Devices::_preOnInitRobotUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //ControlDeviceHardwareControlModeGroups
+ const std::string controlDeviceHardwareControlModeGroupsStr = getProperty<std::string>("ControlDevices_HardwareControlModeGroups").getValue();
+ if (!controlDeviceHardwareControlModeGroupsStr.empty())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //ControlDeviceHardwareControlModeGroups
- const std::string controlDeviceHardwareControlModeGroupsStr = getProperty<std::string>("ControlDevices_HardwareControlModeGroups").getValue();
- if (!controlDeviceHardwareControlModeGroupsStr.empty())
+ const auto numGroups = std::count(
+ controlDeviceHardwareControlModeGroupsStr.begin(),
+ controlDeviceHardwareControlModeGroupsStr.end(), ';'
+ ) + 1;
+ ctrlModeGroups.groups.reserve(numGroups);
+ std::vector<std::string> strGroups;
+ strGroups.reserve(numGroups);
+ boost::algorithm::split(strGroups, controlDeviceHardwareControlModeGroupsStr, boost::is_any_of(";"));
+ for (const auto& gstr : strGroups)
{
- const auto numGroups = std::count(
- controlDeviceHardwareControlModeGroupsStr.begin(),
- controlDeviceHardwareControlModeGroupsStr.end(), ';'
- ) + 1;
- ctrlModeGroups.groups.reserve(numGroups);
- std::vector<std::string> strGroups;
- strGroups.reserve(numGroups);
- boost::algorithm::split(strGroups, controlDeviceHardwareControlModeGroupsStr, boost::is_any_of(";"));
- for (const auto& gstr : strGroups)
+ std::vector<std::string> strElems;
+ boost::algorithm::split(strElems, gstr, boost::is_any_of(","));
+ std::set<std::string> group;
+ for (auto& device : strElems)
{
- std::vector<std::string> strElems;
- boost::algorithm::split(strElems, gstr, boost::is_any_of(","));
- std::set<std::string> group;
- for (auto& device : strElems)
- {
- boost::algorithm::trim(device);
- ARMARX_CHECK_EXPRESSION_W_HINT(
- !device.empty(),
- "The ControlDeviceHardwareControlModeGroups property contains empty device names"
- );
- ARMARX_CHECK_EXPRESSION_W_HINT(
- !ctrlModeGroups.groupsMerged.count(device),
- "The ControlDeviceHardwareControlModeGroups property contains duplicate device names: " << device
- );
- ctrlModeGroups.groupsMerged.emplace(device);
- group.emplace(std::move(device));
- }
- if (!group.empty())
+ boost::algorithm::trim(device);
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ !device.empty(),
+ "The ControlDeviceHardwareControlModeGroups property contains empty device names"
+ );
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ !ctrlModeGroups.groupsMerged.count(device),
+ "The ControlDeviceHardwareControlModeGroups property contains duplicate device names: " << device
+ );
+ ctrlModeGroups.groupsMerged.emplace(device);
+ group.emplace(std::move(device));
+ }
+ if (!group.empty())
+ {
+ ARMARX_DEBUG << "adding device group:\n"
+ << ARMARX_STREAM_PRINTER
{
- ARMARX_DEBUG << "adding device group:\n"
- << ARMARX_STREAM_PRINTER
+ for (const auto& elem : group)
{
- for (const auto& elem : group)
- {
- out << " " << elem << "\n";
- }
- };
- ctrlModeGroups.groups.emplace_back(std::move(group));
- }
+ out << " " << elem << "\n";
+ }
+ };
+ ctrlModeGroups.groups.emplace_back(std::move(group));
}
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
index 490059fa69792ba8de1c659a864e004f55a3b275..f497262b13c192066547af9e09d145d19e93d15c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
@@ -33,424 +33,420 @@
#include "../SensorValues/SensorValue1DoFActuator.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
-
- namespace RobotUnitModule
+ class DevicesPropertyDefinitions: public ModuleBasePropertyDefinitions
{
- class DevicesPropertyDefinitions: public ModuleBasePropertyDefinitions
+ public:
+ DevicesPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
{
- public:
- DevicesPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>(
- "ControlDevices_HardwareControlModeGroups", "",
- "Groups of control devices for which the same hardware control mode has to be set. "
- "For a control mode, the JointControllers of all devices in a group must have the identical hardware control mode. "
- "The kinematic unit will change the control mode of all joints in a group,"
- " if their current hw control mode does not match the new hw control mode. "
- "This can be used if the robot has a multi DOF joint which is represented by multiple 1DOF control devices "
- "(in such a case all of these 1DOF joints should have the same hw control mode, "
- "except he multi DOF joint can handle different control modes at once). "
- "Syntax: semicolon separated groups , each group is CSV of joints "
- "e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two groups!"
- );
- }
- };
+ defineOptionalProperty<std::string>(
+ "ControlDevices_HardwareControlModeGroups", "",
+ "Groups of control devices for which the same hardware control mode has to be set. "
+ "For a control mode, the JointControllers of all devices in a group must have the identical hardware control mode. "
+ "The kinematic unit will change the control mode of all joints in a group,"
+ " if their current hw control mode does not match the new hw control mode. "
+ "This can be used if the robot has a multi DOF joint which is represented by multiple 1DOF control devices "
+ "(in such a case all of these 1DOF joints should have the same hw control mode, "
+ "except he multi DOF joint can handle different control modes at once). "
+ "Syntax: semicolon separated groups , each group is CSV of joints "
+ "e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two groups!"
+ );
+ }
+ };
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" manages sensor and control devices for a RobotUnit and only allows save and sane access.
+ *
+ * @see ModuleBase
+ */
+ class Devices : virtual public ModuleBase, virtual public RobotUnitDevicesInterface
+ {
+ friend class ModuleBase;
+ public:
+ struct ControlDeviceHardwareControlModeGroups;
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" manages sensor and control devices for a RobotUnit and only allows save and sane access.
- *
- * @see ModuleBase
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
*/
- class Devices : virtual public ModuleBase, virtual public RobotUnitDevicesInterface
+ static Devices& Instance()
{
- friend class ModuleBase;
- public:
- struct ControlDeviceHardwareControlModeGroups;
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static Devices& Instance()
- {
- return ModuleBase::Instance<Devices>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_preOnInitRobotUnit
- void _preOnInitRobotUnit();
- /// @see ModuleBase::_preFinishDeviceInitialization
- void _preFinishDeviceInitialization();
- /// @see ModuleBase::_postFinishDeviceInitialization
- void _postFinishDeviceInitialization();
- /// @see ModuleBase::_postFinishRunning
- void _postFinishRunning();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// ice interface //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Returns the names of all \ref ControlDevice "ControlDevices" for the robot.
- * @return The names of all \ref ControlDevice "ControlDevices" for the robot.
- */
- Ice::StringSeq getControlDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ return ModuleBase::Instance<Devices>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_preOnInitRobotUnit
+ void _preOnInitRobotUnit();
+ /// @see ModuleBase::_preFinishDeviceInitialization
+ void _preFinishDeviceInitialization();
+ /// @see ModuleBase::_postFinishDeviceInitialization
+ void _postFinishDeviceInitialization();
+ /// @see ModuleBase::_postFinishRunning
+ void _postFinishRunning();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns the names of all \ref ControlDevice "ControlDevices" for the robot.
+ * @return The names of all \ref ControlDevice "ControlDevices" for the robot.
+ */
+ Ice::StringSeq getControlDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Return the \ref ControlDeviceDescription for the given \ref ControlDevice
- * @param name The \ref ControlDevice
- * @return The \ref ControlDeviceDescription
- * @see ControlDeviceDescription
- * @see getControlDeviceDescriptions
- */
- ControlDeviceDescription getControlDeviceDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Return the \ref ControlDeviceDescription "ControlDeviceDescriptions" for all \ref ControlDevice "ControlDevices"
- * @return The \ref ControlDeviceDescription "ControlDeviceDescriptions"
- * @see ControlDeviceDescription
- * @see getControlDeviceDescription
- */
- ControlDeviceDescriptionSeq getControlDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Return the \ref ControlDeviceDescription for the given \ref ControlDevice
+ * @param name The \ref ControlDevice
+ * @return The \ref ControlDeviceDescription
+ * @see ControlDeviceDescription
+ * @see getControlDeviceDescriptions
+ */
+ ControlDeviceDescription getControlDeviceDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Return the \ref ControlDeviceDescription "ControlDeviceDescriptions" for all \ref ControlDevice "ControlDevices"
+ * @return The \ref ControlDeviceDescription "ControlDeviceDescriptions"
+ * @see ControlDeviceDescription
+ * @see getControlDeviceDescription
+ */
+ ControlDeviceDescriptionSeq getControlDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Return the \ref ControlDeviceStatus for the given \ref ControlDevice
- * @param name The \ref ControlDevice
- * @return The \ref ControlDeviceStatus
- * @see ControlDeviceStatus
- * @see getControlDeviceStatuses
- */
- ControlDeviceStatus getControlDeviceStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Return the \ref ControlDeviceStatus "ControlDeviceStatuses" for all \ref ControlDevice "ControlDevices"
- * @return The \ref ControlDeviceStatus "ControlDeviceStatuses"
- * @see ControlDeviceStatus
- * @see getControlDeviceStatus
- */
- ControlDeviceStatusSeq getControlDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Return the \ref ControlDeviceStatus for the given \ref ControlDevice
+ * @param name The \ref ControlDevice
+ * @return The \ref ControlDeviceStatus
+ * @see ControlDeviceStatus
+ * @see getControlDeviceStatuses
+ */
+ ControlDeviceStatus getControlDeviceStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Return the \ref ControlDeviceStatus "ControlDeviceStatuses" for all \ref ControlDevice "ControlDevices"
+ * @return The \ref ControlDeviceStatus "ControlDeviceStatuses"
+ * @see ControlDeviceStatus
+ * @see getControlDeviceStatus
+ */
+ ControlDeviceStatusSeq getControlDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the names of all \ref SensorDevice "SensorDevices" for the robot.
- * @return The names of all \ref SensorDevice "ControlDevices" for the robot.
- */
- Ice::StringSeq getSensorDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the names of all \ref SensorDevice "SensorDevices" for the robot.
+ * @return The names of all \ref SensorDevice "ControlDevices" for the robot.
+ */
+ Ice::StringSeq getSensorDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Return the \ref SensorDeviceDescription for the given \ref SensorDevice
- * @param name The \ref SensorDevice
- * @return The \ref SensorDeviceDescription
- * @see SensorDeviceDescription
- * @see getSensorDeviceDescriptions
- */
- SensorDeviceDescription getSensorDeviceDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Return the \ref SensorDeviceDescription "SensorDeviceDescriptions" for all \ref SensorDevice "SensorDevices"
- * @return The \ref SensorDeviceDescription "SensorDeviceDescriptions"
- * @see SensorDeviceDescription
- * @see getSensorDeviceDescription
- */
- SensorDeviceDescriptionSeq getSensorDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Return the \ref SensorDeviceDescription for the given \ref SensorDevice
+ * @param name The \ref SensorDevice
+ * @return The \ref SensorDeviceDescription
+ * @see SensorDeviceDescription
+ * @see getSensorDeviceDescriptions
+ */
+ SensorDeviceDescription getSensorDeviceDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Return the \ref SensorDeviceDescription "SensorDeviceDescriptions" for all \ref SensorDevice "SensorDevices"
+ * @return The \ref SensorDeviceDescription "SensorDeviceDescriptions"
+ * @see SensorDeviceDescription
+ * @see getSensorDeviceDescription
+ */
+ SensorDeviceDescriptionSeq getSensorDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Return the \ref SensorDeviceStatus for the given \ref SensorDevice
- * @param name The \ref SensorDevice
- * @return The \ref SensorDeviceStatus
- * @see SensorDeviceStatus
- * @see getSensorDeviceStatuses
- */
- SensorDeviceStatus getSensorDeviceStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Return the \ref SensorDeviceStatus "SensorDeviceStatuses" for all \ref SensorDevice "SensorDevices"
- * @return The \ref SensorDeviceStatus "SensorDeviceStatuses"
- * @see SensorDeviceStatus
- * @see getSensorDeviceStatus
- */
- SensorDeviceStatusSeq getSensorDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////// Module interface /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Returns the number of \ref ControlDevice "ControlDevices"
- * @return The number of \ref ControlDevice "ControlDevices"
- */
- std::size_t getNumberOfControlDevices() const;
- /**
- * @brief Returns the number of \ref SensorDevice "SensorDevices"
- * @return The number of \ref SensorDevice "SensorDevices"
- */
- std::size_t getNumberOfSensorDevices() const;
+ /**
+ * @brief Return the \ref SensorDeviceStatus for the given \ref SensorDevice
+ * @param name The \ref SensorDevice
+ * @return The \ref SensorDeviceStatus
+ * @see SensorDeviceStatus
+ * @see getSensorDeviceStatuses
+ */
+ SensorDeviceStatus getSensorDeviceStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Return the \ref SensorDeviceStatus "SensorDeviceStatuses" for all \ref SensorDevice "SensorDevices"
+ * @return The \ref SensorDeviceStatus "SensorDeviceStatuses"
+ * @see SensorDeviceStatus
+ * @see getSensorDeviceStatus
+ */
+ SensorDeviceStatusSeq getSensorDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// Module interface /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns the number of \ref ControlDevice "ControlDevices"
+ * @return The number of \ref ControlDevice "ControlDevices"
+ */
+ std::size_t getNumberOfControlDevices() const;
+ /**
+ * @brief Returns the number of \ref SensorDevice "SensorDevices"
+ * @return The number of \ref SensorDevice "SensorDevices"
+ */
+ std::size_t getNumberOfSensorDevices() const;
- /**
- * @brief Return the \ref ControlDeviceDescription for the given \ref ControlDevice
- * @param idx The \ref ControlDevice's index
- * @return The \ref ControlDeviceDescription for the given \ref ControlDevice
- * @see ControlDeviceDescription
- * @see getControlDeviceDescription
- * @see getControlDeviceDescriptions
- */
- ControlDeviceDescription getControlDeviceDescription(std::size_t idx) const;
- /**
- * @brief Return the \ref ControlDeviceStatus for the given \ref ControlDevice
- * @param idx The \ref ControlDevice's index
- * @return The \ref ControlDeviceStatus
- * @see ControlDeviceStatus
- * @see getControlDeviceStatus
- * @see getControlDeviceStatuses
- */
- ControlDeviceStatus getControlDeviceStatus(std::size_t idx) const;
+ /**
+ * @brief Return the \ref ControlDeviceDescription for the given \ref ControlDevice
+ * @param idx The \ref ControlDevice's index
+ * @return The \ref ControlDeviceDescription for the given \ref ControlDevice
+ * @see ControlDeviceDescription
+ * @see getControlDeviceDescription
+ * @see getControlDeviceDescriptions
+ */
+ ControlDeviceDescription getControlDeviceDescription(std::size_t idx) const;
+ /**
+ * @brief Return the \ref ControlDeviceStatus for the given \ref ControlDevice
+ * @param idx The \ref ControlDevice's index
+ * @return The \ref ControlDeviceStatus
+ * @see ControlDeviceStatus
+ * @see getControlDeviceStatus
+ * @see getControlDeviceStatuses
+ */
+ ControlDeviceStatus getControlDeviceStatus(std::size_t idx) const;
- /**
- * @brief Return the \ref SensorDeviceDescription for the given \ref SensorDevice
- * @param idx The \ref SensorDevice's index
- * @return The \ref SensorDeviceDescription for the given \ref SensorDevice
- * @see SensorDeviceDescription
- * @see getSensorDeviceDescription
- * @see getSensorDeviceDescriptions
- */
- SensorDeviceDescription getSensorDeviceDescription(std::size_t idx) const;
- /**
- * @brief Return the \ref SensorDeviceStatus for the given \ref SensorDevice
- * @param idx The \ref SensorDevice's index
- * @return The \ref SensorDeviceStatus
- * @see SensorDeviceStatus
- * @see getSensorDeviceStatus
- * @see getSensorDeviceStatuses
- */
- SensorDeviceStatus getSensorDeviceStatus(std::size_t idx) const;
+ /**
+ * @brief Return the \ref SensorDeviceDescription for the given \ref SensorDevice
+ * @param idx The \ref SensorDevice's index
+ * @return The \ref SensorDeviceDescription for the given \ref SensorDevice
+ * @see SensorDeviceDescription
+ * @see getSensorDeviceDescription
+ * @see getSensorDeviceDescriptions
+ */
+ SensorDeviceDescription getSensorDeviceDescription(std::size_t idx) const;
+ /**
+ * @brief Return the \ref SensorDeviceStatus for the given \ref SensorDevice
+ * @param idx The \ref SensorDevice's index
+ * @return The \ref SensorDeviceStatus
+ * @see SensorDeviceStatus
+ * @see getSensorDeviceStatus
+ * @see getSensorDeviceStatuses
+ */
+ SensorDeviceStatus getSensorDeviceStatus(std::size_t idx) const;
- /**
- * @brief Updates the given VirtualRobot with the given \ref SensorValue "SensorValues"
- * @param robot The VirtualRobot to update
- * @param sensors The \ref SensorValue "SensorValues"
- */
- template<class PtrT>
- void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<PtrT>& sensors) const
- {
- updateVirtualRobotFromSensorValues(robot, robot->getRobotNodes(), sensors);
- }
- /**
- * @brief Updates the given VirtualRobot with the given \ref SensorValue "SensorValues"
- *
- * This overload is slightly faster than the other version.
- * @param robot The VirtualRobot to update
- * @param nodes The VirtualRobot's RobotNodes
- * @param sensors The \ref SensorValue "SensorValues"
- */
- template<class PtrT>
- void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes, const std::vector<PtrT>& sensors) const
+ /**
+ * @brief Updates the given VirtualRobot with the given \ref SensorValue "SensorValues"
+ * @param robot The VirtualRobot to update
+ * @param sensors The \ref SensorValue "SensorValues"
+ */
+ template<class PtrT>
+ void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<PtrT>& sensors) const
+ {
+ updateVirtualRobotFromSensorValues(robot, robot->getRobotNodes(), sensors);
+ }
+ /**
+ * @brief Updates the given VirtualRobot with the given \ref SensorValue "SensorValues"
+ *
+ * This overload is slightly faster than the other version.
+ * @param robot The VirtualRobot to update
+ * @param nodes The VirtualRobot's RobotNodes
+ * @param sensors The \ref SensorValue "SensorValues"
+ */
+ template<class PtrT>
+ void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes, const std::vector<PtrT>& sensors) const
+ {
+ for (const SimoxRobotSensorValueMapping& m : simoxRobotSensorValueMapping)
{
- for (const SimoxRobotSensorValueMapping& m : simoxRobotSensorValueMapping)
- {
- const SensorValueBase* sv = sensors.at(m.idxSens);
- ARMARX_CHECK_NOT_NULL(sv);
- const SensorValue1DoFActuatorPosition* svPos = sv->asA<SensorValue1DoFActuatorPosition>();
- ARMARX_CHECK_NOT_NULL(svPos);
- nodes.at(m.idxRobot)->setJointValueNoUpdate(svPos->position);
- }
- robot->applyJointValues();
+ const SensorValueBase* sv = sensors.at(m.idxSens);
+ ARMARX_CHECK_NOT_NULL(sv);
+ const SensorValue1DoFActuatorPosition* svPos = sv->asA<SensorValue1DoFActuatorPosition>();
+ ARMARX_CHECK_NOT_NULL(svPos);
+ nodes.at(m.idxRobot)->setJointValueNoUpdate(svPos->position);
}
+ robot->applyJointValues();
+ }
- /**
- * @brief Returns the groups of \ref ControlDevice "ControlDevices" required to be in the same
- * hardware control mode. (explained \ref RobotUnitCtrlModeGroups "here")
- * @return The groups of \ref ControlDevice "ControlDevices" required to be in the same
- * hardware control mode.
- */
- const ControlDeviceHardwareControlModeGroups& getControlModeGroups() const
- {
- return ctrlModeGroups;
- }
+ /**
+ * @brief Returns the groups of \ref ControlDevice "ControlDevices" required to be in the same
+ * hardware control mode. (explained \ref RobotUnitCtrlModeGroups "here")
+ * @return The groups of \ref ControlDevice "ControlDevices" required to be in the same
+ * hardware control mode.
+ */
+ const ControlDeviceHardwareControlModeGroups& getControlModeGroups() const
+ {
+ return ctrlModeGroups;
+ }
- /**
- * @brief Returns the \ref ControlDevice's index
- * @param deviceName \ref ControlDevice's name.
- * @return The \ref ControlDevice's index
- */
- std::size_t getControlDeviceIndex(const std::string& deviceName) const;
- /**
- * @brief Returns the \ref SensorDevice's index
- * @param deviceName \ref SensorDevice's name.
- * @return The \ref SensorDevice's index
- */
- std::size_t getSensorDeviceIndex(const std::string& deviceName) const;
+ /**
+ * @brief Returns the \ref ControlDevice's index
+ * @param deviceName \ref ControlDevice's name.
+ * @return The \ref ControlDevice's index
+ */
+ std::size_t getControlDeviceIndex(const std::string& deviceName) const;
+ /**
+ * @brief Returns the \ref SensorDevice's index
+ * @param deviceName \ref SensorDevice's name.
+ * @return The \ref SensorDevice's index
+ */
+ std::size_t getSensorDeviceIndex(const std::string& deviceName) const;
- /**
- * @brief Returns the \ref ControlDevice
- * @param deviceName \ref ControlDevice's name.
- * @return The \ref ControlDevice
- */
- ConstControlDevicePtr getControlDevice(const std::string& deviceName) const; /// TODO move to attorney for NJointController
- /**
- * @brief Returns the \ref SensorDevice
- * @param deviceName \ref SensorDevice's name.
- * @return The \ref SensorDevice
- */
- ConstSensorDevicePtr getSensorDevice(const std::string& deviceName) const; /// TODO move to attorney for NJointController
+ /**
+ * @brief Returns the \ref ControlDevice
+ * @param deviceName \ref ControlDevice's name.
+ * @return The \ref ControlDevice
+ */
+ ConstControlDevicePtr getControlDevice(const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
+ /**
+ * @brief Returns the \ref SensorDevice
+ * @param deviceName \ref SensorDevice's name.
+ * @return The \ref SensorDevice
+ */
+ ConstSensorDevicePtr getSensorDevice(const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
- /**
- * @brief Returns all \ref SensorDevice "SensorDevices"
- * @return All \ref SensorDevice "SensorDevices"
- */
- const KeyValueVector<std::string, SensorDevicePtr>& getSensorDevices() const ///TODO remove from interface
- {
- return sensorDevices;
- }
+ /**
+ * @brief Returns all \ref SensorDevice "SensorDevices"
+ * @return All \ref SensorDevice "SensorDevices"
+ */
+ const KeyValueVector<std::string, SensorDevicePtr>& getSensorDevices() const ///TODO remove from interface
+ {
+ return sensorDevices;
+ }
- /**
- * @brief Returns all \ref ControlDevice "ControlDevices"
- * @return All \ref ControlDevice "ControlDevices"
- */
- const KeyValueVector<std::string, ControlDevicePtr>& getControlDevices() const ///TODO remove from interface
- {
- return controlDevices;
- }
+ /**
+ * @brief Returns all \ref ControlDevice "ControlDevices"
+ * @return All \ref ControlDevice "ControlDevices"
+ */
+ const KeyValueVector<std::string, ControlDevicePtr>& getControlDevices() const ///TODO remove from interface
+ {
+ return controlDevices;
+ }
- /**
- * @brief Returns const pointers to all \ref SensorDevice "SensorDevices"
- * @return Const pointers to all \ref SensorDevice "SensorDevices"
- */
- const std::map<std::string, ConstSensorDevicePtr>& getSensorDevicesConstPtr() const ///TODO rename to getSensorDevices
- {
- return sensorDevicesConstPtr;
- }
- /**
- * @brief Returns const pointers to all \ref ControlDevice "ControlDevices"
- * @return Const pointers to all \ref ControlDevice "ControlDevices"
- */
- const std::map<std::string, ConstControlDevicePtr>& getControlDevicesConstPtr() const ///TODO rename to getControlDevices
- {
- return controlDevicesConstPtr;
- }
- protected:
- /**
- * @brief Adds a \ref ControlDevice to the robot.
- * \warning Must not be called when the \ref RobotUnit's \ref RobotUnitState "state" is not \ref RobotUnitState::InitializingDevices
- * @param cd The \ref ControlDevice to add
- */
- void addControlDevice(const ControlDevicePtr& cd);
- /**
- * @brief Adds a \ref SensorDevice to the robot.
- * \warning Must not be called when the \ref RobotUnit's \ref RobotUnitState "state" is not \ref RobotUnitState::InitializingDevices
- * @param cd The \ref SensorDevice to add
- */
- void addSensorDevice(const SensorDevicePtr& sd);
- /**
- * @brief Creates the \ref SensorDevice used to log timings in the \ref ControlThread
- * (This function is supposed to be used in the \ref ModuleBase::finishDeviceInitialization)
- * @return The \ref SensorDevice used to log timings in the \ref ControlThread
- * @see rtGetRTThreadTimingsSensorDevice
- */
- virtual RTThreadTimingsSensorDevicePtr createRTThreadTimingSensorDevice() const;
- /**
- * @brief Returns the \ref SensorDevice used to log timings in the \ref ControlThread.
- * (This function is supposed to be used in the \ref ControlThread)
- * @return The \ref SensorDevice used to log timings in the \ref ControlThread
- * @see createRTThreadTimingSensorDevice
- */
- RTThreadTimingsSensorDevice& rtGetRTThreadTimingsSensorDevice()
- {
- return *rtThreadTimingsSensorDevice;
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////// implementation //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * @brief Returns the stop movement \ref JointController "JointControllers"
- * for all \ref ControlDevice "ControlDevices"
- * @return The stop movement \ref JointController "JointControllers"
- */
- std::vector<JointController*> getStopMovementJointControllers() const;
- /**
- * @brief Returns the emergency stop \ref JointController "JointControllers"
- * for all \ref ControlDevice "ControlDevices"
- * @return The emergency stop \ref JointController "JointControllers"
- */
- std::vector<JointController*> getEmergencyStopJointControllers() const;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief Struct correlating a \ref VirtualRobot::RobotNode to a \ref SensorDevice
- struct SimoxRobotSensorValueMapping
- {
- /// @brief The RobotNode's index
- std::size_t idxRobot;
- /// @brief The \ref SensorDevice's index
- std::size_t idxSens;
- };
- /// @brief Mapping from RobotNode index to \ref SensorDevice
- std::vector<SimoxRobotSensorValueMapping> simoxRobotSensorValueMapping;
- /// @brief Pointers to all \ref SensorValueBase "SensorValues"
- std::vector<const SensorValueBase*> sensorValues;
- /// @brief Pointers to all \ref ControlTargetBase "ControlTargets"
- std::vector<std::vector<PropagateConst<ControlTargetBase*>>> controlTargets;
- public:
- /// @brief Holds data about a device group requiring the same hardware control mode. (explained \ref RobotUnitCtrlModeGroups "here")
- struct ControlDeviceHardwareControlModeGroups
- {
- /// @brief contains a vector per group.
- /// Each vector contains the names of devices in this group.
- std::vector<std::set<std::string>> groups;
- /// @brief contains a vector per group.
- /// Each vector contains the indices of devices in this group.
- std::vector<std::vector<std::size_t>> deviceIndices;
- /// @brief contains the names of all devices in any group
- std::set<std::string> groupsMerged;
- /// @brief contains the group index for each device (or the sentinel)
- std::vector<std::size_t> groupIndices;
- };
- private:
- //ctrl dev
- /// @brief \ref ControlDevice "ControlDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
- KeyValueVector<std::string, ControlDevicePtr> controlDevices;
- /// @brief const pointer to all ControlDevices (passed to GenerateConfigDescription of a NJointController)
- std::map<std::string, ConstControlDevicePtr> controlDevicesConstPtr;
- /// @brief Guards access to all \ref ControlDevice "ControlDevices"
- mutable MutexType controlDevicesMutex;
+ /**
+ * @brief Returns const pointers to all \ref SensorDevice "SensorDevices"
+ * @return Const pointers to all \ref SensorDevice "SensorDevices"
+ */
+ const std::map<std::string, ConstSensorDevicePtr>& getSensorDevicesConstPtr() const ///TODO rename to getSensorDevices
+ {
+ return sensorDevicesConstPtr;
+ }
+ /**
+ * @brief Returns const pointers to all \ref ControlDevice "ControlDevices"
+ * @return Const pointers to all \ref ControlDevice "ControlDevices"
+ */
+ const std::map<std::string, ConstControlDevicePtr>& getControlDevicesConstPtr() const ///TODO rename to getControlDevices
+ {
+ return controlDevicesConstPtr;
+ }
+ protected:
+ /**
+ * @brief Adds a \ref ControlDevice to the robot.
+ * \warning Must not be called when the \ref RobotUnit's \ref RobotUnitState "state" is not \ref RobotUnitState::InitializingDevices
+ * @param cd The \ref ControlDevice to add
+ */
+ void addControlDevice(const ControlDevicePtr& cd);
+ /**
+ * @brief Adds a \ref SensorDevice to the robot.
+ * \warning Must not be called when the \ref RobotUnit's \ref RobotUnitState "state" is not \ref RobotUnitState::InitializingDevices
+ * @param cd The \ref SensorDevice to add
+ */
+ void addSensorDevice(const SensorDevicePtr& sd);
+ /**
+ * @brief Creates the \ref SensorDevice used to log timings in the \ref ControlThread
+ * (This function is supposed to be used in the \ref ModuleBase::finishDeviceInitialization)
+ * @return The \ref SensorDevice used to log timings in the \ref ControlThread
+ * @see rtGetRTThreadTimingsSensorDevice
+ */
+ virtual RTThreadTimingsSensorDevicePtr createRTThreadTimingSensorDevice() const;
+ /**
+ * @brief Returns the \ref SensorDevice used to log timings in the \ref ControlThread.
+ * (This function is supposed to be used in the \ref ControlThread)
+ * @return The \ref SensorDevice used to log timings in the \ref ControlThread
+ * @see createRTThreadTimingSensorDevice
+ */
+ RTThreadTimingsSensorDevice& rtGetRTThreadTimingsSensorDevice()
+ {
+ return *rtThreadTimingsSensorDevice;
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * @brief Returns the stop movement \ref JointController "JointControllers"
+ * for all \ref ControlDevice "ControlDevices"
+ * @return The stop movement \ref JointController "JointControllers"
+ */
+ std::vector<JointController*> getStopMovementJointControllers() const;
+ /**
+ * @brief Returns the emergency stop \ref JointController "JointControllers"
+ * for all \ref ControlDevice "ControlDevices"
+ * @return The emergency stop \ref JointController "JointControllers"
+ */
+ std::vector<JointController*> getEmergencyStopJointControllers() const;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief Struct correlating a \ref VirtualRobot::RobotNode to a \ref SensorDevice
+ struct SimoxRobotSensorValueMapping
+ {
+ /// @brief The RobotNode's index
+ std::size_t idxRobot;
+ /// @brief The \ref SensorDevice's index
+ std::size_t idxSens;
+ };
+ /// @brief Mapping from RobotNode index to \ref SensorDevice
+ std::vector<SimoxRobotSensorValueMapping> simoxRobotSensorValueMapping;
+ /// @brief Pointers to all \ref SensorValueBase "SensorValues"
+ std::vector<const SensorValueBase*> sensorValues;
+ /// @brief Pointers to all \ref ControlTargetBase "ControlTargets"
+ std::vector<std::vector<PropagateConst<ControlTargetBase*>>> controlTargets;
+ public:
+ /// @brief Holds data about a device group requiring the same hardware control mode. (explained \ref RobotUnitCtrlModeGroups "here")
+ struct ControlDeviceHardwareControlModeGroups
+ {
+ /// @brief contains a vector per group.
+ /// Each vector contains the names of devices in this group.
+ std::vector<std::set<std::string>> groups;
+ /// @brief contains a vector per group.
+ /// Each vector contains the indices of devices in this group.
+ std::vector<std::vector<std::size_t>> deviceIndices;
+ /// @brief contains the names of all devices in any group
+ std::set<std::string> groupsMerged;
+ /// @brief contains the group index for each device (or the sentinel)
+ std::vector<std::size_t> groupIndices;
+ };
+ private:
+ //ctrl dev
+ /// @brief \ref ControlDevice "ControlDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
+ KeyValueVector<std::string, ControlDevicePtr> controlDevices;
+ /// @brief const pointer to all ControlDevices (passed to GenerateConfigDescription of a NJointControllerBase)
+ std::map<std::string, ConstControlDevicePtr> controlDevicesConstPtr;
+ /// @brief Guards access to all \ref ControlDevice "ControlDevices"
+ mutable MutexType controlDevicesMutex;
- /// @brief Device groups requiring the same hardware control mode.
- ControlDeviceHardwareControlModeGroups ctrlModeGroups;
+ /// @brief Device groups requiring the same hardware control mode.
+ ControlDeviceHardwareControlModeGroups ctrlModeGroups;
- //sens dev
- /// @brief \ref SensorDevice "SensorDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
- KeyValueVector<std::string, SensorDevicePtr > sensorDevices;
- /// @brief const pointer to all SensorDevices (passed to GenerateConfigDescription of a NJointController)
- std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
- /// @brief Guards access to all \ref SensorDevice "SensorDevices"
- mutable MutexType sensorDevicesMutex;
+ //sens dev
+ /// @brief \ref SensorDevice "SensorDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
+ KeyValueVector<std::string, SensorDevicePtr > sensorDevices;
+ /// @brief const pointer to all SensorDevices (passed to GenerateConfigDescription of a NJointControllerBase)
+ std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
+ /// @brief Guards access to all \ref SensorDevice "SensorDevices"
+ mutable MutexType sensorDevicesMutex;
- /// @brief a pointer to the RTThreadTimingsSensorDevice used to meassure timings in the rt loop
- RTThreadTimingsSensorDevicePtr rtThreadTimingsSensorDevice;
- /// @brief The device name of rtThreadTimingsSensorDevice
- static const std::string rtThreadTimingsSensorDeviceName;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref Publisher.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class DevicesAttorneyForPublisher;
- /**
- * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class DevicesAttorneyForControlThread;
- /**
- * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThreadDataBuffer.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class DevicesAttorneyForControlThreadDataBuffer;
- /**
- * \brief This class allows minimal access to private members of \ref armarx::RobotUnitModule::Devices in a sane fashion for \ref armarx::NJointController.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class DevicesAttorneyForNJointController;
- };
- }
+ /// @brief a pointer to the RTThreadTimingsSensorDevice used to meassure timings in the rt loop
+ RTThreadTimingsSensorDevicePtr rtThreadTimingsSensorDevice;
+ /// @brief The device name of rtThreadTimingsSensorDevice
+ static const std::string rtThreadTimingsSensorDeviceName;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref Publisher.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class DevicesAttorneyForPublisher;
+ /**
+ * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class DevicesAttorneyForControlThread;
+ /**
+ * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThreadDataBuffer.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class DevicesAttorneyForControlThreadDataBuffer;
+ /**
+ * \brief This class allows minimal access to private members of \ref armarx::RobotUnitModule::Devices in a sane fashion for \ref armarx::NJointControllerBase.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class DevicesAttorneyForNJointController;
+ };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 2906c13427f768dd2a33f582f2acbc1ecaa19675..f5d357ce82651f0f5aa52c94bf8015a158f85f75 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -30,204 +30,214 @@
#include "../util/ControlThreadOutputBuffer.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
{
- void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ if (!rtLoggingEntries.count(token->getId()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
- if (!rtLoggingEntries.count(token->getId()))
- {
- throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
- }
- rtLoggingEntries.at(token->getId())->marker = marker;
+ throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
}
+ rtLoggingEntries.at(token->getId())->marker = marker;
+ }
- RemoteReferenceCounterBasePtr Logging::startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current&)
+ RemoteReferenceCounterBasePtr Logging::startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current&)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ StringStringDictionary alias;
+ for (const auto& name : loggingNames)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- StringStringDictionary alias;
- for (const auto& name : loggingNames)
- {
- alias.emplace(name, "");
- }
- return startRtLoggingWithAliasNames(formatString, alias, Ice::emptyCurrent);
+ alias.emplace(name, "");
}
+ return startRtLoggingWithAliasNames(formatString, alias, Ice::emptyCurrent);
+ }
- void Logging::stopRtLogging(const RemoteReferenceCounterBasePtr& token, const Ice::Current&)
+ void Logging::stopRtLogging(const RemoteReferenceCounterBasePtr& token, const Ice::Current&)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ if (!rtLoggingEntries.count(token->getId()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
- if (!rtLoggingEntries.count(token->getId()))
- {
- throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
- }
- ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << rtLoggingEntries.at(token->getId())->filename;
- rtLoggingEntries.at(token->getId())->stopLogging = true;
+ throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
}
+ ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << rtLoggingEntries.at(token->getId())->filename;
+ rtLoggingEntries.at(token->getId())->stopLogging = true;
+ }
- Ice::StringSeq Logging::getLoggingNames(const Ice::Current&) const
+ Ice::StringSeq Logging::getLoggingNames(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ Ice::StringSeq result;
+ for (const auto& strs : sensorDeviceValueLoggingNames)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- Ice::StringSeq result;
- for (const auto& strs : sensorDeviceValueLoggingNames)
+ result.insert(result.end(), strs.begin(), strs.end());
+ }
+ for (const auto& strvecss : controlDeviceValueLoggingNames)
+ {
+ for (const auto& strs : strvecss)
{
result.insert(result.end(), strs.begin(), strs.end());
}
- for (const auto& strvecss : controlDeviceValueLoggingNames)
- {
- for (const auto& strs : strvecss)
- {
- result.insert(result.end(), strs.begin(), strs.end());
- }
- }
- return result;
}
+ return result;
+ }
- RemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current&)
+ RemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current&)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ FileSystemPathBuilder pb {formatString};
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ if (rtLoggingEntries.count(pb.getPath()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- FileSystemPathBuilder pb {formatString};
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
- if (rtLoggingEntries.count(pb.getPath()))
- {
- throw InvalidArgumentException {"There already is a logger for the path '" + pb.getPath() + "'"};
- }
- rtLoggingEntries[pb.getPath()].reset(new CSVLoggingEntry());
- auto ptr = rtLoggingEntries[pb.getPath()];
- CSVLoggingEntry& e = *ptr;
- e.filename = pb.getPath();
- pb.createParentDirectories();
- e.stream.open(e.filename);
- if (!e.stream)
- {
- rtLoggingEntries.erase(pb.getPath());
- throw LogicError {"RtLogging could not open filestream for '" + pb.getPath() + "'"};
- }
+ throw InvalidArgumentException {"There already is a logger for the path '" + pb.getPath() + "'"};
+ }
+ rtLoggingEntries[pb.getPath()].reset(new CSVLoggingEntry());
+ auto ptr = rtLoggingEntries[pb.getPath()];
+ CSVLoggingEntry& e = *ptr;
+ e.filename = pb.getPath();
+ pb.createParentDirectories();
+ e.stream.open(e.filename);
+ if (!e.stream)
+ {
+ rtLoggingEntries.erase(pb.getPath());
+ throw LogicError {"RtLogging could not open filestream for '" + pb.getPath() + "'"};
+ }
- std::stringstream header;
- header << "marker;iteration;timestamp";
- auto logDev = [&](const std::string & dev)
+ std::stringstream header;
+ header << "marker;iteration;timestamp";
+ auto logDev = [&](const std::string & dev)
+ {
+ for (const auto& pair : aliasNames)
{
- for (const auto& pair : aliasNames)
+ std::string name = pair.first;
+ if (boost::starts_with(dev + "$", name))
{
- std::string name = pair.first;
- if (boost::starts_with(dev + "$", name))
+ if ((name.size() > 0 && name.back() == '$'))
{
- if ((name.size() > 0 && name.back() == '$'))
- {
- name.resize(name.size() - 1);
- }
- const std::string& alias = pair.second.empty() ? name : pair.second;
- header << ";" << alias << dev.substr(name.size());
- return true;
+ name.resize(name.size() - 1);
}
+ const std::string& alias = pair.second.empty() ? name : pair.second;
+ header << ";" << alias << dev.substr(name.size());
+ return true;
}
- return false;
- };
+ }
+ return false;
+ };
- //get logged sensor device values
+ //get logged sensor device values
+ {
+ e.loggedSensorDeviceValues.reserve(sensorDeviceValueLoggingNames.size());
+ for (const auto& svfields : sensorDeviceValueLoggingNames)
{
- e.loggedSensorDeviceValues.reserve(sensorDeviceValueLoggingNames.size());
- for (const auto& svfields : sensorDeviceValueLoggingNames)
+ e.loggedSensorDeviceValues.emplace_back();
+ auto& svfieldsFlags = e.loggedSensorDeviceValues.back(); //vv
+ svfieldsFlags.reserve(svfields.size());
+ for (const auto& field : svfields)
{
- e.loggedSensorDeviceValues.emplace_back();
- auto& svfieldsFlags = e.loggedSensorDeviceValues.back(); //vv
- svfieldsFlags.reserve(svfields.size());
- for (const auto& field : svfields)
- {
- svfieldsFlags.emplace_back(logDev(field));
- }
+ svfieldsFlags.emplace_back(logDev(field));
}
}
- //get logged control device values
+ }
+ //get logged control device values
+ {
+ e.loggedControlDeviceValues.reserve(controlDeviceValueLoggingNames.size());
+ for (const auto& sjctrls : controlDeviceValueLoggingNames)
{
- e.loggedControlDeviceValues.reserve(controlDeviceValueLoggingNames.size());
- for (const auto& sjctrls : controlDeviceValueLoggingNames)
+ e.loggedControlDeviceValues.emplace_back();
+ auto& sjctrlsFlags = e.loggedControlDeviceValues.back(); //vv
+ sjctrlsFlags.reserve(sjctrls.size());
+ for (const auto& ctargfields : sjctrls)
{
- e.loggedControlDeviceValues.emplace_back();
- auto& sjctrlsFlags = e.loggedControlDeviceValues.back(); //vv
- sjctrlsFlags.reserve(sjctrls.size());
- for (const auto& ctargfields : sjctrls)
- {
- sjctrlsFlags.emplace_back();
- auto& ctargfieldsFlags = sjctrlsFlags.back(); //v
- ctargfieldsFlags.reserve(ctargfields.size());
+ sjctrlsFlags.emplace_back();
+ auto& ctargfieldsFlags = sjctrlsFlags.back(); //v
+ ctargfieldsFlags.reserve(ctargfields.size());
- for (const auto& field : ctargfields)
- {
- ctargfieldsFlags.emplace_back(logDev(field));
- }
+ for (const auto& field : ctargfields)
+ {
+ ctargfieldsFlags.emplace_back(logDev(field));
}
}
}
-
- //write header
- e.stream << header.str() << std::flush; // newline is written at the beginning of each log line
- //create and return handle
- auto block = getArmarXManager()->createRemoteReferenceCount(
- [ptr]()
- {
- ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << ptr->filename;
- ptr->stopLogging = true;
- }, e.filename);
- auto counter = block->getReferenceCounter();
- block->activateCounting();
- ARMARX_DEBUG_S << "RobotUnit: start RtLogging for file " << ptr->filename;
- return counter;
}
- void Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
+ //write header
+ e.stream << header.str() << std::flush; // newline is written at the beginning of each log line
+ //create and return handle
+ auto block = getArmarXManager()->createRemoteReferenceCount(
+ [ptr]()
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
- FileSystemPathBuilder pb {formatString};
- pb.createParentDirectories();
- std::ofstream outCSV {pb.getPath() + ".csv"};
- if (!outCSV)
- {
- throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".csv'"};
- }
- std::ofstream outMsg {pb.getPath() + ".messages"};
- if (!outMsg)
+ ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << ptr->filename;
+ ptr->stopLogging = true;
+ }, e.filename);
+ auto counter = block->getReferenceCounter();
+ block->activateCounting();
+ ARMARX_DEBUG_S << "RobotUnit: start RtLogging for file " << ptr->filename;
+ return counter;
+ }
+
+ void Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ FileSystemPathBuilder pb {formatString};
+ pb.createParentDirectories();
+ std::ofstream outCSV {pb.getPath() + ".csv"};
+ if (!outCSV)
+ {
+ throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".csv'"};
+ }
+ std::ofstream outMsg {pb.getPath() + ".messages"};
+ if (!outMsg)
+ {
+ throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".messages'"};
+ }
+ ARMARX_INFO << "writing the last " << backlog.size() << " iterations to " << pb.getPath() << ".{csv, messages}";
+ //write csv header
+ {
+ outCSV << "iteration;timestamp";
+ for (const auto& vs : sensorDeviceValueLoggingNames)
{
- throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".messages'"};
+ for (const auto& s : vs)
+ {
+ outCSV << ";" << s;
+ }
}
- ARMARX_INFO << "writing the last " << backlog.size() << " iterations to " << pb.getPath() << ".{csv, messages}";
- //write csv header
+ for (const auto& vvs : controlDeviceValueLoggingNames)
{
- outCSV << "iteration;timestamp";
- for (const auto& vs : sensorDeviceValueLoggingNames)
+ for (const auto& vs : vvs)
{
for (const auto& s : vs)
{
outCSV << ";" << s;
}
}
- for (const auto& vvs : controlDeviceValueLoggingNames)
+ }
+ outCSV << std::endl;
+ }
+
+ for (const ::armarx::detail::ControlThreadOutputBufferEntry& iteration : backlog)
+ {
+ //write csv data
+ {
+ outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
+ //sens
{
- for (const auto& vs : vvs)
+ for (const SensorValueBase* val : iteration.sensors)
{
- for (const auto& s : vs)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- outCSV << ";" << s;
+ outCSV << ";" << val->getDataFieldAsString(idxField);
}
}
}
- outCSV << std::endl;
- }
-
- for (const ::armarx::detail::ControlThreadOutputBufferEntry& iteration : backlog)
- {
- //write csv data
+ //ctrl
{
- outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
- //sens
+ for (const auto& vals : iteration.control)
{
- for (const SensorValueBase* val : iteration.sensors)
+ for (const ControlTargetBase* val : vals)
{
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
@@ -235,288 +245,275 @@ namespace armarx
}
}
}
- //ctrl
- {
- for (const auto& vals : iteration.control)
- {
- for (const ControlTargetBase* val : vals)
- {
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
- {
- outCSV << ";" << val->getDataFieldAsString(idxField);
- }
- }
- }
- }
- outCSV << std::endl;
}
- //write message data
+ outCSV << std::endl;
+ }
+ //write message data
+ {
+ bool atLeastOneMessage = false;
+ for (const ::armarx::detail::RtMessageLogEntryBase* msg : iteration.messages.getEntries())
{
- bool atLeastOneMessage = false;
- for (const ::armarx::detail::RtMessageLogEntryBase* msg : iteration.messages.getEntries())
- {
- if (!msg)
- {
- break;
- }
- outMsg << "[" << msg->getTime().toDateTime() << "] iteration "
- << iteration.iteration << ":\n"
- << msg->format() << std::endl;
- atLeastOneMessage = true;
- }
- if (atLeastOneMessage)
+ if (!msg)
{
- outMsg << "\nmessages lost: " << iteration.messages.messagesLost
- << " (required additional "
- << iteration.messages.requiredAdditionalBufferSpace << " bytes, "
- << iteration.messages.requiredAdditionalEntries << " message entries)\n" << std::endl;
+ break;
}
+ outMsg << "[" << msg->getTime().toDateTime() << "] iteration "
+ << iteration.iteration << ":\n"
+ << msg->format() << std::endl;
+ atLeastOneMessage = true;
+ }
+ if (atLeastOneMessage)
+ {
+ outMsg << "\nmessages lost: " << iteration.messages.messagesLost
+ << " (required additional "
+ << iteration.messages.requiredAdditionalBufferSpace << " bytes, "
+ << iteration.messages.requiredAdditionalEntries << " message entries)\n" << std::endl;
}
}
}
+ }
- void Logging::_preFinishRunning()
+ void Logging::_preFinishRunning()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ defaultLogHandle = nullptr;
+ if (rtLoggingTask)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- defaultLogHandle = nullptr;
- if (rtLoggingTask)
+ ARMARX_DEBUG << "shutting down rt logging task";
+ rtLoggingTask->stop();
+ while (rtLoggingTask->isFunctionExecuting() || rtLoggingTask->isRunning())
{
- ARMARX_DEBUG << "shutting down rt logging task";
- rtLoggingTask->stop();
- while (rtLoggingTask->isFunctionExecuting() || rtLoggingTask->isRunning())
- {
- ARMARX_FATAL << deactivateSpam(0.1) << "RT LOGGING TASK IS RUNNING AFTER IT WAS STOPPED!";
- }
- rtLoggingTask = nullptr;
- ARMARX_DEBUG << "shutting down rt logging task done";
+ ARMARX_FATAL << deactivateSpam(0.1) << "RT LOGGING TASK IS RUNNING AFTER IT WAS STOPPED!";
}
+ rtLoggingTask = nullptr;
+ ARMARX_DEBUG << "shutting down rt logging task done";
}
+ }
- void Logging::_preFinishControlThreadInitialization()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- controlThreadId = LogSender::getThreadId();
- ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
+ void Logging::_preFinishControlThreadInitialization()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ controlThreadId = LogSender::getThreadId();
+ ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
- ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestep;
- rtLoggingTask->start();
- }
+ ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestep;
+ rtLoggingTask->start();
+ }
- void Logging::doLogging()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
- const auto now = IceUtil::Time::now();
- // entries are removed last
+ void Logging::doLogging()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ const auto now = IceUtil::Time::now();
+ // entries are removed last
- //remove backlog entries
+ //remove backlog entries
+ {
+ while (!backlog.empty() && backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
{
- while (!backlog.empty() && backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
- {
- backlog.pop_front();
- }
+ backlog.pop_front();
}
- //log all
+ }
+ //log all
+ {
+ _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().foreachNewLoggingEntry(
+ [&](const ControlThreadOutputBuffer::Entry & data)
{
- _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().foreachNewLoggingEntry(
- [&](const ControlThreadOutputBuffer::Entry & data)
+ //base (marker;iteration;timestamp)
{
- //base (marker;iteration;timestamp)
+ for (auto& pair : rtLoggingEntries)
{
- for (auto& pair : rtLoggingEntries)
- {
- CSVLoggingEntry& e = *pair.second;
- e.stream << "\n"
- << e.marker << ";"
- << data.iteration << ";"
- << data.sensorValuesTimestamp.toMicroSeconds();
- e.marker.clear();
- }
+ CSVLoggingEntry& e = *pair.second;
+ e.stream << "\n"
+ << e.marker << ";"
+ << data.iteration << ";"
+ << data.sensorValuesTimestamp.toMicroSeconds();
+ e.marker.clear();
}
- //sens
+ }
+ //sens
+ {
+ //sensors
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
{
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
+ const SensorValueBase* val = data.sensors.at(idxDev);
+ //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- const SensorValueBase* val = data.sensors.at(idxDev);
- //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ const auto str = val->getDataFieldAsString(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
{
- if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
- {
- entry->stream << ";" << str;
- }
+ entry->stream << ";" << str;
}
}
}
}
- //ctrl
+ }
+ //ctrl
+ {
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
{
- //joint controllers
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
+ const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- const auto& vals = data.control.at(idxDev);
- //control value (e.g. v_platform)
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ const ControlTargetBase* val = vals.at(idxVal);
+ //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- const ControlTargetBase* val = vals.at(idxVal);
- //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ const auto str = val->getDataFieldAsString(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
{
- if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
- {
- entry->stream << ";" << str;
- }
+ entry->stream << ";" << str;
}
}
}
}
}
- //store data to backlog
+ }
+ //store data to backlog
+ {
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
- {
- backlog.emplace_back(data, true); //true for minimal copy
- }
+ backlog.emplace_back(data, true); //true for minimal copy
}
- //print + reset messages
+ }
+ //print + reset messages
+ {
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
{
- for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ if (!ptr)
{
- if (!ptr)
- {
- break;
- }
- ptr->print(controlThreadId);
+ break;
}
+ ptr->print(controlThreadId);
}
- });
- }
- ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
- //flush all files
- {
- for (auto& pair : rtLoggingEntries)
- {
- pair.second->stream << std::flush;
}
+ });
+ }
+ ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
+ //flush all files
+ {
+ for (auto& pair : rtLoggingEntries)
+ {
+ pair.second->stream << std::flush;
}
+ }
- //remove entries
+ //remove entries
+ {
+ std::vector<std::string> toRemove;
+ toRemove.reserve(rtLoggingEntries.size());
+ for (auto& pair : rtLoggingEntries)
{
- std::vector<std::string> toRemove;
- toRemove.reserve(rtLoggingEntries.size());
- for (auto& pair : rtLoggingEntries)
- {
- if (pair.second->stopLogging)
- {
- //can't remove the current elemet
- //(this would invalidate the current iterator)
- toRemove.emplace_back(pair.first);
- }
- }
- for (const auto& rem : toRemove)
+ if (pair.second->stopLogging)
{
- rtLoggingEntries.erase(rem);
+ //can't remove the current elemet
+ //(this would invalidate the current iterator)
+ toRemove.emplace_back(pair.first);
}
}
+ for (const auto& rem : toRemove)
+ {
+ rtLoggingEntries.erase(rem);
+ }
}
+ }
- void Logging::_postOnInitRobotUnit()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- rtLoggingTimestep = getProperty<std::size_t>("RTLogging_PeriodMs");
- ARMARX_CHECK_LESS_W_HINT(0, rtLoggingTimestep, "The property RTLoggingPeriodMs must not be 0");
+ void Logging::_postOnInitRobotUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ rtLoggingTimestep = getProperty<std::size_t>("RTLogging_PeriodMs");
+ ARMARX_CHECK_LESS_W_HINT(0, rtLoggingTimestep, "The property RTLoggingPeriodMs must not be 0");
- messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
- messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
- ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
+ messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
+ messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
- messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
- messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
- ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
+ messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
+ messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
- rtLoggingBacklogRetentionTime = IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
+ rtLoggingBacklogRetentionTime = IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
- ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
- }
+ ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+ }
- void Logging::_postFinishDeviceInitialization()
+ void Logging::_postFinishDeviceInitialization()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //init buffer
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //init buffer
- {
- std::size_t ctrlThreadPeriodUs = static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
- std::size_t logThreadPeriodUs = rtLoggingTimestep * 1000;
- std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
+ std::size_t ctrlThreadPeriodUs = static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
+ std::size_t logThreadPeriodUs = rtLoggingTimestep * 1000;
+ std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
- const auto bufferSize = _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
- nBuffers, _module<Devices>().getControlDevices(), _module<Devices>().getSensorDevices(),
- messageBufferSize, messageBufferNumberEntries,
- messageBufferMaxSize, messageBufferMaxNumberEntries);
- ARMARX_INFO << "RTLogging activated! using "
- << nBuffers << "buffers "
- << "(buffersize = " << bufferSize << " bytes)";
- }
- //init logging names
+ const auto bufferSize = _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
+ nBuffers, _module<Devices>().getControlDevices(), _module<Devices>().getSensorDevices(),
+ messageBufferSize, messageBufferNumberEntries,
+ messageBufferMaxSize, messageBufferMaxNumberEntries);
+ ARMARX_INFO << "RTLogging activated! using "
+ << nBuffers << "buffers "
+ << "(buffersize = " << bufferSize << " bytes)";
+ }
+ //init logging names
+ {
+ //sensorDevices
+ controlDeviceValueLoggingNames.reserve(_module<Devices>().getControlDevices().size());
+ for (const auto& cd : _module<Devices>().getControlDevices().values())
{
- //sensorDevices
- controlDeviceValueLoggingNames.reserve(_module<Devices>().getControlDevices().size());
- for (const auto& cd : _module<Devices>().getControlDevices().values())
+ controlDeviceValueLoggingNames.emplace_back();
+ auto& namesForDev = controlDeviceValueLoggingNames.back();
+ namesForDev.reserve(cd->getJointControllers().size());
+ for (auto jointC : cd->getJointControllers())
{
- controlDeviceValueLoggingNames.emplace_back();
- auto& namesForDev = controlDeviceValueLoggingNames.back();
- namesForDev.reserve(cd->getJointControllers().size());
- for (auto jointC : cd->getJointControllers())
- {
- const auto names = jointC->getControlTarget()->getDataFieldNames();
- namesForDev.emplace_back();
- auto& fullNames = namesForDev.back();
- fullNames.reserve(names.size());
- for (const auto& name : names)
- {
- fullNames.emplace_back(
- "ctrl." +
- cd->getDeviceName() + "." +
- jointC->getControlMode() + "." +
- name);
- }
- }
- }
- //sensorDevices
- sensorDeviceValueLoggingNames.reserve(_module<Devices>().getSensorDevices().size());
- for (const auto& sd : _module<Devices>().getSensorDevices().values())
- {
- const auto names = sd->getSensorValue()->getDataFieldNames();
- sensorDeviceValueLoggingNames.emplace_back();
- auto& fullNames = sensorDeviceValueLoggingNames.back();
+ const auto names = jointC->getControlTarget()->getDataFieldNames();
+ namesForDev.emplace_back();
+ auto& fullNames = namesForDev.back();
fullNames.reserve(names.size());
for (const auto& name : names)
{
fullNames.emplace_back(
- "sens." +
- sd->getDeviceName() + "." +
+ "ctrl." +
+ cd->getDeviceName() + "." +
+ jointC->getControlMode() + "." +
name);
}
}
}
- //start logging thread is done in rtinit
- //maybe add the default log
+ //sensorDevices
+ sensorDeviceValueLoggingNames.reserve(_module<Devices>().getSensorDevices().size());
+ for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
- const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
- if (!loggingpath.empty())
+ const auto names = sd->getSensorValue()->getDataFieldNames();
+ sensorDeviceValueLoggingNames.emplace_back();
+ auto& fullNames = sensorDeviceValueLoggingNames.back();
+ fullNames.reserve(names.size());
+ for (const auto& name : names)
{
- defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
+ fullNames.emplace_back(
+ "sens." +
+ sd->getDeviceName() + "." +
+ name);
}
}
- //setup thread (don't start it
- rtLoggingTask = new RTLoggingTaskT([&] {doLogging();}, rtLoggingTimestep, false, getName() + "_RTLoggingTask");
- rtLoggingTask->setDelayWarningTolerance(rtLoggingTimestep * 10);
}
+ //start logging thread is done in rtinit
+ //maybe add the default log
+ {
+ const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
+ if (!loggingpath.empty())
+ {
+ defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
+ }
+ }
+ //setup thread (don't start it
+ rtLoggingTask = new RTLoggingTaskT([&] {doLogging();}, rtLoggingTimestep, false, getName() + "_RTLoggingTask");
+ rtLoggingTask->setDelayWarningTolerance(rtLoggingTimestep * 10);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 4207da6dd7a4cf2ac3b54e85994fed76bd168000..68cc02b917827edbb5442a7ef28d7e47d50d3776 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -34,183 +34,180 @@
#include "RobotUnitModuleBase.h"
#include "../util/ControlThreadOutputBuffer.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ class LoggingPropertyDefinitions: public ModuleBasePropertyDefinitions
{
- class LoggingPropertyDefinitions: public ModuleBasePropertyDefinitions
+ public:
+ LoggingPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
{
- public:
- LoggingPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::size_t>(
- "RTLogging_PeriodMs", 10,
- "Period of the rt-logging thread in milliseconds. "
- "A high period can cause spikes in disk activity if data is logged to disk. "
- "A low period causes more computation overhead and memory consumption. "
- "Must not be 0.");
-
- defineOptionalProperty<std::string>(
- "RTLogging_DefaultLog", "",
- "If rt logging is active and a file path is given, all data is logged to this file.");
-
- defineOptionalProperty<std::size_t>(
- "RTLogging_MessageNumber", 1000,
- "Number of messages that can be logged in the control thread");
- defineOptionalProperty<std::size_t>(
- "RTLogging_MessageBufferSize", 1024 * 1024,
- "Number of bytes that can be logged in the control thread");
- defineOptionalProperty<std::size_t>(
- "RTLogging_MaxMessageNumber", 16000,
- "Max number of messages that can be logged in the control thread");
- defineOptionalProperty<std::size_t>(
- "RTLogging_MaxMessageBufferSize", 16 * 1024 * 1024,
- "Max number of bytes that can be logged in the control thread");
-
- defineOptionalProperty<std::size_t>(
- "RTLogging_KeepIterationsForMs", 60 * 1000,
- "All logging data (SensorValues, ControlTargets, Messages) is keept for this duration "
- "and can be dunped in case of an error.");
- }
- };
+ defineOptionalProperty<std::size_t>(
+ "RTLogging_PeriodMs", 10,
+ "Period of the rt-logging thread in milliseconds. "
+ "A high period can cause spikes in disk activity if data is logged to disk. "
+ "A low period causes more computation overhead and memory consumption. "
+ "Must not be 0.");
+
+ defineOptionalProperty<std::string>(
+ "RTLogging_DefaultLog", "",
+ "If rt logging is active and a file path is given, all data is logged to this file.");
+
+ defineOptionalProperty<std::size_t>(
+ "RTLogging_MessageNumber", 1000,
+ "Number of messages that can be logged in the control thread");
+ defineOptionalProperty<std::size_t>(
+ "RTLogging_MessageBufferSize", 1024 * 1024,
+ "Number of bytes that can be logged in the control thread");
+ defineOptionalProperty<std::size_t>(
+ "RTLogging_MaxMessageNumber", 16000,
+ "Max number of messages that can be logged in the control thread");
+ defineOptionalProperty<std::size_t>(
+ "RTLogging_MaxMessageBufferSize", 16 * 1024 * 1024,
+ "Max number of bytes that can be logged in the control thread");
+
+ defineOptionalProperty<std::size_t>(
+ "RTLogging_KeepIterationsForMs", 60 * 1000,
+ "All logging data (SensorValues, ControlTargets, Messages) is keept for this duration "
+ "and can be dunped in case of an error.");
+ }
+ };
+
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" manages logging of data.
+ *
+ * There are two types of data:
+ * - Messages: These are logged via \ref ARMARX_RT_LOGF macros and printed to the corresponding ArmarX streams
+ * - \ref SensorValueBase "SensorValues" / \ref ControlTargetBase "ControlTargets": These can be logged to CSV files
+ *
+ * @see ModuleBase
+ */
+ class Logging : virtual public ModuleBase, virtual public RobotUnitLoggingInterface
+ {
+ friend class ModuleBase;
+ public:
+ /**
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
+ */
+ static Logging& Instance()
+ {
+ return ModuleBase::Instance<Logging>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_postOnInitRobotUnit
+ void _postOnInitRobotUnit();
+ /// @see ModuleBase::_postFinishDeviceInitialization
+ void _postFinishDeviceInitialization();
+ /// @see ModuleBase::_preFinishControlThreadInitialization
+ void _preFinishControlThreadInitialization();
+ /// @see ModuleBase::_preFinishRunning
+ void _preFinishRunning();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Starts logging to a CSV file
+ * @param formatString The file to log to.
+ * @param loggingNames The data fields to log.
+ * @return A handle to the log. If it's last copy is deleted, logging is stopped.
+ */
+ RemoteReferenceCounterBasePtr startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Starts logging to a CSV file
+ * @param formatString The file to log to.
+ * @param aliasNames Data fields to log. For an entry (key,value), the datafield key is logged and the table heading value is used.
+ * If value is empty, key is used as heading.
+ * @return A handle to the log. If it's last copy is deleted, logging is stopped.
+ */
+ RemoteReferenceCounterBasePtr startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current& = Ice::emptyCurrent) override;
+
+ /**
+ * @brief Stops logging to the given log.
+ * @param token The log to close.
+ */
+ void stopRtLogging(const armarx::RemoteReferenceCounterBasePtr& token, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Adds a string to the log (it is added in a special column).
+ * @param token The log.
+ * @param marker The string to add.
+ */
+ void addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current& = Ice::emptyCurrent) override;
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" manages logging of data.
- *
- * There are two types of data:
- * - Messages: These are logged via \ref ARMARX_RT_LOGF macros and printed to the corresponding ArmarX streams
- * - \ref SensorValueBase "SensorValues" / \ref ControlTargetBase "ControlTargets": These can be logged to CSV files
- *
- * @see ModuleBase
+ * @brief Returns the names of all loggable data fields.
+ * @return The names of all loggable data fields.
*/
- class Logging : virtual public ModuleBase, virtual public RobotUnitLoggingInterface
+ Ice::StringSeq getLoggingNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Dumps the backlog of all recent iterations to the given file.
+ * This helps debugging.
+ * @param formatString The file.
+ */
+ void writeRecentIterationsToFile(const std::string& formatString, const Ice::Current& = Ice::emptyCurrent) const override;
+
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief Executes the logging loop.
+ void doLogging();
+
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ using RTLoggingTaskT = SimplePeriodicTask<std::function<void(void)>>;
+
+ /// @brief Data structure holding information about an logging entry.
+ struct CSVLoggingEntry
{
- friend class ModuleBase;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static Logging& Instance()
- {
- return ModuleBase::Instance<Logging>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_postOnInitRobotUnit
- void _postOnInitRobotUnit();
- /// @see ModuleBase::_postFinishDeviceInitialization
- void _postFinishDeviceInitialization();
- /// @see ModuleBase::_preFinishControlThreadInitialization
- void _preFinishControlThreadInitialization();
- /// @see ModuleBase::_preFinishRunning
- void _preFinishRunning();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// ice interface //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Starts logging to a CSV file
- * @param formatString The file to log to.
- * @param loggingNames The data fields to log.
- * @return A handle to the log. If it's last copy is deleted, logging is stopped.
- */
- RemoteReferenceCounterBasePtr startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Starts logging to a CSV file
- * @param formatString The file to log to.
- * @param aliasNames Data fields to log. For an entry (key,value), the datafield key is logged and the table heading value is used.
- * If value is empty, key is used as heading.
- * @return A handle to the log. If it's last copy is deleted, logging is stopped.
- */
- RemoteReferenceCounterBasePtr startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current& = Ice::emptyCurrent) override;
-
- /**
- * @brief Stops logging to the given log.
- * @param token The log to close.
- */
- void stopRtLogging(const armarx::RemoteReferenceCounterBasePtr& token, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Adds a string to the log (it is added in a special column).
- * @param token The log.
- * @param marker The string to add.
- */
- void addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current& = Ice::emptyCurrent) override;
-
- /**
- * @brief Returns the names of all loggable data fields.
- * @return The names of all loggable data fields.
- */
- Ice::StringSeq getLoggingNames(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Dumps the backlog of all recent iterations to the given file.
- * This helps debugging.
- * @param formatString The file.
- */
- void writeRecentIterationsToFile(const std::string& formatString, const Ice::Current& = Ice::emptyCurrent) const override;
-
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////// implementation //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief Executes the logging loop.
- void doLogging();
-
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- using RTLoggingTaskT = SimplePeriodicTask<std::function<void(void)>>;
-
- /// @brief Data structure holding information about an logging entry.
- struct CSVLoggingEntry
- {
- /// @brief Whether logging should be stopped.
- std::atomic_bool stopLogging {false};
- /// @brief Bools whether a control device field should be logged.
- std::vector<std::vector<std::vector<char>>> loggedControlDeviceValues;
- /// @brief Bools whether a sensor device field should be logged.
- std::vector<std::vector<char>> loggedSensorDeviceValues;
- /// @brief The file stream for the csv file
- std::ofstream stream;
- /// @brief The file name
- std::string filename;
- /// @brief The marker to add in the next logging iteration. (protected by \ref rtLoggingMutex)
- std::string marker;
- };
-
- /// @brief The thread executing the logging functions.
- RTLoggingTaskT::pointer_type rtLoggingTask;
- /// @brief All entries for logging.
- std::map<std::string, std::shared_ptr<CSVLoggingEntry>> rtLoggingEntries;
- /// @brief Handle for the default log. (This log can be enabled via a property and logs everything)
- RemoteReferenceCounterBasePtr defaultLogHandle;
- /// @brief The stored backlog (it is dumped when \ref writeRecentIterationsToFile is called)
- std::deque< ::armarx::detail::ControlThreadOutputBufferEntry > backlog;
- /// @brief The control threads unix thread id
- Ice::Int controlThreadId {0};
- /// @brief Mutex protecting the data structures of this class
- mutable std::mutex rtLoggingMutex;
- /// @brief Data field names for all \ref ControlTargetBase "ControlTargets"
- std::vector<std::vector<std::vector<std::string>>> controlDeviceValueLoggingNames;
- /// @brief Data field names for all \ref SensorValueBase "SensorValues"
- std::vector<std::vector< std::string >> sensorDeviceValueLoggingNames;
-
- /// @brief The initial size of the Message entry buffer
- std::size_t messageBufferSize {0};
- /// @brief The initial number of Message entries
- std::size_t messageBufferNumberEntries {0};
- /// @brief The maximal size of the Message entry buffer
- std::size_t messageBufferMaxSize {0};
- /// @brief The maximal number of Message entries
- std::size_t messageBufferMaxNumberEntries {0};
- /// @brief The logging thread's period
- std::size_t rtLoggingTimestep {0};
- /// @brief The time an entry shold remain in the backlog.
- IceUtil::Time rtLoggingBacklogRetentionTime;
+ /// @brief Whether logging should be stopped.
+ std::atomic_bool stopLogging {false};
+ /// @brief Bools whether a control device field should be logged.
+ std::vector<std::vector<std::vector<char>>> loggedControlDeviceValues;
+ /// @brief Bools whether a sensor device field should be logged.
+ std::vector<std::vector<char>> loggedSensorDeviceValues;
+ /// @brief The file stream for the csv file
+ std::ofstream stream;
+ /// @brief The file name
+ std::string filename;
+ /// @brief The marker to add in the next logging iteration. (protected by \ref rtLoggingMutex)
+ std::string marker;
};
- }
+
+ /// @brief The thread executing the logging functions.
+ RTLoggingTaskT::pointer_type rtLoggingTask;
+ /// @brief All entries for logging.
+ std::map<std::string, std::shared_ptr<CSVLoggingEntry>> rtLoggingEntries;
+ /// @brief Handle for the default log. (This log can be enabled via a property and logs everything)
+ RemoteReferenceCounterBasePtr defaultLogHandle;
+ /// @brief The stored backlog (it is dumped when \ref writeRecentIterationsToFile is called)
+ std::deque< ::armarx::detail::ControlThreadOutputBufferEntry > backlog;
+ /// @brief The control threads unix thread id
+ Ice::Int controlThreadId {0};
+ /// @brief Mutex protecting the data structures of this class
+ mutable std::mutex rtLoggingMutex;
+ /// @brief Data field names for all \ref ControlTargetBase "ControlTargets"
+ std::vector<std::vector<std::vector<std::string>>> controlDeviceValueLoggingNames;
+ /// @brief Data field names for all \ref SensorValueBase "SensorValues"
+ std::vector<std::vector< std::string >> sensorDeviceValueLoggingNames;
+
+ /// @brief The initial size of the Message entry buffer
+ std::size_t messageBufferSize {0};
+ /// @brief The initial number of Message entries
+ std::size_t messageBufferNumberEntries {0};
+ /// @brief The maximal size of the Message entry buffer
+ std::size_t messageBufferMaxSize {0};
+ /// @brief The maximal number of Message entries
+ std::size_t messageBufferMaxNumberEntries {0};
+ /// @brief The logging thread's period
+ std::size_t rtLoggingTimestep {0};
+ /// @brief The time an entry shold remain in the backlog.
+ IceUtil::Time rtLoggingBacklogRetentionTime;
+ };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
index 8f5ca1a41d4bdcf2c265d39a0b8befd91b8e26ac..28a25a4706505ce721037efff9b6d7348531b97c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
@@ -25,36 +25,33 @@
#include "RobotUnitModuleManagement.h"
#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ void Management::_preOnInitRobotUnit()
{
- void Management::_preOnInitRobotUnit()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- additionalObjectSchedulerCount = getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue();
- heartbeatRequired = getProperty<bool>("HeartbeatRequired").getValue();
- heartbeatMaxCycleMS = getProperty<long>("HeartbeatMaxCycleMS").getValue();
- heartbeatStartupMarginMS = getProperty<long>("HeartbeatStartupMarginMS").getValue();
- getArmarXManager()->increaseSchedulers(static_cast<int>(additionalObjectSchedulerCount));
-
- usingTopic(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
- }
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ additionalObjectSchedulerCount = getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue();
+ heartbeatRequired = getProperty<bool>("HeartbeatRequired").getValue();
+ heartbeatMaxCycleMS = getProperty<long>("HeartbeatMaxCycleMS").getValue();
+ heartbeatStartupMarginMS = getProperty<long>("HeartbeatStartupMarginMS").getValue();
+ getArmarXManager()->increaseSchedulers(static_cast<int>(additionalObjectSchedulerCount));
+
+ usingTopic(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
+ }
- void Management::_postFinishControlThreadInitialization()
- {
- controlLoopStartTime = TimeUtil::GetTime(true).toMilliSeconds();
- }
+ void Management::_postFinishControlThreadInitialization()
+ {
+ controlLoopStartTime = TimeUtil::GetTime(true).toMilliSeconds();
+ }
- void Management::aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&)
+ void Management::aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&)
+ {
+ heartbeatRequired = true;
+ if (overallHealthState == RobotHealthState::HealthOK || overallHealthState == RobotHealthState::HealthWarning)
{
- heartbeatRequired = true;
- if (overallHealthState == RobotHealthState::HealthOK || overallHealthState == RobotHealthState::HealthWarning)
- {
- lastHeartbeat = TimeUtil::GetTime(true).toMilliSeconds();
- }
+ lastHeartbeat = TimeUtil::GetTime(true).toMilliSeconds();
}
-
}
+
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
index a6653bacf974d242f8fc20ae52e517c6253a96f4..b4aced7b9e0fb437985e14d8bf8a2fa1adfb9440 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
@@ -24,97 +24,94 @@
#include "RobotUnitModuleBase.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
- {
- class ManagementPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class ManagementPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ public:
+ ManagementPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
{
- public:
- ManagementPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::uint64_t>(
- "AdditionalObjectSchedulerCount", 10,
- "Number of ObjectSchedulers to be added");
+ defineOptionalProperty<std::uint64_t>(
+ "AdditionalObjectSchedulerCount", 10,
+ "Number of ObjectSchedulers to be added");
- defineOptionalProperty<bool>(
- "HeartbeatRequired", true,
- "Whether the Heatbeat is required.");
- defineOptionalProperty<long>(
- "HeartbeatMaxCycleMS", 100,
- "The heartbeats cycle time");
- defineOptionalProperty<long>(
- "HeartbeatStartupMarginMS", 1000,
- "Startup time for heartbeats");
- defineOptionalProperty<std::string>(
- "AggregatedRobotHealthTopicName", "AggregatedRobotHealthTopic",
- "Name of the AggregatedRobotHealthTopic");
- }
- };
+ defineOptionalProperty<bool>(
+ "HeartbeatRequired", true,
+ "Whether the Heatbeat is required.");
+ defineOptionalProperty<long>(
+ "HeartbeatMaxCycleMS", 100,
+ "The heartbeats cycle time");
+ defineOptionalProperty<long>(
+ "HeartbeatStartupMarginMS", 1000,
+ "Startup time for heartbeats");
+ defineOptionalProperty<std::string>(
+ "AggregatedRobotHealthTopicName", "AggregatedRobotHealthTopic",
+ "Name of the AggregatedRobotHealthTopic");
+ }
+ };
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" handles some general management tasks.
+ * It implements the \ref RobotUnitManagementInterface.
+ *
+ * @see ModuleBase
+ */
+ class Management : virtual public ModuleBase, virtual public RobotUnitManagementInterface
+ {
+ friend class ModuleBase;
+ public:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" handles some general management tasks.
- * It implements the \ref RobotUnitManagementInterface.
- *
- * @see ModuleBase
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
*/
- class Management : virtual public ModuleBase, virtual public RobotUnitManagementInterface
+ static Management& Instance()
{
- friend class ModuleBase;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static Management& Instance()
- {
- return ModuleBase::Instance<Management>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_preOnInitRobotUnit
- void _preOnInitRobotUnit();
- void _postFinishControlThreadInitialization();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// ice interface //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Returns whether the RobotUnit is running.
- * @return Whether the RobotUnit is running.
- */
- bool isRunning(const Ice::Current& = Ice::emptyCurrent) const override
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return getRobotUnitState() == RobotUnitState::Running;
- }
- void aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&) override;
+ return ModuleBase::Instance<Management>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_preOnInitRobotUnit
+ void _preOnInitRobotUnit();
+ void _postFinishControlThreadInitialization();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns whether the RobotUnit is running.
+ * @return Whether the RobotUnit is running.
+ */
+ bool isRunning(const Ice::Current& = Ice::emptyCurrent) const override
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return getRobotUnitState() == RobotUnitState::Running;
+ }
+ void aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&) override;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief The number of additional object schedulers
- std::int64_t additionalObjectSchedulerCount {0};
- std::int64_t controlLoopStartTime {0};
- std::int64_t heartbeatStartupMarginMS {0};
- std::atomic_bool heartbeatRequired{false};
- std::atomic_long heartbeatMaxCycleMS{100};
- std::atomic_long lastHeartbeat{0};
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief The number of additional object schedulers
+ std::int64_t additionalObjectSchedulerCount {0};
+ std::int64_t controlLoopStartTime {0};
+ std::int64_t heartbeatStartupMarginMS {0};
+ std::atomic_bool heartbeatRequired{false};
+ std::atomic_long heartbeatMaxCycleMS{100};
+ std::atomic_long lastHeartbeat{0};
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * \brief This class allows minimal access to private members of \ref Management in a sane fashion for \ref ControlThread.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class ManagementAttorneyForControlThread;
- };
- }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * \brief This class allows minimal access to private members of \ref Management in a sane fashion for \ref ControlThread.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class ManagementAttorneyForControlThread;
+ };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index c24b433f99d08cad72c4d91b8893b4ce20f5987f..f53c5d85786549f1ae216ab27fa42b556f2102bb 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -25,559 +25,552 @@
#include "../NJointControllers/NJointController.h"
#include "../Units/RobotUnitSubUnit.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ /**
+ * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref Publisher.
+ * \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 NJointControllerAttorneyForPublisher
{
- /**
- * \brief This class allows minimal access to private members of \ref NJointController in a sane fashion for \ref Publisher.
- * \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 NJointControllerAttorneyForPublisher
- {
- friend class Publisher;
- static bool GetStatusReportedActive(const NJointControllerPtr& nJointCtrl)
- {
- return nJointCtrl->statusReportedActive;
- }
- static bool GetStatusReportedRequested(const NJointControllerPtr& nJointCtrl)
- {
- return nJointCtrl->statusReportedRequested;
- }
- static void UpdateStatusReported(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
- nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
- }
- static void Publish(
- const NJointControllerPtr& nJointCtrl,
- const SensorAndControl& sac,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
- {
- nJointCtrl ->publish(sac, draw, observer);
- }
- static void DeactivatePublishing(
- const NJointControllerPtr& nJointCtrl,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
- {
- nJointCtrl->deactivatePublish(draw, observer);
- }
- };
- /**
- * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref Publisher.
- * \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 DevicesAttorneyForPublisher
- {
- friend class Publisher;
- static const std::string& GetSensorDeviceName(Publisher* p, std::size_t idx)
- {
- return p->_module<Devices>().sensorDevices.at(idx)->getDeviceName();
- }
- };
- /**
- * \brief This class allows minimal access to private members of \ref Units in a sane fashion for \ref Publisher.
- * \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 UnitsAttorneyForPublisher
- {
- friend class Publisher;
- static const std::vector<RobotUnitSubUnitPtr>& GetSubUnits(Publisher* p)
- {
- return p->_module<Units>().subUnits;
- }
- };
- /**
- * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref Publisher.
- * \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 ControllerManagementAttorneyForPublisher
- {
- friend class Publisher;
- static const std::map<std::string, NJointControllerPtr>& GetNJointControllers(Publisher* p)
- {
- return p->_module<ControllerManagement>().nJointControllers;
- }
- static void RemoveNJointControllersToBeDeleted(Publisher* p, bool blocking, RobotUnitListenerPrx l)
- {
- p->_module<ControllerManagement>().removeNJointControllersToBeDeleted(blocking, l);
- }
- };
- /**
- * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- class ControlThreadAttorneyForPublisher
- {
- friend class Publisher;
- static void ProcessEmergencyStopRequest(Publisher* p)
- {
- return p->_module<ControlThread>().processEmergencyStopRequest();
- }
- };
- }
+ friend class Publisher;
+ static bool GetStatusReportedActive(const NJointControllerBasePtr& nJointCtrl)
+ {
+ return nJointCtrl->statusReportedActive;
+ }
+ static bool GetStatusReportedRequested(const NJointControllerBasePtr& nJointCtrl)
+ {
+ return nJointCtrl->statusReportedRequested;
+ }
+ static void UpdateStatusReported(const NJointControllerBasePtr& nJointCtrl)
+ {
+ nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
+ nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
+ }
+ static void Publish(
+ const NJointControllerBasePtr& nJointCtrl,
+ const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer
+ )
+ {
+ nJointCtrl ->publish(sac, draw, observer);
+ }
+ static void DeactivatePublishing(
+ const NJointControllerBasePtr& nJointCtrl,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer
+ )
+ {
+ nJointCtrl->deactivatePublish(draw, observer);
+ }
+ };
+ /**
+ * \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref Publisher.
+ * \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 DevicesAttorneyForPublisher
+ {
+ friend class Publisher;
+ static const std::string& GetSensorDeviceName(Publisher* p, std::size_t idx)
+ {
+ return p->_module<Devices>().sensorDevices.at(idx)->getDeviceName();
+ }
+ };
+ /**
+ * \brief This class allows minimal access to private members of \ref Units in a sane fashion for \ref Publisher.
+ * \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 UnitsAttorneyForPublisher
+ {
+ friend class Publisher;
+ static const std::vector<RobotUnitSubUnitPtr>& GetSubUnits(Publisher* p)
+ {
+ return p->_module<Units>().subUnits;
+ }
+ };
+ /**
+ * \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref Publisher.
+ * \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 ControllerManagementAttorneyForPublisher
+ {
+ friend class Publisher;
+ static const std::map<std::string, NJointControllerBasePtr>& GetNJointControllers(Publisher* p)
+ {
+ return p->_module<ControllerManagement>().nJointControllers;
+ }
+ static void RemoveNJointControllersToBeDeleted(Publisher* p, bool blocking, RobotUnitListenerPrx l)
+ {
+ p->_module<ControllerManagement>().removeNJointControllersToBeDeleted(blocking, l);
+ }
+ };
+ /**
+ * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ class ControlThreadAttorneyForPublisher
+ {
+ friend class Publisher;
+ static void ProcessEmergencyStopRequest(Publisher* p)
+ {
+ return p->_module<ControlThread>().processEmergencyStopRequest();
+ }
+ };
}
-
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ const std::map<std::string, NJointControllerBasePtr>& Publisher::getNJointControllers()
{
- const std::map<std::string, NJointControllerPtr>& Publisher::getNJointControllers()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return ControllerManagementAttorneyForPublisher::GetNJointControllers(this);
- }
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return ControllerManagementAttorneyForPublisher::GetNJointControllers(this);
+ }
- TimedVariantPtr Publisher::publishNJointClassNames()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- const auto beg = TimeUtil::GetTime(true);
+ TimedVariantPtr Publisher::publishNJointClassNames()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ const auto beg = TimeUtil::GetTime(true);
- const auto classNames = NJointControllerRegistry::getKeys();
- if (lastReportedClasses.size() != classNames.size())
+ const auto classNames = NJointControllerRegistry::getKeys();
+ if (lastReportedClasses.size() != classNames.size())
+ {
+ for (const auto& name : classNames)
{
- for (const auto& name : classNames)
+ if (!lastReportedClasses.count(name))
{
- if (!lastReportedClasses.count(name))
- {
- robotUnitListenerBatchPrx->nJointControllerClassAdded(name);
- lastReportedClasses.emplace(name);
- }
+ robotUnitListenerBatchPrx->nJointControllerClassAdded(name);
+ lastReportedClasses.emplace(name);
}
}
-
- const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishNJointControllerUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer
- )
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- const auto beg = TimeUtil::GetTime(true);
-
- //publish controller updates
- auto debugDrawerBatchPrx = debugDrawerPrx->ice_batchOneway();
- auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
- for (const auto& pair : getNJointControllers())
- {
- const auto begInner = TimeUtil::GetTime(true);
- const NJointControllerPtr& nJointCtrl = pair.second;
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ }
- //run some hook for active (used for visu)
- NJointControllerAttorneyForPublisher::Publish(nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
- if (
- NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) != nJointCtrl->isControllerActive() ||
- NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) != nJointCtrl->isControllerRequested()
- )
- {
- NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
- robotUnitListenerBatchPrx->nJointControllerStatusChanged(nJointCtrl->getControllerStatus());
- }
+ TimedVariantPtr Publisher::publishNJointControllerUpdates(
+ StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer
+ )
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ const auto beg = TimeUtil::GetTime(true);
- const auto endInner = TimeUtil::GetTime(true);
- timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
+ //publish controller updates
+ auto debugDrawerBatchPrx = debugDrawerPrx->ice_batchOneway();
+ auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
+ for (const auto& pair : getNJointControllers())
+ {
+ const auto begInner = TimeUtil::GetTime(true);
+ const NJointControllerBasePtr& nJointCtrl = pair.second;
+
+ //run some hook for active (used for visu)
+ NJointControllerAttorneyForPublisher::Publish(nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
+ if (
+ NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) != nJointCtrl->isControllerActive() ||
+ NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) != nJointCtrl->isControllerRequested()
+ )
+ {
+ NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
+ robotUnitListenerBatchPrx->nJointControllerStatusChanged(nJointCtrl->getControllerStatus());
}
- debugObserverBatchPrx->ice_flushBatchRequests();
- debugDrawerBatchPrx->ice_flushBatchRequests();
- const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ const auto endInner = TimeUtil::GetTime(true);
+ timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
}
+ debugObserverBatchPrx->ice_flushBatchRequests();
+ debugDrawerBatchPrx->ice_flushBatchRequests();
- TimedVariantPtr Publisher::publishUnitUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer,
- const JointAndNJointControllers& activatedControllers)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- const auto beg = TimeUtil::GetTime(true);
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ }
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
- publishNewSensorDataTime = TimeUtil::GetTime(true);
- for (const RobotUnitSubUnitPtr& rsu : UnitsAttorneyForPublisher::GetSubUnits(this))
+ TimedVariantPtr Publisher::publishUnitUpdates(
+ StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ const JointAndNJointControllers& activatedControllers)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ const auto beg = TimeUtil::GetTime(true);
+
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
+ publishNewSensorDataTime = TimeUtil::GetTime(true);
+ for (const RobotUnitSubUnitPtr& rsu : UnitsAttorneyForPublisher::GetSubUnits(this))
+ {
+ if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
{
- if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
- {
- const auto begInner = TimeUtil::GetTime(true);
- rsu->update(controlThreadOutputBuffer, activatedControllers);
- const auto endInner = TimeUtil::GetTime(true);
- timingMap["UnitUpdate_" + rsu->getName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
- }
+ const auto begInner = TimeUtil::GetTime(true);
+ rsu->update(controlThreadOutputBuffer, activatedControllers);
+ const auto endInner = TimeUtil::GetTime(true);
+ timingMap["UnitUpdate_" + rsu->getName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
}
-
- const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishControlUpdates(
- const SensorAndControl& controlThreadOutputBuffer,
- bool haveSensorAndControlValsChanged,
- bool publishToObserver,
- const JointAndNJointControllers& activatedControllers,
- const std::vector<JointController*>& requestedJointControllers)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- const auto beg = TimeUtil::GetTime(true);
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ }
+
+ TimedVariantPtr Publisher::publishControlUpdates(
+ const SensorAndControl& controlThreadOutputBuffer,
+ bool haveSensorAndControlValsChanged,
+ bool publishToObserver,
+ const JointAndNJointControllers& activatedControllers,
+ const std::vector<JointController*>& requestedJointControllers)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ const auto beg = TimeUtil::GetTime(true);
+
+ StringVariantBaseMap ctrlDevMap;
+ for (std::size_t ctrlidx = 0 ; ctrlidx < _module<Devices>().getNumberOfControlDevices(); ++ctrlidx)
+ {
+ const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
- StringVariantBaseMap ctrlDevMap;
- for (std::size_t ctrlidx = 0 ; ctrlidx < _module<Devices>().getNumberOfControlDevices(); ++ctrlidx)
+ StringToStringVariantBaseMapMap variants;
+ ControlDeviceStatus status;
+ const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
+ status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
+ status.deviceName = ctrlDev.getDeviceName();
+ if (activeJointCtrl)
{
- const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
-
- StringToStringVariantBaseMapMap variants;
- ControlDeviceStatus status;
- const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
- status.deviceName = ctrlDev.getDeviceName();
- if (activeJointCtrl)
+ auto additionalDatafields = activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
+ for (auto& pair : additionalDatafields)
{
- auto additionalDatafields = activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
- for (auto& pair : additionalDatafields)
- {
- ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() + "_" + pair.first] = pair.second;
- }
+ ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() + "_" + pair.first] = pair.second;
}
+ }
- for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
+ for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
+ {
+ const auto& controlMode = ctrlVal->getControlMode();
+ variants[controlMode] = ctrlVal->toVariants(lastControlThreadTimestamp);
+ if (
+ haveSensorAndControlValsChanged &&
+ !variants[controlMode].empty() &&
+ observerPublishControlTargets &&
+ publishToObserver
+ )
{
- const auto& controlMode = ctrlVal->getControlMode();
- variants[controlMode] = ctrlVal->toVariants(lastControlThreadTimestamp);
- if (
- haveSensorAndControlValsChanged &&
- !variants[controlMode].empty() &&
- observerPublishControlTargets &&
- publishToObserver
- )
+ for (const auto& pair : variants[controlMode])
{
- for (const auto& pair : variants[controlMode])
- {
- ctrlDevMap[status.deviceName + "_" + pair.first] = pair.second;
- }
+ ctrlDevMap[status.deviceName + "_" + pair.first] = pair.second;
}
}
-
- ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(ctrlidx));
- status.requestedControlMode = requestedJointControllers.at(ctrlidx)->getControlMode();
- status.controlTargetValues = std::move(variants);
- status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
- robotUnitListenerBatchPrx->controlDeviceStatusChanged(status);
- }
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
- {
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
}
- const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(ctrlidx));
+ status.requestedControlMode = requestedJointControllers.at(ctrlidx)->getControlMode();
+ status.controlTargetValues = std::move(variants);
+ status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
+ robotUnitListenerBatchPrx->controlDeviceStatusChanged(status);
}
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ {
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
+ }
+
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ }
- TimedVariantPtr Publisher::publishSensorUpdates(
- bool publishToObserver,
- const SensorAndControl& controlThreadOutputBuffer)
+ TimedVariantPtr Publisher::publishSensorUpdates(
+ bool publishToObserver,
+ const SensorAndControl& controlThreadOutputBuffer)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ const auto beg = TimeUtil::GetTime(true);
+
+ StringVariantBaseMap sensorDevMap;
+ const auto numSensorDev = _module<Devices>().getNumberOfSensorDevices();
+ ARMARX_CHECK_EQUAL(controlThreadOutputBuffer.sensors.size(), _module<Devices>().getNumberOfSensorDevices());
+ for (std::size_t sensidx = 0 ; sensidx < numSensorDev; ++sensidx)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- const auto beg = TimeUtil::GetTime(true);
+ const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
+ auto variants = sensVal.toVariants(lastControlThreadTimestamp);
- StringVariantBaseMap sensorDevMap;
- const auto numSensorDev = _module<Devices>().getNumberOfSensorDevices();
- ARMARX_CHECK_EQUAL(controlThreadOutputBuffer.sensors.size(), _module<Devices>().getNumberOfSensorDevices());
- for (std::size_t sensidx = 0 ; sensidx < numSensorDev; ++sensidx)
+ if (!variants.empty())
{
- const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
- auto variants = sensVal.toVariants(lastControlThreadTimestamp);
+ SensorDeviceStatus status;
+ status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(this, sensidx);
- if (!variants.empty())
+ if (observerPublishSensorValues && publishToObserver)
{
- SensorDeviceStatus status;
- status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(this, sensidx);
-
- if (observerPublishSensorValues && publishToObserver)
+ for (const auto& pair : variants)
{
- for (const auto& pair : variants)
- {
- sensorDevMap[status.deviceName + "_" + pair.first] = pair.second;
- }
+ sensorDevMap[status.deviceName + "_" + pair.first] = pair.second;
}
-
- status.sensorValue = std::move(variants);
- status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
- robotUnitListenerBatchPrx->sensorDeviceStatusChanged(status);
}
+
+ status.sensorValue = std::move(variants);
+ status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
+ robotUnitListenerBatchPrx->sensorDeviceStatusChanged(status);
}
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ }
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ {
+ std::stringstream s;
+ for (auto& pair : sensorDevMap)
+ {
+ s << pair.first << ": \t" << (pair.second ? pair.second->ice_id() + "\t with datatype \t" + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ }
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
{
- std::stringstream s;
for (auto& pair : sensorDevMap)
{
- s << pair.first << ": \t" << (pair.second ? pair.second->ice_id() + "\t with datatype \t" + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
}
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
- {
- for (auto& pair : sensorDevMap)
- {
- out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
- }
- };
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
- }
-
- const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ };
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
}
- std::string Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return robotUnitListenerTopicName;
- }
+ const auto end = TimeUtil::GetTime(true);
+ return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ }
- std::string Publisher::getDebugDrawerTopicName(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return debugDrawerUpdatesTopicName;
- }
+ std::string Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return robotUnitListenerTopicName;
+ }
- std::string Publisher::getDebugObserverTopicName(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return debugObserverTopicName;
- }
+ std::string Publisher::getDebugDrawerTopicName(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return debugDrawerUpdatesTopicName;
+ }
- RobotUnitListenerPrx Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robotUnitListenerPrx);
- return robotUnitListenerPrx;
- }
+ std::string Publisher::getDebugObserverTopicName(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return debugObserverTopicName;
+ }
- DebugObserverInterfacePrx Publisher::getDebugObserverProxy(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(debugObserverPrx);
- return debugObserverPrx;
- }
+ RobotUnitListenerPrx Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_CHECK_EXPRESSION(robotUnitListenerPrx);
+ return robotUnitListenerPrx;
+ }
- DebugDrawerInterfacePrx Publisher::getDebugDrawerProxy(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(debugDrawerPrx);
- return debugDrawerPrx;
- }
+ DebugObserverInterfacePrx Publisher::getDebugObserverProxy(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_CHECK_EXPRESSION(debugObserverPrx);
+ return debugObserverPrx;
+ }
- void Publisher::_preOnConnectRobotUnit()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic " << getRobotUnitListenerTopicName();
- robotUnitListenerPrx = getTopic<RobotUnitListenerPrx>(getRobotUnitListenerTopicName());
- robotUnitListenerBatchPrx = robotUnitListenerPrx->ice_batchOneway();
+ DebugDrawerInterfacePrx Publisher::getDebugDrawerProxy(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_CHECK_EXPRESSION(debugDrawerPrx);
+ return debugDrawerPrx;
+ }
- ARMARX_DEBUG << "retrieving DebugDrawerInterfacePrx for topic " << getDebugDrawerTopicName();
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getDebugDrawerTopicName());
+ void Publisher::_preOnConnectRobotUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic " << getRobotUnitListenerTopicName();
+ robotUnitListenerPrx = getTopic<RobotUnitListenerPrx>(getRobotUnitListenerTopicName());
+ robotUnitListenerBatchPrx = robotUnitListenerPrx->ice_batchOneway();
- ARMARX_DEBUG << "retrieving DebugObserverInterfacePrx for topic " << getDebugObserverTopicName();
- debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getDebugObserverTopicName());
- }
+ ARMARX_DEBUG << "retrieving DebugDrawerInterfacePrx for topic " << getDebugDrawerTopicName();
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getDebugDrawerTopicName());
- void Publisher::_preOnInitRobotUnit()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- robotUnitListenerTopicName = getProperty<std::string>("RobotUnitListenerTopicName").getValue();
- debugDrawerUpdatesTopicName = getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
- debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
+ ARMARX_DEBUG << "retrieving DebugObserverInterfacePrx for topic " << getDebugObserverTopicName();
+ debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getDebugObserverTopicName());
+ }
- observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
- observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
- debugObserverSkipIterations = std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
+ void Publisher::_preOnInitRobotUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ robotUnitListenerTopicName = getProperty<std::string>("RobotUnitListenerTopicName").getValue();
+ debugDrawerUpdatesTopicName = getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
+ debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
- publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());
+ observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
+ observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ debugObserverSkipIterations = std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
- offeringTopic(getRobotUnitListenerTopicName());
- offeringTopic(getDebugDrawerTopicName());
- offeringTopic(getDebugObserverTopicName());
- getArmarXManager()->addObject(robotUnitObserver);
- }
+ publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());
- void Publisher::_icePropertiesInitialized()
+ offeringTopic(getRobotUnitListenerTopicName());
+ offeringTopic(getDebugDrawerTopicName());
+ offeringTopic(getDebugObserverTopicName());
+ getArmarXManager()->addObject(robotUnitObserver);
+ }
+
+ void Publisher::_icePropertiesInitialized()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(getIceProperties(), "", getConfigDomain());
+ addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObserver));
+ }
+
+ void Publisher::_componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (changedProperties.count("ObserverPublishSensorValues"))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(getIceProperties(), "", getConfigDomain());
- addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObserver));
+ observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
}
-
- void Publisher::_componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+ if (changedProperties.count("ObserverPublishControlTargets"))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- if (changedProperties.count("ObserverPublishSensorValues"))
- {
- observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
- }
- if (changedProperties.count("ObserverPublishControlTargets"))
- {
- observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
- }
- if (changedProperties.count("ObserverPrintEveryNthIterations"))
- {
- debugObserverSkipIterations = getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
- }
+ observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ }
+ if (changedProperties.count("ObserverPrintEveryNthIterations"))
+ {
+ debugObserverSkipIterations = getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
}
+ }
- void Publisher::_preFinishRunning()
+ void Publisher::_preFinishRunning()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (publisherTask)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- if (publisherTask)
+ ARMARX_DEBUG << "shutting down publisher task";
+ publisherTask->stop();
+ std::this_thread::sleep_for(std::chrono::milliseconds {10});
+ while (publisherTask->isFunctionExecuting() || publisherTask->isRunning())
{
- ARMARX_DEBUG << "shutting down publisher task";
- publisherTask->stop();
- std::this_thread::sleep_for(std::chrono::milliseconds {10});
- while (publisherTask->isFunctionExecuting() || publisherTask->isRunning())
- {
- ARMARX_FATAL << deactivateSpam(0.1) << "PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
- }
- publisherTask = nullptr;
- //since the drawer queues draw events and we want to clear the layers, we have to sleep here
- std::this_thread::sleep_for(std::chrono::milliseconds {100});
- ARMARX_DEBUG << "shutting down publisher task done";
- }
- for (const auto& pair : getNJointControllers())
- {
- ARMARX_DEBUG << "forcing deactivation of publishing for " << pair.first;
- NJointControllerAttorneyForPublisher::DeactivatePublishing(pair.second, debugDrawerPrx, debugObserverPrx);
+ ARMARX_FATAL << deactivateSpam(0.1) << "PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
}
+ publisherTask = nullptr;
+ //since the drawer queues draw events and we want to clear the layers, we have to sleep here
+ std::this_thread::sleep_for(std::chrono::milliseconds {100});
+ ARMARX_DEBUG << "shutting down publisher task done";
}
-
- void Publisher::_postFinishControlThreadInitialization()
+ for (const auto& pair : getNJointControllers())
{
- //start publisher
- publishNewSensorDataTime = TimeUtil::GetTime(true);
- publisherTask = new PublisherTaskT([&] {publish({});}, publishPeriodMs, false, getName() + "_PublisherTask");
- ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
- publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
- publisherTask->start();
+ ARMARX_DEBUG << "forcing deactivation of publishing for " << pair.first;
+ NJointControllerAttorneyForPublisher::DeactivatePublishing(pair.second, debugDrawerPrx, debugObserverPrx);
}
+ }
- void Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- auto begPublish = TimeUtil::GetTime(true);
+ void Publisher::_postFinishControlThreadInitialization()
+ {
+ //start publisher
+ publishNewSensorDataTime = TimeUtil::GetTime(true);
+ publisherTask = new PublisherTaskT([&] {publish({});}, publishPeriodMs, false, getName() + "_PublisherTask");
+ ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
+ publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
+ publisherTask->start();
+ }
- if (getRobotUnitState() != RobotUnitState::Running)
- {
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state " << getRobotUnitState();
- return;
- }
- if (isShuttingDown())
- {
- return;
- }
- GuardType guard;
- try
- {
- guard = getGuard();
- }
- catch (...)
- {
- //shutting down
- return;
- }
- throwIfStateIsNot(RobotUnitState::Running, __FUNCTION__);
+ void Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ auto begPublish = TimeUtil::GetTime(true);
+
+ if (getRobotUnitState() != RobotUnitState::Running)
+ {
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state " << getRobotUnitState();
+ return;
+ }
+ if (isShuttingDown())
+ {
+ return;
+ }
+ GuardType guard;
+ try
+ {
+ guard = getGuard();
+ }
+ catch (...)
+ {
+ //shutting down
+ return;
+ }
+ throwIfStateIsNot(RobotUnitState::Running, __FUNCTION__);
- //maybe change the emergency stop state
- ControlThreadAttorneyForPublisher::ProcessEmergencyStopRequest(this);
+ //maybe change the emergency stop state
+ ControlThreadAttorneyForPublisher::ProcessEmergencyStopRequest(this);
- //get batch proxies
- //delete NJoint queued for deletion
- ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(this, false, robotUnitListenerBatchPrx);
- //swap buffers in
- const bool haveActivatedControllersChanged = _module<ControlThreadDataBuffer>().activatedControllersChanged();
- const bool haveSensorAndControlValsChanged = _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
- //setup datastructures
- const auto& controlThreadOutputBuffer = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer
+ //get batch proxies
+ //delete NJoint queued for deletion
+ ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(this, false, robotUnitListenerBatchPrx);
+ //swap buffers in
+ const bool haveActivatedControllersChanged = _module<ControlThreadDataBuffer>().activatedControllersChanged();
+ const bool haveSensorAndControlValsChanged = _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
+ //setup datastructures
+ const auto& controlThreadOutputBuffer = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer
- const auto activatedControllers = _module<ControlThreadDataBuffer>().getActivatedControllers();
- const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
+ const auto activatedControllers = _module<ControlThreadDataBuffer>().getActivatedControllers();
+ const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
- lastControlThreadTimestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
+ lastControlThreadTimestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
- const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
- //publish publishing meta state
- additionalMap["haveActivatedControllersChanged"] = new TimedVariant {haveActivatedControllersChanged, lastControlThreadTimestamp};
- additionalMap["haveSensorAndControlValsChanged"] = new TimedVariant {haveSensorAndControlValsChanged, lastControlThreadTimestamp};
- additionalMap["publishToObserver" ] = new TimedVariant {publishToObserver, lastControlThreadTimestamp};
- additionalMap["SoftwareEmergencyStopState" ] = new TimedVariant {_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() == EmergencyStopState::eEmergencyStopActive ? "EmergencyStopActive" : "EmergencyStopInactive",
- lastControlThreadTimestamp
- };
+ const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
+ //publish publishing meta state
+ additionalMap["haveActivatedControllersChanged"] = new TimedVariant {haveActivatedControllersChanged, lastControlThreadTimestamp};
+ additionalMap["haveSensorAndControlValsChanged"] = new TimedVariant {haveSensorAndControlValsChanged, lastControlThreadTimestamp};
+ additionalMap["publishToObserver" ] = new TimedVariant {publishToObserver, lastControlThreadTimestamp};
+ additionalMap["SoftwareEmergencyStopState" ] = new TimedVariant {_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() == EmergencyStopState::eEmergencyStopActive ? "EmergencyStopActive" : "EmergencyStopInactive",
+ lastControlThreadTimestamp
+ };
- //update
- if (haveSensorAndControlValsChanged)
- {
- timingMap["UnitUpdate"] = publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
- timingMap["SensorUpdates"] = publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
- }
- else
- {
- const double sensSecondsAgo = (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
- if (sensSecondsAgo > 1)
- {
- ARMARX_WARNING << deactivateSpam(spamdelay) << "armarx::RobotUnit::publish: Last sensor value update is "
- << sensSecondsAgo << " seconds ago!";
- }
- }
- if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
+ //update
+ if (haveSensorAndControlValsChanged)
+ {
+ timingMap["UnitUpdate"] = publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
+ timingMap["SensorUpdates"] = publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
+ }
+ else
+ {
+ const double sensSecondsAgo = (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
+ if (sensSecondsAgo > 1)
{
- timingMap["ControlUpdates"] = publishControlUpdates(
- controlThreadOutputBuffer,
- haveSensorAndControlValsChanged, publishToObserver,
- activatedControllers, requestedJointControllers);
+ ARMARX_WARNING << deactivateSpam(spamdelay) << "armarx::RobotUnit::publish: Last sensor value update is "
+ << sensSecondsAgo << " seconds ago!";
}
- //call publish hook + publish NJointController changes
- timingMap["NJointControllerUpdates"] = publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);
+ }
+ if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
+ {
+ timingMap["ControlUpdates"] = publishControlUpdates(
+ controlThreadOutputBuffer,
+ haveSensorAndControlValsChanged, publishToObserver,
+ activatedControllers, requestedJointControllers);
+ }
+ //call publish hook + publish NJointControllerBase changes
+ timingMap["NJointControllerUpdates"] = publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);
- //report new class names
- timingMap["ClassNameUpdates"] = publishNJointClassNames();
- timingMap["RobotUnitListenerFlush"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant){robotUnitListenerBatchPrx->ice_flushBatchRequests();}, lastControlThreadTimestamp
- };
+ //report new class names
+ timingMap["ClassNameUpdates"] = publishNJointClassNames();
+ timingMap["RobotUnitListenerFlush"] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant){robotUnitListenerBatchPrx->ice_flushBatchRequests();}, lastControlThreadTimestamp
+ };
- if (publishToObserver)
+ if (publishToObserver)
+ {
+ timingMap["LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
- timingMap["LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
- {
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
- robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
- robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
- }
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
}
+ }
- //warn if there are unset jointCtrl controllers
- {
- const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
- if (std::any_of(jointCtrls.begin(), jointCtrls.end(), [](const JointController * jointCtrl)
+ //warn if there are unset jointCtrl controllers
+ {
+ const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
+ if (std::any_of(jointCtrls.begin(), jointCtrls.end(), [](const JointController * jointCtrl)
+ {
+ return !jointCtrl;
+ }))
{
- return !jointCtrl;
- }))
- {
- ARMARX_WARNING << deactivateSpam(5) << "armarx::RobotUnit::publish: Some activated JointControllers are reported to be nullptr! "
- << "(did you forget to call rtSwitchControllerSetup()?)";
- }
+ ARMARX_WARNING << deactivateSpam(5) << "armarx::RobotUnit::publish: Some activated JointControllers are reported to be nullptr! "
+ << "(did you forget to call rtSwitchControllerSetup()?)";
}
- ++publishIterationCount;
- lastPublishLoop = TimeUtil::GetTime(true) - begPublish;
}
+ ++publishIterationCount;
+ lastPublishLoop = TimeUtil::GetTime(true) - begPublish;
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
index a10ed61e235d36bb2d00b1ac886b898bc37d0637..15b67d9fae37f0cd7b117759fb157136e76dafec 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -31,253 +31,253 @@
#include "RobotUnitModuleBase.h"
#include "../RobotUnitObserver.h"
+namespace armarx::detail
+{
+ struct ControlThreadOutputBufferEntry;
+}
+
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
-
- namespace detail
- {
- struct ControlThreadOutputBufferEntry;
- }
using SensorAndControl = detail::ControlThreadOutputBufferEntry;
+}
- namespace RobotUnitModule
+namespace armarx::RobotUnitModule
+{
+ class PublisherPropertyDefinitions: public ModuleBasePropertyDefinitions
{
- class PublisherPropertyDefinitions: public ModuleBasePropertyDefinitions
+ public:
+ PublisherPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
{
- public:
- PublisherPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>(
- "RobotUnitListenerTopicName", "RobotUnitListenerTopic",
- "The topic receiving events for RobotUnitListener");
- defineOptionalProperty<std::string>(
- "DebugDrawerUpdatesTopicName", "DebugDrawerUpdates",
- "The topic receiving events for debug drawing");
- defineOptionalProperty<std::size_t>(
- "PublishPeriodMs", 10,
- "Milliseconds between each publish");
+ defineOptionalProperty<std::string>(
+ "RobotUnitListenerTopicName", "RobotUnitListenerTopic",
+ "The topic receiving events for RobotUnitListener");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerUpdatesTopicName", "DebugDrawerUpdates",
+ "The topic receiving events for debug drawing");
+ defineOptionalProperty<std::size_t>(
+ "PublishPeriodMs", 10,
+ "Milliseconds between each publish");
- defineOptionalProperty<bool>(
- "ObserverPublishSensorValues", true,
- "Whether sensor values are send to the observer", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>(
- "ObserverPublishControlTargets", true,
- "Whether control targets are send to the observer", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<std::string>(
- "DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
- defineOptionalProperty<std::uint64_t>(
- "ObserverPrintEveryNthIterations", 1,
- "Only every nth iteration data is send to the debug observer", PropertyDefinitionBase::eModifiable);
- }
- };
+ defineOptionalProperty<bool>(
+ "ObserverPublishSensorValues", true,
+ "Whether sensor values are send to the observer", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>(
+ "ObserverPublishControlTargets", true,
+ "Whether control targets are send to the observer", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<std::string>(
+ "DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
+ defineOptionalProperty<std::uint64_t>(
+ "ObserverPrintEveryNthIterations", 1,
+ "Only every nth iteration data is send to the debug observer", PropertyDefinitionBase::eModifiable);
+ }
+ };
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" manages publishing of all data to Topics,
+ * updating of all units managed by \ref Units "the Units module" and calling of \ref NJointControllerBase hooks.
+ *
+ * @see ModuleBase
+ */
+ class Publisher : virtual public ModuleBase, virtual public RobotUnitPublishingInterface
+ {
+ friend class ModuleBase;
+ public:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" manages publishing of all data to Topics,
- * updating of all units managed by \ref Units "the Units module" and calling of \ref NJointController hooks.
- *
- * @see ModuleBase
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
*/
- class Publisher : virtual public ModuleBase, virtual public RobotUnitPublishingInterface
+ static Publisher& Instance()
{
- friend class ModuleBase;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static Publisher& Instance()
- {
- return ModuleBase::Instance<Publisher>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_icePropertiesInitialized
- void _icePropertiesInitialized();
- /// @see ModuleBase::_componentPropertiesUpdated
- void _componentPropertiesUpdated(const std::set<std::string>& changedProperties);
+ return ModuleBase::Instance<Publisher>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_icePropertiesInitialized
+ void _icePropertiesInitialized();
+ /// @see ModuleBase::_componentPropertiesUpdated
+ void _componentPropertiesUpdated(const std::set<std::string>& changedProperties);
- /// @see ModuleBase::_preOnInitRobotUnit
- void _preOnInitRobotUnit();
- /// @see ModuleBase::_preOnConnectRobotUnit
- void _preOnConnectRobotUnit();
- /// @see ModuleBase::_postFinishControlThreadInitialization
- void _postFinishControlThreadInitialization();
- /// @see ModuleBase::_preFinishRunning
- void _preFinishRunning();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// ice interface //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Returns the name of the used DebugDrawerTopic
- * @return The name of the used DebugDrawerTopic
- */
- std::string getDebugDrawerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the name of the used DebugObserverTopic
- * @return The name of the used DebugObserverTopic
- */
- std::string getDebugObserverTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the name of the used RobotUnitListenerTopic
- * @return The name of the used RobotUnitListenerTopic
- */
- std::string getRobotUnitListenerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
+ /// @see ModuleBase::_preOnInitRobotUnit
+ void _preOnInitRobotUnit();
+ /// @see ModuleBase::_preOnConnectRobotUnit
+ void _preOnConnectRobotUnit();
+ /// @see ModuleBase::_postFinishControlThreadInitialization
+ void _postFinishControlThreadInitialization();
+ /// @see ModuleBase::_preFinishRunning
+ void _preFinishRunning();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns the name of the used DebugDrawerTopic
+ * @return The name of the used DebugDrawerTopic
+ */
+ std::string getDebugDrawerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the name of the used DebugObserverTopic
+ * @return The name of the used DebugObserverTopic
+ */
+ std::string getDebugObserverTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the name of the used RobotUnitListenerTopic
+ * @return The name of the used RobotUnitListenerTopic
+ */
+ std::string getRobotUnitListenerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the used DebugDrawerProxy
- * @return The used DebugDrawerProxy
- */
- DebugDrawerInterfacePrx getDebugDrawerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the used RobotUnitListenerProxy
- * @return The used RobotUnitListenerProxy
- */
- RobotUnitListenerPrx getRobotUnitListenerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the used DebugObserverProxy
- * @return The used DebugObserverProxy
- */
- DebugObserverInterfacePrx getDebugObserverProxy(const Ice::Current& = Ice::emptyCurrent) const override;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////// Module interface /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- /**
- * @brief Publishes data
- * @param additionalMap Data published to the RobotUnitObserver's additional channel (This field is used by deriving classes)
- * @param timingMap Data published to the RobotUnitObserver's timing channel (This field is used by deriving classes)
- */
- virtual void publish(StringVariantBaseMap additionalMap = {}, StringVariantBaseMap timingMap = {});
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////// implementation //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * @brief Returns a map containing all current NJointControllers.
- * @return A map containing all current NJointControllers.
- */
- const std::map<std::string, NJointControllerPtr>& getNJointControllers();
- /**
- * @brief Returns the used RobotUnitObserver
- * @return The used RobotUnitObserver
- */
- const RobotUnitObserverPtr& getRobotUnitObserver() const
- {
- ARMARX_CHECK_EXPRESSION(robotUnitObserver);
- return robotUnitObserver;
- }
- /**
- * @brief Publishes updtes about new classes od \ref NJointController "NJointControllers"
- * @return The time required by this function as a \ref Variant
- */
- TimedVariantPtr publishNJointClassNames();
- /**
- * @brief Publishes updates of \ref NJointController "NJointControllers" ([de]activate + publish hooks)
- * @param timingMap Timings of this publish iteration (out param)
- * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
- * @return The time required by this function as a \ref Variant
- * @see NJointController::onPublish
- */
- TimedVariantPtr publishNJointControllerUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer);
- /**
- * @brief Updates all sub units and publishes the timings of these updates
- * @param timingMap Timings of this publish iteration (out param)
- * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
- * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointController "NJointControllers"
- * active in the published \ref ControlThread iteration
- * @return The time required by this function as a \ref Variant
- */
- TimedVariantPtr publishUnitUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer,
- const JointAndNJointControllers& activatedControllers);
- /**
- * @brief Publishes data about updates of \ref JointController "JointControllers" and their \ref ControlTargetBase "ControlTargets"
- * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
- * @param haveSensorAndControlValsChanged Whether \ref ControlTargetBase "ControlTargets" were updated by the \ref ControlThread
- * @param publishToObserver Whether data should be published to observers
- * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointController "NJointControllers"
- * active in the published \ref ControlThread iteration
- * @param requestedJointControllers
- * @return The time required by this function as a \ref Variant
- */
- TimedVariantPtr publishControlUpdates(
- const SensorAndControl& controlThreadOutputBuffer,
- bool haveSensorAndControlValsChanged,
- bool publishToObserver,
- const JointAndNJointControllers& activatedControllers,
- const std::vector<JointController*>& requestedJointControllers);
- /**
- * @brief Publishes Updates about \ref SensorValueBase "SensorValues" (To \ref RobotUnitListener and \ref RobotUnitObserver)
- * @param publishToObserver Whether data should be published to observers
- * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
- * @return The time required by this function as a \ref Variant
- */
- TimedVariantPtr publishSensorUpdates(
- bool publishToObserver,
- const SensorAndControl& controlThreadOutputBuffer);
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- static constexpr int spamdelay = 30;
+ /**
+ * @brief Returns the used DebugDrawerProxy
+ * @return The used DebugDrawerProxy
+ */
+ DebugDrawerInterfacePrx getDebugDrawerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the used RobotUnitListenerProxy
+ * @return The used RobotUnitListenerProxy
+ */
+ RobotUnitListenerPrx getRobotUnitListenerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the used DebugObserverProxy
+ * @return The used DebugObserverProxy
+ */
+ DebugObserverInterfacePrx getDebugObserverProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// Module interface /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ /**
+ * @brief Publishes data
+ * @param additionalMap Data published to the RobotUnitObserver's additional channel (This field is used by deriving classes)
+ * @param timingMap Data published to the RobotUnitObserver's timing channel (This field is used by deriving classes)
+ */
+ virtual void publish(StringVariantBaseMap additionalMap = {}, StringVariantBaseMap timingMap = {});
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * @brief Returns a map containing all current NJointControllers.
+ * @return A map containing all current NJointControllers.
+ */
+ const std::map<std::string, NJointControllerBasePtr>& getNJointControllers();
+ /**
+ * @brief Returns the used RobotUnitObserver
+ * @return The used RobotUnitObserver
+ */
+ const RobotUnitObserverPtr& getRobotUnitObserver() const
+ {
+ ARMARX_CHECK_EXPRESSION(robotUnitObserver);
+ return robotUnitObserver;
+ }
+ /**
+ * @brief Publishes updtes about new classes od \ref NJointControllerBase "NJointControllers"
+ * @return The time required by this function as a \ref Variant
+ */
+ TimedVariantPtr publishNJointClassNames();
+ /**
+ * @brief Publishes updates of \ref NJointControllerBase "NJointControllers" ([de]activate + publish hooks)
+ * @param timingMap Timings of this publish iteration (out param)
+ * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
+ * @return The time required by this function as a \ref Variant
+ * @see NJointControllerBase::onPublish
+ */
+ TimedVariantPtr publishNJointControllerUpdates(
+ StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer);
+ /**
+ * @brief Updates all sub units and publishes the timings of these updates
+ * @param timingMap Timings of this publish iteration (out param)
+ * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
+ * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointControllerBase "NJointControllers"
+ * active in the published \ref ControlThread iteration
+ * @return The time required by this function as a \ref Variant
+ */
+ TimedVariantPtr publishUnitUpdates(
+ StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ const JointAndNJointControllers& activatedControllers);
+ /**
+ * @brief Publishes data about updates of \ref JointController "JointControllers" and their \ref ControlTargetBase "ControlTargets"
+ * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
+ * @param haveSensorAndControlValsChanged Whether \ref ControlTargetBase "ControlTargets" were updated by the \ref ControlThread
+ * @param publishToObserver Whether data should be published to observers
+ * @param activatedControllers The \ref JointController "JointControllers" and \ref NJointControllerBase "NJointControllers"
+ * active in the published \ref ControlThread iteration
+ * @param requestedJointControllers
+ * @return The time required by this function as a \ref Variant
+ */
+ TimedVariantPtr publishControlUpdates(
+ const SensorAndControl& controlThreadOutputBuffer,
+ bool haveSensorAndControlValsChanged,
+ bool publishToObserver,
+ const JointAndNJointControllers& activatedControllers,
+ const std::vector<JointController*>& requestedJointControllers);
+ /**
+ * @brief Publishes Updates about \ref SensorValueBase "SensorValues" (To \ref RobotUnitListener and \ref RobotUnitObserver)
+ * @param publishToObserver Whether data should be published to observers
+ * @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
+ * @return The time required by this function as a \ref Variant
+ */
+ TimedVariantPtr publishSensorUpdates(
+ bool publishToObserver,
+ const SensorAndControl& controlThreadOutputBuffer);
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ static constexpr int spamdelay = 30;
- using PublisherTaskT = SimplePeriodicTask<std::function<void(void)>>;
+ using PublisherTaskT = SimplePeriodicTask<std::function<void(void)>>;
- /// @brief The name of the used RobotUnitListenerTopic
- std::string robotUnitListenerTopicName;
- /// @brief The name of the used DebugDrawerTopic
- std::string debugDrawerUpdatesTopicName;
- /// @brief The name of the used DebugObserverTopic
- std::string debugObserverTopicName;
+ /// @brief The name of the used RobotUnitListenerTopic
+ std::string robotUnitListenerTopicName;
+ /// @brief The name of the used DebugDrawerTopic
+ std::string debugDrawerUpdatesTopicName;
+ /// @brief The name of the used DebugObserverTopic
+ std::string debugObserverTopicName;
- /// @brief The used DebugDrawerProxy
- DebugDrawerInterfacePrx debugDrawerPrx;
- /// @brief The used DebugObserverProxy
- DebugObserverInterfacePrx debugObserverPrx;
+ /// @brief The used DebugDrawerProxy
+ DebugDrawerInterfacePrx debugDrawerPrx;
+ /// @brief The used DebugObserverProxy
+ DebugObserverInterfacePrx debugObserverPrx;
- /// @brief the number of calles to \ref publish
- std::uint64_t publishIterationCount {0};
+ /// @brief the number of calles to \ref publish
+ std::uint64_t publishIterationCount {0};
- /// @brief The time required by the last iteration of \ref publish to publish sensor data
- IceUtil::Time publishNewSensorDataTime;
- /// @brief The thread executing the publisher loop
- PublisherTaskT::pointer_type publisherTask; ///TODO use std thread
- /// @brief The already reported classes of \ref NJointController "NJointControllers"
- std::set<std::string> lastReportedClasses;
+ /// @brief The time required by the last iteration of \ref publish to publish sensor data
+ IceUtil::Time publishNewSensorDataTime;
+ /// @brief The thread executing the publisher loop
+ PublisherTaskT::pointer_type publisherTask; ///TODO use std thread
+ /// @brief The already reported classes of \ref NJointControllerBase "NJointControllers"
+ std::set<std::string> lastReportedClasses;
- /// @brief Whether \ref SensorValueBase "SensorValues" should be published to the observers
- std::atomic_bool observerPublishSensorValues;
- /// @brief Whether \ref ControlTargetBase "ControlTargets" should be published to the observers
- std::atomic_bool observerPublishControlTargets;
- /// @brief How many iterations of \ref publish shold not publish data to the debug observer.
- std::atomic<std::uint64_t> debugObserverSkipIterations;
+ /// @brief Whether \ref SensorValueBase "SensorValues" should be published to the observers
+ std::atomic_bool observerPublishSensorValues;
+ /// @brief Whether \ref ControlTargetBase "ControlTargets" should be published to the observers
+ std::atomic_bool observerPublishControlTargets;
+ /// @brief How many iterations of \ref publish shold not publish data to the debug observer.
+ std::atomic<std::uint64_t> debugObserverSkipIterations;
- /// @brief The time required by the last iteration of \ref publish
- /// \warning May only be accessed by the publish thread.
- IceUtil::Time lastPublishLoop;
+ /// @brief The time required by the last iteration of \ref publish
+ /// \warning May only be accessed by the publish thread.
+ IceUtil::Time lastPublishLoop;
- /// @brief The used RobotUnitObserver
- RobotUnitObserverPtr robotUnitObserver;
+ /// @brief The used RobotUnitObserver
+ RobotUnitObserverPtr robotUnitObserver;
- /// @brief The period of the publisher loop
- std::size_t publishPeriodMs {1};
+ /// @brief The period of the publisher loop
+ std::size_t publishPeriodMs {1};
- /// @brief A proxy to the used RobotUnitListener topic
- RobotUnitListenerPrx robotUnitListenerPrx;
- /// @brief A batch proxy to the used RobotUnitListener topic
- /// \warning May only be accessed by the publish thread.
- RobotUnitListenerPrx robotUnitListenerBatchPrx;
+ /// @brief A proxy to the used RobotUnitListener topic
+ RobotUnitListenerPrx robotUnitListenerPrx;
+ /// @brief A batch proxy to the used RobotUnitListener topic
+ /// \warning May only be accessed by the publish thread.
+ RobotUnitListenerPrx robotUnitListenerBatchPrx;
- /// \warning May only be accessed by the publish thread.
- IceUtil::Time lastControlThreadTimestamp;
- };
- }
+ /// \warning May only be accessed by the publish thread.
+ IceUtil::Time lastControlThreadTimestamp;
+ };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index e6d317984ddc31b1c9d9bb92947ada0d700b2a4a..12acad70bbf3e49c8d2aaa4ff6e69c299570fd8c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -30,125 +30,122 @@
#include "RobotUnitModuleRobotData.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ const std::string& RobotData::getRobotPlatformName() const
{
- const std::string& RobotData::getRobotPlatformName() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
- return robotPlatformName;
- }
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
+ return robotPlatformName;
+ }
- std::string RobotData::getRobotPlatformInstanceName() const
- {
- return robotPlatformInstanceName;
- }
+ std::string RobotData::getRobotPlatformInstanceName() const
+ {
+ return robotPlatformInstanceName;
+ }
- const std::string& RobotData::getRobotNodetSeName() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
- return robotNodeSetName;
- }
+ const std::string& RobotData::getRobotNodetSeName() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
+ return robotNodeSetName;
+ }
- const std::string& RobotData::getRobotProjectName() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
- return robotProjectName;
- }
+ const std::string& RobotData::getRobotProjectName() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
+ return robotProjectName;
+ }
- const std::string& RobotData::getRobotFileName() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
- return robotFileName;
- }
+ const std::string& RobotData::getRobotFileName() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
+ return robotFileName;
+ }
- std::string RobotData::getRobotName() const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
- return robot->getName();
- }
+ std::string RobotData::getRobotName() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {robotMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
+ return robot->getName();
+ }
- VirtualRobot::RobotPtr RobotData::cloneRobot(bool updateCollisionModel) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
- ARMARX_CHECK_EXPRESSION(robotPool);
- const VirtualRobot::RobotPtr clone = robotPool->getRobot();
- clone->setUpdateVisualization(false);
- clone->setUpdateCollisionModel(updateCollisionModel);
- return clone;
- }
+ VirtualRobot::RobotPtr RobotData::cloneRobot(bool updateCollisionModel) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {robotMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
+ ARMARX_CHECK_EXPRESSION(robotPool);
+ const VirtualRobot::RobotPtr clone = robotPool->getRobot();
+ clone->setUpdateVisualization(false);
+ clone->setUpdateCollisionModel(updateCollisionModel);
+ return clone;
+ }
- void RobotData::_initVirtualRobot()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
- ARMARX_CHECK_IS_NULL(robot);
+ void RobotData::_initVirtualRobot()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {robotMutex};
+ ARMARX_CHECK_IS_NULL(robot);
- std::string robotName = getProperty<std::string>("RobotName").getValue();
+ std::string robotName = getProperty<std::string>("RobotName").getValue();
- robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
- robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
- robotFileName = getProperty<std::string>("RobotFileName").getValue();
- robotPlatformName = getProperty<std::string>("PlatformName").getValue();
- robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+ robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
+ robotFileName = getProperty<std::string>("RobotFileName").getValue();
+ robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+ robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
- //load robot
+ //load robot
+ {
+ Ice::StringSeq includePaths;
+ if (!robotProjectName.empty())
{
- Ice::StringSeq includePaths;
- if (!robotProjectName.empty())
- {
- CMakePackageFinder finder(robotProjectName);
- auto pathsString = finder.getDataDir();
- boost::split(includePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
- ArmarXDataPath::addDataPaths(includePaths);
- }
- if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
- {
- std::stringstream str;
- str << "Could not find robot file " + robotFileName
- << "\nCollected paths from RobotFileNameProject '" << robotProjectName << "':" << includePaths;
- throw UserException(str.str());
- }
- // read the robot
- try
+ CMakePackageFinder finder(robotProjectName);
+ auto pathsString = finder.getDataDir();
+ boost::split(includePaths,
+ pathsString,
+ boost::is_any_of(";,"),
+ boost::token_compress_on);
+ ArmarXDataPath::addDataPaths(includePaths);
+ }
+ if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
+ {
+ std::stringstream str;
+ str << "Could not find robot file " + robotFileName
+ << "\nCollected paths from RobotFileNameProject '" << robotProjectName << "':" << includePaths;
+ throw UserException(str.str());
+ }
+ // read the robot
+ try
+ {
+ robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
+ if (robotName.empty())
{
- robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
- if (robotName.empty())
- {
- robotName = robot->getName();
- }
- else
- {
- robot->setName(robotName);
- }
+ robotName = robot->getName();
}
- catch (VirtualRobot::VirtualRobotException& e)
+ else
{
- throw UserException(e.what());
+ robot->setName(robotName);
}
- ARMARX_INFO << "Loaded robot:"
- << "\n\tProject : " << robotProjectName
- << "\n\tName : " << robotName
- << "\n\tRobot file : " << robotFileName
- << "\n\tRobotNodeSet : " << robotNodeSetName
- << "\n\tNodeNames : " << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
- ARMARX_CHECK_NOT_NULL(robot);
- robotPool.reset(new RobotPool(robot, 10));
}
+ catch (VirtualRobot::VirtualRobotException& e)
+ {
+ throw UserException(e.what());
+ }
+ ARMARX_INFO << "Loaded robot:"
+ << "\n\tProject : " << robotProjectName
+ << "\n\tName : " << robotName
+ << "\n\tRobot file : " << robotFileName
+ << "\n\tRobotNodeSet : " << robotNodeSetName
+ << "\n\tNodeNames : " << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
+ ARMARX_CHECK_NOT_NULL(robot);
+ robotPool.reset(new RobotPool(robot, 10));
}
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index ff1e0637952cff7229a8ece8af322cfb205bd23f..126aa742114c889b2cef6eeabfdf70302b91bd07 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -33,123 +33,124 @@
namespace armarx
{
using RobotPoolPtr = std::shared_ptr<class RobotPool>;
+}
- namespace RobotUnitModule
+namespace armarx::RobotUnitModule
+{
+ class RobotDataPropertyDefinitions: public ModuleBasePropertyDefinitions
{
- class RobotDataPropertyDefinitions: public ModuleBasePropertyDefinitions
+ public:
+ RobotDataPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
{
- public:
- RobotDataPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
- {
- defineRequiredProperty<std::string>(
- "RobotFileName", "Robot file name, e.g. robot_model.xml");
- defineOptionalProperty<std::string>(
- "RobotFileNameProject", "",
- "Project in which the robot filename is located (if robot is loaded from an external project)");
-
- defineOptionalProperty<std::string>(
- "RobotName", "",
- "Override robot name if you want to load multiple robots of the same type");
- defineOptionalProperty<std::string>(
- "RobotNodeSetName", "Robot",
- "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
- defineOptionalProperty<std::string>(
- "PlatformName", "Platform",
- "Name of the platform needs to correspond to a node in the virtual robot.");
- defineOptionalProperty<std::string>(
- "PlatformInstanceName", "Platform",
- "Name of the platform instance (will publish values on PlatformInstanceName + 'State')");
-
- }
- };
+ defineRequiredProperty<std::string>(
+ "RobotFileName", "Robot file name, e.g. robot_model.xml");
+ defineOptionalProperty<std::string>(
+ "RobotFileNameProject", "",
+ "Project in which the robot filename is located (if robot is loaded from an external project)");
+
+ defineOptionalProperty<std::string>(
+ "RobotName", "",
+ "Override robot name if you want to load multiple robots of the same type");
+ defineOptionalProperty<std::string>(
+ "RobotNodeSetName", "Robot",
+ "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+ defineOptionalProperty<std::string>(
+ "PlatformName", "Platform",
+ "Name of the platform needs to correspond to a node in the virtual robot.");
+ defineOptionalProperty<std::string>(
+ "PlatformInstanceName", "Platform",
+ "Name of the platform instance (will publish values on PlatformInstanceName + 'State')");
+
+ }
+ };
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" holds all high-level data about the robot.
+ *
+ * @see ModuleBase
+ */
+ class RobotData : virtual public ModuleBase
+ {
+ friend class ModuleBase;
+ public:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" holds all high-level data about the robot.
- *
- * @see ModuleBase
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
*/
- class RobotData : virtual public ModuleBase
+ static RobotData& Instance()
{
- friend class ModuleBase;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static RobotData& Instance()
- {
- return ModuleBase::Instance<RobotData>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_preOnInitRobotUnit
- void _initVirtualRobot();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////// Module interface /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Returns the name of the robot's platform
- * @return The name of the robot's platform
- */
- const std::string& getRobotPlatformName() const;
- /**
- * @brief Returns the name of the robot's RobotNodeSet
- * @return The name of the robot's RobotNodeSet
- */
- const std::string& getRobotNodetSeName() const;
- /**
- * @brief Returns the name of the project containing the robot's model
- * @return The name of the project containing the robot's model
- */
- const std::string& getRobotProjectName() const;
- /**
- * @brief Returns the file name of the robot's model
- * @return The file name of the robot's model
- */
- const std::string& getRobotFileName() const;
-
- /**
- * @brief Returns the robot's name
- * @return The robot's name
- */
- std::string getRobotName() const;
-
- /**
- * @brief Returns the name of the robot platform instance. Used for the platform topic: RobotPlatformInstance + "State"
- */
- std::string getRobotPlatformInstanceName() const;
-
-
- /**
- * @brief Returns a clone of the robot's model
- * @return A clone of the robot's model
- */
- VirtualRobot::RobotPtr cloneRobot(bool updateCollisionModel = false) const;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
-
- private:
- /// @brief The name of the robot's RobotNodeSet
- std::string robotNodeSetName;
- /// @brief The name of the project containing the robot's model
- std::string robotProjectName;
- /// @brief The file name of the robot's model
- std::string robotFileName;
- /// @brief The name of the robot's platform
- std::string robotPlatformName;
- /// @brief The name of the robot's platform instance
- std::string robotPlatformInstanceName;
-
- /// @brief The robot's model.
- VirtualRobot::RobotPtr robot;
- RobotPoolPtr robotPool;
- /// @brief A mutex guarding \ref robot
- mutable std::mutex robotMutex;
- };
- }
+ return ModuleBase::Instance<RobotData>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_preOnInitRobotUnit
+ void _initVirtualRobot();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// Module interface /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns the name of the robot's platform
+ * @return The name of the robot's platform
+ */
+ const std::string& getRobotPlatformName() const;
+ /**
+ * @brief Returns the name of the robot's RobotNodeSet
+ * @return The name of the robot's RobotNodeSet
+ */
+ const std::string& getRobotNodetSeName() const;
+ /**
+ * @brief Returns the name of the project containing the robot's model
+ * @return The name of the project containing the robot's model
+ */
+ const std::string& getRobotProjectName() const;
+ /**
+ * @brief Returns the file name of the robot's model
+ * @return The file name of the robot's model
+ */
+ const std::string& getRobotFileName() const;
+
+ /**
+ * @brief Returns the robot's name
+ * @return The robot's name
+ */
+ std::string getRobotName() const;
+
+ /**
+ * @brief Returns the name of the robot platform instance. Used for the platform topic: RobotPlatformInstance + "State"
+ */
+ std::string getRobotPlatformInstanceName() const;
+
+
+ /**
+ * @brief Returns a clone of the robot's model
+ * @return A clone of the robot's model
+ */
+ VirtualRobot::RobotPtr cloneRobot(bool updateCollisionModel = false) const;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+
+ private:
+ /// @brief The name of the robot's RobotNodeSet
+ std::string robotNodeSetName;
+ /// @brief The name of the project containing the robot's model
+ std::string robotProjectName;
+ /// @brief The file name of the robot's model
+ std::string robotFileName;
+ /// @brief The name of the robot's platform
+ std::string robotPlatformName;
+ /// @brief The name of the robot's platform instance
+ std::string robotPlatformInstanceName;
+
+ /// @brief The robot's model.
+ VirtualRobot::RobotPtr robot;
+ RobotPoolPtr robotPool;
+ /// @brief A mutex guarding \ref robot
+ mutable std::mutex robotMutex;
+ };
}
+
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 7405b7b789fba2278ffba3af9572d8bce6b35b47..f74a0d463b60018b8a6eba1f4beec34ced4ca8f5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -30,315 +30,313 @@
#define FLOOR_OBJ_STR "FLOOR"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ void SelfCollisionChecker::_preOnInitRobotUnit()
{
- void SelfCollisionChecker::_preOnInitRobotUnit()
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //get the robot
+ selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
+ //get pairs to check
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //get the robot
- selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
- //get pairs to check
+ const std::string colModelsString = getProperty<std::string>("SelfCollisionCheck_ModelGroupsToCheck").getValue();
+ std::vector<std::string> groups;
+ if (!colModelsString.empty())
{
- const std::string colModelsString = getProperty<std::string>("SelfCollisionCheck_ModelGroupsToCheck").getValue();
- std::vector<std::string> groups;
- if (!colModelsString.empty())
- {
- groups = armarx::Split(colModelsString, ";", true, true);
- }
- ARMARX_DEBUG << "Processing groups for self collision checking:";
- for (std::string& group : groups)
+ groups = armarx::Split(colModelsString, ";", true, true);
+ }
+ ARMARX_DEBUG << "Processing groups for self collision checking:";
+ for (std::string& group : groups)
+ {
+ ARMARX_DEBUG << "---- group: " << group;
+ // Removing parentheses
+ boost::trim_if(group, boost::is_any_of(" \t{}"));
+ std::set<std::set<std::string>> setsOfNode;
{
- ARMARX_DEBUG << "---- group: " << group;
- // Removing parentheses
- boost::trim_if(group, boost::is_any_of(" \t{}"));
- std::set<std::set<std::string>> setsOfNode;
+ auto splittedRaw = armarx::Split(group, ",", true, true);
+ if (splittedRaw.size() < 2)
{
- auto splittedRaw = armarx::Split(group, ",", true, true);
- if (splittedRaw.size() < 2)
- {
- continue;
- }
- for (auto& subentry : splittedRaw)
+ continue;
+ }
+ for (auto& subentry : splittedRaw)
+ {
+ boost::trim_if(subentry, boost::is_any_of(" \t{}"));
+ if (selfCollisionAvoidanceRobot->hasRobotNodeSet(subentry))
{
- boost::trim_if(subentry, boost::is_any_of(" \t{}"));
- if (selfCollisionAvoidanceRobot->hasRobotNodeSet(subentry))
+ std::set<std::string> nodes;
+ //the entry is a set
+ for (const auto& node : selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)->getAllRobotNodes())
{
- std::set<std::string> nodes;
- //the entry is a set
- for (const auto& node : selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)->getAllRobotNodes())
- {
- if (!node->getCollisionModel())
- {
-
- ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '"
- << node->getName() << "'";
- continue;
- }
- nodes.emplace(node->getName());
- ARMARX_DEBUG << "-------- from set: " << subentry << ", node: " << node->getName();
- }
- setsOfNode.emplace(std::move(nodes));
- }
- else if (selfCollisionAvoidanceRobot->hasRobotNode(subentry))
- {
- //the entry is a node
- if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)->getCollisionModel())
+ if (!node->getCollisionModel())
{
ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '"
- << selfCollisionAvoidanceRobot->getRobotNode(subentry)->getName() << "'";
+ << node->getName() << "'";
continue;
}
- setsOfNode.emplace(std::set<std::string> {subentry});
- ARMARX_DEBUG << "-------- node: " << subentry;
- }
- else if (subentry == FLOOR_OBJ_STR)
- {
- //the entry is the floor
- setsOfNode.emplace(std::set<std::string> {subentry});
- ARMARX_DEBUG << "-------- floor: " << subentry;
+ nodes.emplace(node->getName());
+ ARMARX_DEBUG << "-------- from set: " << subentry << ", node: " << node->getName();
}
- else
+ setsOfNode.emplace(std::move(nodes));
+ }
+ else if (selfCollisionAvoidanceRobot->hasRobotNode(subentry))
+ {
+ //the entry is a node
+ if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)->getCollisionModel())
{
- ARMARX_ERROR << "No RobotNodeSet or RobotNode with name '"
- << subentry
- << "' defined in " << _module<RobotData>().getRobotFileName() << ". Skipping.";
+
+ ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '"
+ << selfCollisionAvoidanceRobot->getRobotNode(subentry)->getName() << "'";
continue;
}
+ setsOfNode.emplace(std::set<std::string> {subentry});
+ ARMARX_DEBUG << "-------- node: " << subentry;
+ }
+ else if (subentry == FLOOR_OBJ_STR)
+ {
+ //the entry is the floor
+ setsOfNode.emplace(std::set<std::string> {subentry});
+ ARMARX_DEBUG << "-------- floor: " << subentry;
+ }
+ else
+ {
+ ARMARX_ERROR << "No RobotNodeSet or RobotNode with name '"
+ << subentry
+ << "' defined in " << _module<RobotData>().getRobotFileName() << ". Skipping.";
+ continue;
}
}
+ }
- auto addCombinationOfSetsToCollisionCheck = [this](const std::set<std::string>& a, const std::set<std::string>& b)
+ auto addCombinationOfSetsToCollisionCheck = [this](const std::set<std::string>& a, const std::set<std::string>& b)
+ {
+ for (const auto& nodeA : a)
{
- for (const auto& nodeA : a)
+ for (const auto& nodeB : b)
{
- for (const auto& nodeB : b)
+ if (nodeA == nodeB)
{
- if (nodeA == nodeB)
- {
- continue;
- }
- if (nodeA < nodeB)
- {
- ARMARX_DEBUG << "------------ " << nodeA << " " << nodeB;
- namePairsToCheck.emplace(nodeA, nodeB);
- }
- else
- {
- ARMARX_DEBUG << "------------ " << nodeB << " " << nodeA;
- namePairsToCheck.emplace(nodeB, nodeA);
- }
+ continue;
+ }
+ if (nodeA < nodeB)
+ {
+ ARMARX_DEBUG << "------------ " << nodeA << " " << nodeB;
+ namePairsToCheck.emplace(nodeA, nodeB);
+ }
+ else
+ {
+ ARMARX_DEBUG << "------------ " << nodeB << " " << nodeA;
+ namePairsToCheck.emplace(nodeB, nodeA);
}
}
- };
+ }
+ };
- ARMARX_DEBUG << "-------- adding pairs to check:";
- for (auto setAIt = setsOfNode.begin(); setAIt != setsOfNode.end(); ++setAIt)
+ ARMARX_DEBUG << "-------- adding pairs to check:";
+ for (auto setAIt = setsOfNode.begin(); setAIt != setsOfNode.end(); ++setAIt)
+ {
+ auto setBIt = setAIt;
+ ++setBIt;
+ for (; setBIt != setsOfNode.end(); ++setBIt)
{
- auto setBIt = setAIt;
- ++setBIt;
- for (; setBIt != setsOfNode.end(); ++setBIt)
- {
- addCombinationOfSetsToCollisionCheck(*setAIt, *setBIt);
- }
+ addCombinationOfSetsToCollisionCheck(*setAIt, *setBIt);
}
+ }
- ARMARX_DEBUG << "-------- group: " << group << "...DONE!\n";
- }
- ARMARX_DEBUG << "Processing groups for self collision checking...DONE!";
+ ARMARX_DEBUG << "-------- group: " << group << "...DONE!\n";
}
- setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
- setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
+ ARMARX_DEBUG << "Processing groups for self collision checking...DONE!";
}
+ setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
+ setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
+ }
- void SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current&)
+ void SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current&)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
+ if (distance < 0)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
- if (distance < 0)
- {
- throw InvalidArgumentException
- {
- std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal distance:" + std::to_string(distance)
- };
- }
- if (distance == minSelfDistance && !nodePairsToCheck.empty())
+ throw InvalidArgumentException
{
- return;
- }
- //reset data
- ARMARX_CHECK_GREATER(distance, 0);
- minSelfDistance = distance;
- selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
- selfCollisionAvoidanceRobotNodes = selfCollisionAvoidanceRobot->getRobotNodes();
- nodePairsToCheck.clear();
- //set floor
+ std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal distance:" + std::to_string(distance)
+ };
+ }
+ if (distance == minSelfDistance && !nodePairsToCheck.empty())
+ {
+ return;
+ }
+ //reset data
+ ARMARX_CHECK_GREATER(distance, 0);
+ minSelfDistance = distance;
+ selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
+ selfCollisionAvoidanceRobotNodes = selfCollisionAvoidanceRobot->getRobotNodes();
+ nodePairsToCheck.clear();
+ //set floor
+ {
+ floor.reset(new VirtualRobot::SceneObjectSet("FLOORSET", selfCollisionAvoidanceRobot->getCollisionChecker()));
+ static constexpr float floorSize = 1e16f;
+ VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(floorSize, floorSize, std::min(0.001f, minSelfDistance / 2),
+ VirtualRobot::VisualizationFactory::Color::Red(), "",
+ selfCollisionAvoidanceRobot->getCollisionChecker());
+ boxOb->setGlobalPose(Eigen::Matrix4f::Identity());
+ boxOb->setName(FLOOR_OBJ_STR);
+ floor->addSceneObject(boxOb);
+ }
+ //inflate robot
+ for (const auto& node : selfCollisionAvoidanceRobotNodes)
+ {
+ if (node->getCollisionModel())
{
- floor.reset(new VirtualRobot::SceneObjectSet("FLOORSET", selfCollisionAvoidanceRobot->getCollisionChecker()));
- static constexpr float floorSize = 1e16f;
- VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(floorSize, floorSize, std::min(0.001f, minSelfDistance / 2),
- VirtualRobot::VisualizationFactory::Color::Red(), "",
- selfCollisionAvoidanceRobot->getCollisionChecker());
- boxOb->setGlobalPose(Eigen::Matrix4f::Identity());
- boxOb->setName(FLOOR_OBJ_STR);
- floor->addSceneObject(boxOb);
+ node->getCollisionModel()->inflateModel(minSelfDistance / 2.f);
}
- //inflate robot
- for (const auto& node : selfCollisionAvoidanceRobotNodes)
- {
- if (node->getCollisionModel())
- {
- node->getCollisionModel()->inflateModel(minSelfDistance / 2.f);
- }
- }
- //collect pairs
- for (const auto& pair : namePairsToCheck)
- {
- VirtualRobot::SceneObjectPtr first =
- (pair.first == FLOOR_OBJ_STR) ?
- floor->getSceneObject(0) :
- selfCollisionAvoidanceRobot->getRobotNode(pair.first);
+ }
+ //collect pairs
+ for (const auto& pair : namePairsToCheck)
+ {
+ VirtualRobot::SceneObjectPtr first =
+ (pair.first == FLOOR_OBJ_STR) ?
+ floor->getSceneObject(0) :
+ selfCollisionAvoidanceRobot->getRobotNode(pair.first);
- VirtualRobot::SceneObjectPtr second =
- (pair.second == FLOOR_OBJ_STR) ?
- floor->getSceneObject(0) :
- selfCollisionAvoidanceRobot->getRobotNode(pair.second);
+ VirtualRobot::SceneObjectPtr second =
+ (pair.second == FLOOR_OBJ_STR) ?
+ floor->getSceneObject(0) :
+ selfCollisionAvoidanceRobot->getRobotNode(pair.second);
- nodePairsToCheck.emplace_back(first, second);
- }
- ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.size());
+ nodePairsToCheck.emplace_back(first, second);
}
+ ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.size());
+ }
- void SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
+ void SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (freq < 0)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- if (freq < 0)
+ throw InvalidArgumentException
{
- throw InvalidArgumentException
- {
- std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal frequency:" + std::to_string(freq)
- };
- }
- checkFrequency = freq;
+ std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal frequency:" + std::to_string(freq)
+ };
}
+ checkFrequency = freq;
+ }
- bool SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return checkFrequency != 0;
- }
+ bool SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return checkFrequency != 0;
+ }
- float SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
+ float SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return checkFrequency;
+ }
+
+ float SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return minSelfDistance;
+ }
+
+ void SelfCollisionChecker::_componentPropertiesUpdated(const std::set<std::string>& changed)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (changed.count("SelfCollisionCheck_Frequency"))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return checkFrequency;
+ setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
}
-
- float SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
+ if (changed.count("SelfCollisionCheck_MinSelfDistance"))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return minSelfDistance;
+ setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
}
+ }
+
+ void SelfCollisionChecker::_postFinishControlThreadInitialization()
+ {
+ selfCollisionAvoidanceThread = std::thread {[&]{selfCollisionAvoidanceTask();}};
+ }
- void SelfCollisionChecker::_componentPropertiesUpdated(const std::set<std::string>& changed)
+ void SelfCollisionChecker::selfCollisionAvoidanceTask()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_INFO << "Self collision checker: entered selfCollisionAvoidanceTask";
+ ARMARX_ON_SCOPE_EXIT { ARMARX_INFO << "Self collision checker: leaving selfCollisionAvoidanceTask"; };
+ while (true)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- if (changed.count("SelfCollisionCheck_Frequency"))
+ const auto startT = std::chrono::high_resolution_clock::now();
+ //done
+ if (isShuttingDown())
{
- setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
+ return;
}
- if (changed.count("SelfCollisionCheck_MinSelfDistance"))
+ const auto freq = checkFrequency.load();
+ const bool inEmergencyStop = _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
+ if (inEmergencyStop || freq == 0)
{
- setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
+ ARMARX_INFO << deactivateSpam() << "Self collision checker: skipping check " << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
+ //currently wait
+ std::this_thread::sleep_for(std::chrono::microseconds {1000});
+ continue;
}
- }
-
- void SelfCollisionChecker::_postFinishControlThreadInitialization()
- {
- selfCollisionAvoidanceThread = std::thread {[&]{selfCollisionAvoidanceTask();}};
- }
-
- void SelfCollisionChecker::selfCollisionAvoidanceTask()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_INFO << "Self collision checker: entered selfCollisionAvoidanceTask";
- ARMARX_ON_SCOPE_EXIT { ARMARX_INFO << "Self collision checker: leaving selfCollisionAvoidanceTask"; };
- while (true)
+ //update robot + check
{
- const auto startT = std::chrono::high_resolution_clock::now();
- //done
- if (isShuttingDown())
- {
- return;
- }
- const auto freq = checkFrequency.load();
- const bool inEmergencyStop = _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
- if (inEmergencyStop || freq == 0)
- {
- ARMARX_INFO << deactivateSpam() << "Self collision checker: skipping check " << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
- //currently wait
- std::this_thread::sleep_for(std::chrono::microseconds {1000});
- continue;
- }
- //update robot + check
- {
- std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
- //update robot
- _module<ControlThreadDataBuffer>().updateVirtualRobot(selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
+ std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
+ //update robot
+ _module<ControlThreadDataBuffer>().updateVirtualRobot(selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
- //check for all nodes 0
+ //check for all nodes 0
+ {
+ bool allJoints0 = true;
+ for (const VirtualRobot::RobotNodePtr node : selfCollisionAvoidanceRobotNodes)
{
- bool allJoints0 = true;
- for (const VirtualRobot::RobotNodePtr node : selfCollisionAvoidanceRobotNodes)
- {
- if (0 != node->getJointValue())
- {
- allJoints0 = false;
- break;
- }
- }
- if (allJoints0)
+ if (0 != node->getJointValue())
{
- continue;
+ allJoints0 = false;
+ break;
}
}
-
- bool collision = false;
- for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
+ if (allJoints0)
{
- const auto& pair = nodePairsToCheck.at(idx);
- if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
- {
- collision = true;
- lastCollisionPairIndex = idx;
- ARMARX_WARNING << "Self collision checker: COLLISION: '" << pair.first->getName() << "' and '" << pair.second->getName() << "'";
- _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- // since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
- _module<ControlThreadDataBuffer>().setActivateControllersRequest({});
- break;
- }
+ continue;
}
- if (!collision)
+ }
+
+ bool collision = false;
+ for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
+ {
+ const auto& pair = nodePairsToCheck.at(idx);
+ if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
{
- ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: no collision found between the " << nodePairsToCheck.size() << " pairs";
+ collision = true;
+ lastCollisionPairIndex = idx;
+ ARMARX_WARNING << "Self collision checker: COLLISION: '" << pair.first->getName() << "' and '" << pair.second->getName() << "'";
+ _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ // since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
+ _module<ControlThreadDataBuffer>().setActivateControllersRequest({});
+ break;
}
}
- //sleep remaining
- std::this_thread::sleep_until(startT + std::chrono::microseconds {static_cast<int64_t>(1000000 / freq)});
+ if (!collision)
+ {
+ ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: no collision found between the " << nodePairsToCheck.size() << " pairs";
+ }
}
+ //sleep remaining
+ std::this_thread::sleep_until(startT + std::chrono::microseconds {static_cast<int64_t>(1000000 / freq)});
}
+ }
- void SelfCollisionChecker::_preFinishRunning()
+ void SelfCollisionChecker::_preFinishRunning()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_INFO << "Stopping Self Collision Avoidance.";
+ if (selfCollisionAvoidanceThread.joinable())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_INFO << "Stopping Self Collision Avoidance.";
- if (selfCollisionAvoidanceThread.joinable())
- {
- selfCollisionAvoidanceThread.join();
- }
+ selfCollisionAvoidanceThread.join();
}
}
}
+
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
index e2a9d9c3e1022d3b51bc6fb2ed2b6a906f2262da..d1d059ea78d90f41b47f3d2aa3f77e9303e73974 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
@@ -31,132 +31,129 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotUnitModuleBase.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ class SelfCollisionCheckerPropertyDefinitions: public ModuleBasePropertyDefinitions
{
- class SelfCollisionCheckerPropertyDefinitions: public ModuleBasePropertyDefinitions
+ public:
+ SelfCollisionCheckerPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
{
- public:
- SelfCollisionCheckerPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
- {
- defineOptionalProperty<float>(
- "SelfCollisionCheck_Frequency", 10,
- "Frequency [Hz] of self collision checking (default = 10). If set to 0, no cllisions will be checked.",
- PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>(
- "SelfCollisionCheck_MinSelfDistance", 20,
- "Minimum allowed distance (mm) between each collision model before entering emergency stop (default = 20).",
- PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<std::string>(
- "SelfCollisionCheck_ModelGroupsToCheck", "",
- "Comma seperated list of groups (two or more) of collision models (RobotNodeSets or RobotNodes) that "
- "should be checked against each other by collision avoidance \n"
- "(e.g. {rnsColModel1,rnsColModel2};{rnsColModel3,rnsColModel4}; .... "
- "says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 will only be checked against rnsColModel4). \n"
- "Following predefined descriptors can be used: 'FLOOR' which represents a virtual collision model of the floor.");
- }
- };
+ defineOptionalProperty<float>(
+ "SelfCollisionCheck_Frequency", 10,
+ "Frequency [Hz] of self collision checking (default = 10). If set to 0, no cllisions will be checked.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "SelfCollisionCheck_MinSelfDistance", 20,
+ "Minimum allowed distance (mm) between each collision model before entering emergency stop (default = 20).",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<std::string>(
+ "SelfCollisionCheck_ModelGroupsToCheck", "",
+ "Comma seperated list of groups (two or more) of collision models (RobotNodeSets or RobotNodes) that "
+ "should be checked against each other by collision avoidance \n"
+ "(e.g. {rnsColModel1,rnsColModel2};{rnsColModel3,rnsColModel4}; .... "
+ "says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 will only be checked against rnsColModel4). \n"
+ "Following predefined descriptors can be used: 'FLOOR' which represents a virtual collision model of the floor.");
+ }
+ };
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" manages self collision avoidance.
+ * If the distance between a pair of bodies falls below a threshold, the the emergency stop is activated.
+ *
+ * @see ModuleBase
+ */
+ class SelfCollisionChecker : virtual public ModuleBase, virtual public RobotUnitSelfCollisionCheckerInterface
+ {
+ friend class ModuleBase;
+ public:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" manages self collision avoidance.
- * If the distance between a pair of bodies falls below a threshold, the the emergency stop is activated.
- *
- * @see ModuleBase
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
*/
- class SelfCollisionChecker : virtual public ModuleBase, virtual public RobotUnitSelfCollisionCheckerInterface
+ static SelfCollisionChecker& Instance()
{
- friend class ModuleBase;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static SelfCollisionChecker& Instance()
- {
- return ModuleBase::Instance<SelfCollisionChecker>();
- }
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_preOnInitRobotUnit
- void _preOnInitRobotUnit();
- /// @see ModuleBase::_postFinishControlThreadInitialization
- void _postFinishControlThreadInitialization();
- /// @see ModuleBase::_preFinishRunning
- void _preFinishRunning();
- /// @see ModuleBase::_componentPropertiesUpdated
- void _componentPropertiesUpdated(const std::set<std::string>& changed);
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// ice interface //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Sets the minimal distance (mm) between a configured pair of bodies to 'distance'.
- * @param distance The minimal distance (mm) between a pair of bodies.
- */
- void setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Sets the frequency of self collision checks.
- * @param freq The frequency of self collision checks.
- */
- void setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current& = Ice::emptyCurrent) override;
+ return ModuleBase::Instance<SelfCollisionChecker>();
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_preOnInitRobotUnit
+ void _preOnInitRobotUnit();
+ /// @see ModuleBase::_postFinishControlThreadInitialization
+ void _postFinishControlThreadInitialization();
+ /// @see ModuleBase::_preFinishRunning
+ void _preFinishRunning();
+ /// @see ModuleBase::_componentPropertiesUpdated
+ void _componentPropertiesUpdated(const std::set<std::string>& changed);
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Sets the minimal distance (mm) between a configured pair of bodies to 'distance'.
+ * @param distance The minimal distance (mm) between a pair of bodies.
+ */
+ void setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current& = Ice::emptyCurrent) override;
+ /**
+ * @brief Sets the frequency of self collision checks.
+ * @param freq The frequency of self collision checks.
+ */
+ void setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current& = Ice::emptyCurrent) override;
- /**
- * @brief Returns whether the frequency of self collision checks is above 0.
- * @return Whether the frequency of self collision checks is above 0.
- */
- bool isSelfCollisionCheckEnabled(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the frequency of self collision checks.
- * @return The frequency of self collision checks.
- */
- float getSelfCollisionAvoidanceFrequency(const Ice::Current& = Ice::emptyCurrent) const override;
- /**
- * @brief Returns the minimal distance (mm) between a pair of bodies.
- * @return The minimal distance (mm) between a pair of bodies.
- */
- float getSelfCollisionAvoidanceDistance(const Ice::Current& = Ice::emptyCurrent) const override;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////// implementation //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief The Loop executing the self collision check
- void selfCollisionAvoidanceTask();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief The thread executing \ref selfCollisionAvoidanceTask
- std::thread selfCollisionAvoidanceThread;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////////// config //////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief The list of pairs of collision models that should be checked agaisnt each other (names only)
- std::set< std::pair<std::string, std::string>> namePairsToCheck;
- /// @brief The frequency of self collision checks.
- std::atomic<float> checkFrequency {0};
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////// col data /////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- ///@ brief The mutex guarding data used for self collision checks.
- std::mutex selfCollisionDataMutex;
- /// @brief Min allowed distance (mm) between each pair of collision models
- std::atomic<float> minSelfDistance {0};
- /// @brief The list of pairs of collision models that should be checked agaisnt each other (model nodes only)
- std::vector<std::pair<VirtualRobot::SceneObjectPtr, VirtualRobot::SceneObjectPtr>> nodePairsToCheck;
- /// @brief The Robot used for self collison checks
- VirtualRobot::RobotPtr selfCollisionAvoidanceRobot;
- /// @brief The Robot Nodes of the Robot used for collision checks
- std::vector<VirtualRobot::RobotNodePtr> selfCollisionAvoidanceRobotNodes;
- /// @brief A Collision object for the floor
- VirtualRobot::SceneObjectSetPtr floor;
- /// @brief The pair of nodes causing the last detected collision (as index in \ref nodePairsToCheck or -1)
- std::atomic_size_t lastCollisionPairIndex {std::numeric_limits<std::size_t>::max()};
- };
- }
+ /**
+ * @brief Returns whether the frequency of self collision checks is above 0.
+ * @return Whether the frequency of self collision checks is above 0.
+ */
+ bool isSelfCollisionCheckEnabled(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the frequency of self collision checks.
+ * @return The frequency of self collision checks.
+ */
+ float getSelfCollisionAvoidanceFrequency(const Ice::Current& = Ice::emptyCurrent) const override;
+ /**
+ * @brief Returns the minimal distance (mm) between a pair of bodies.
+ * @return The minimal distance (mm) between a pair of bodies.
+ */
+ float getSelfCollisionAvoidanceDistance(const Ice::Current& = Ice::emptyCurrent) const override;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief The Loop executing the self collision check
+ void selfCollisionAvoidanceTask();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief The thread executing \ref selfCollisionAvoidanceTask
+ std::thread selfCollisionAvoidanceThread;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////////// config //////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief The list of pairs of collision models that should be checked agaisnt each other (names only)
+ std::set< std::pair<std::string, std::string>> namePairsToCheck;
+ /// @brief The frequency of self collision checks.
+ std::atomic<float> checkFrequency {0};
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// col data /////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ ///@ brief The mutex guarding data used for self collision checks.
+ std::mutex selfCollisionDataMutex;
+ /// @brief Min allowed distance (mm) between each pair of collision models
+ std::atomic<float> minSelfDistance {0};
+ /// @brief The list of pairs of collision models that should be checked agaisnt each other (model nodes only)
+ std::vector<std::pair<VirtualRobot::SceneObjectPtr, VirtualRobot::SceneObjectPtr>> nodePairsToCheck;
+ /// @brief The Robot used for self collison checks
+ VirtualRobot::RobotPtr selfCollisionAvoidanceRobot;
+ /// @brief The Robot Nodes of the Robot used for collision checks
+ std::vector<VirtualRobot::RobotNodePtr> selfCollisionAvoidanceRobotNodes;
+ /// @brief A Collision object for the floor
+ VirtualRobot::SceneObjectSetPtr floor;
+ /// @brief The pair of nodes causing the last detected collision (as index in \ref nodePairsToCheck or -1)
+ std::atomic_size_t lastCollisionPairIndex {std::numeric_limits<std::size_t>::max()};
+ };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 92d0c1162ddf1c9143102d38815dae5308a3f752..0ff29e89c33667dd2f82fc2eaa1072bf14e1c35c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -36,191 +36,182 @@
#include "../RobotUnit.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ /**
+ * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref RobotUnitEmergencyStopMaster.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ class ControlThreadAttorneyForRobotUnitEmergencyStopMaster
{
- /**
- * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref RobotUnitEmergencyStopMaster.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- class ControlThreadAttorneyForRobotUnitEmergencyStopMaster
- {
- friend class RobotUnitEmergencyStopMaster;
- static void SetEmergencyStopStateNoReportToTopic(ControlThread* controlThreadModule, EmergencyStopState state)
- {
- controlThreadModule->setEmergencyStopStateNoReportToTopic(state);
- }
- };
- }
+ friend class RobotUnitEmergencyStopMaster;
+ static void SetEmergencyStopStateNoReportToTopic(ControlThread* controlThreadModule, EmergencyStopState state)
+ {
+ controlThreadModule->setEmergencyStopStateNoReportToTopic(state);
+ }
+ };
}
-
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ class RobotUnitEmergencyStopMaster :
+ virtual public ManagedIceObject,
+ virtual public EmergencyStopMasterInterface
{
- class RobotUnitEmergencyStopMaster :
- virtual public ManagedIceObject,
- virtual public EmergencyStopMasterInterface
- {
- public:
- RobotUnitEmergencyStopMaster(ControlThread* controlThreadModule, std::string emergencyStopTopicName) :
- controlThreadModule {controlThreadModule},
- emergencyStopTopicName {emergencyStopTopicName}
- {
- ARMARX_CHECK_NOT_NULL(controlThreadModule);
- ARMARX_CHECK_EXPRESSION(!emergencyStopTopicName.empty());
- }
+ public:
+ RobotUnitEmergencyStopMaster(ControlThread* controlThreadModule, std::string emergencyStopTopicName) :
+ controlThreadModule {controlThreadModule},
+ emergencyStopTopicName {emergencyStopTopicName}
+ {
+ ARMARX_CHECK_NOT_NULL(controlThreadModule);
+ ARMARX_CHECK_EXPRESSION(!emergencyStopTopicName.empty());
+ }
- void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = Ice::emptyCurrent) final override
- {
- if (getEmergencyStopState() == state)
- {
- return;
- }
- ControlThreadAttorneyForRobotUnitEmergencyStopMaster::SetEmergencyStopStateNoReportToTopic(controlThreadModule, state);
- emergencyStopTopic->reportEmergencyStopState(state);
- }
- EmergencyStopState getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
+ void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = Ice::emptyCurrent) final override
+ {
+ if (getEmergencyStopState() == state)
{
- return controlThreadModule->getEmergencyStopState();
+ return;
}
+ ControlThreadAttorneyForRobotUnitEmergencyStopMaster::SetEmergencyStopStateNoReportToTopic(controlThreadModule, state);
+ emergencyStopTopic->reportEmergencyStopState(state);
+ }
+ EmergencyStopState getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
+ {
+ return controlThreadModule->getEmergencyStopState();
+ }
- protected:
- void onInitComponent() final override
- {
- armarx::ManagedIceObject::offeringTopic(emergencyStopTopicName);
- }
- void onConnectComponent() final override
- {
- emergencyStopTopic = armarx::ManagedIceObject::getTopic<EmergencyStopListenerPrx>(emergencyStopTopicName);
- setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
- }
+ protected:
+ void onInitComponent() final override
+ {
+ armarx::ManagedIceObject::offeringTopic(emergencyStopTopicName);
+ }
+ void onConnectComponent() final override
+ {
+ emergencyStopTopic = armarx::ManagedIceObject::getTopic<EmergencyStopListenerPrx>(emergencyStopTopicName);
+ setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
+ }
- std::string getDefaultName() const final override
- {
- return "EmergencyStopMaster";
- }
+ std::string getDefaultName() const final override
+ {
+ return "EmergencyStopMaster";
+ }
- protected:
- ControlThread* const controlThreadModule;
- const std::string emergencyStopTopicName;
- EmergencyStopListenerPrx emergencyStopTopic;
- };
- }
+ protected:
+ ControlThread* const controlThreadModule;
+ const std::string emergencyStopTopicName;
+ EmergencyStopListenerPrx emergencyStopTopic;
+ };
}
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ Ice::ObjectProxySeq Units::getUnits(const Ice::Current&) const
{
- Ice::ObjectProxySeq Units::getUnits(const Ice::Current&) const
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::vector<ManagedIceObjectPtr> unitsCopy;
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::vector<ManagedIceObjectPtr> unitsCopy;
- {
- auto guard = getGuard();
- //copy to keep lock retention time low
- unitsCopy = units;
- }
- Ice::ObjectProxySeq r;
- r.reserve(unitsCopy.size());
- for (const ManagedIceObjectPtr& unit : unitsCopy)
- {
- r.emplace_back(unit->getProxy(-1, true));
- }
- return r;
+ auto guard = getGuard();
+ //copy to keep lock retention time low
+ unitsCopy = units;
}
-
- Ice::ObjectPrx Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
+ Ice::ObjectProxySeq r;
+ r.reserve(unitsCopy.size());
+ for (const ManagedIceObjectPtr& unit : unitsCopy)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //no lock required
- ManagedIceObjectPtr unit = getUnit(staticIceId);
- if (unit)
- {
- return unit->getProxy(-1, true);
- }
- return nullptr;
+ r.emplace_back(unit->getProxy(-1, true));
}
+ return r;
+ }
- const EmergencyStopMasterInterfacePtr& Units::getEmergencyStopMaster() const
+ Ice::ObjectPrx Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //no lock required
+ ManagedIceObjectPtr unit = getUnit(staticIceId);
+ if (unit)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return emergencyStopMaster;
+ return unit->getProxy(-1, true);
}
+ return nullptr;
+ }
- void Units::initializeDefaultUnits()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- auto beg = TimeUtil::GetTime(true);
- {
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- ARMARX_INFO << "initializing default units";
- initializeKinematicUnit();
- ARMARX_DEBUG << "KinematicUnit initialized";
- initializePlatformUnit();
- ARMARX_DEBUG << "PlatformUnit initialized";
- initializeForceTorqueUnit();
- ARMARX_DEBUG << "ForceTorqueUnit initialized";
- initializeInertialMeasurementUnit();
- ARMARX_DEBUG << "InertialMeasurementUnit initialized";
- initializeTrajectoryControllerUnit();
- ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
- initializeTcpControllerUnit();
- ARMARX_DEBUG << "TcpControllerUnit initialized";
- }
- ARMARX_INFO << "initializing default units...done! " << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
- }
+ const EmergencyStopMasterInterfacePtr& Units::getEmergencyStopMaster() const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ return emergencyStopMaster;
+ }
- void Units::initializeKinematicUnit()
+ void Units::initializeDefaultUnits()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ auto beg = TimeUtil::GetTime(true);
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- using IfaceT = KinematicUnitInterface;
-
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- //check if unit is already added
- if (getUnit(IfaceT::ice_staticId()))
- {
- return;
- }
- auto unit = createKinematicSubUnit(getIceProperties()->clone());
- //add
- if (unit)
- {
- addUnit(std::move(unit));
- }
+ ARMARX_INFO << "initializing default units";
+ initializeKinematicUnit();
+ ARMARX_DEBUG << "KinematicUnit initialized";
+ initializePlatformUnit();
+ ARMARX_DEBUG << "PlatformUnit initialized";
+ initializeForceTorqueUnit();
+ ARMARX_DEBUG << "ForceTorqueUnit initialized";
+ initializeInertialMeasurementUnit();
+ ARMARX_DEBUG << "InertialMeasurementUnit initialized";
+ initializeTrajectoryControllerUnit();
+ ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
+ initializeTcpControllerUnit();
+ ARMARX_DEBUG << "TcpControllerUnit initialized";
}
+ ARMARX_INFO << "initializing default units...done! " << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
+ }
- ManagedIceObjectPtr Units::createKinematicSubUnit(
- const Ice::PropertiesPtr& properties,
- const std::string& positionControlMode,
- const std::string& velocityControlMode,
- const std::string& torqueControlMode
- )
+ void Units::initializeKinematicUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ using IfaceT = KinematicUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+ auto unit = createKinematicSubUnit(getIceProperties()->clone());
+ //add
+ if (unit)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- /// TODO move init code to Kinematic sub unit
- using UnitT = KinematicSubUnit;
+ addUnit(std::move(unit));
+ }
+ }
- //add ctrl et al
- std::map<std::string, UnitT::ActuatorData> devs;
- for (const ControlDevicePtr& controlDevice : _module<Devices>().getControlDevices().values())
- {
- ARMARX_CHECK_EXPRESSION(controlDevice);
- const auto& controlDeviceName = controlDevice->getDeviceName();
- if (!controlDevice->hasTag(ControlDeviceTags::CreateNoDefaultController))
- {
- UnitT::ActuatorData ad;
- ad.name = controlDeviceName;
- ad.sensorDeviceIndex =
- _module<Devices>().getSensorDevices().has(controlDeviceName) ?
- _module<Devices>().getSensorDevices().index(controlDeviceName) :
- std::numeric_limits<std::size_t>::max();
- /// TODO use better kinematic unit controllers (posThroughVel + ramps)
+ ManagedIceObjectPtr Units::createKinematicSubUnit(
+ const Ice::PropertiesPtr& properties,
+ const std::string& positionControlMode,
+ const std::string& velocityControlMode,
+ const std::string& torqueControlMode
+ )
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ /// TODO move init code to Kinematic sub unit
+ using UnitT = KinematicSubUnit;
+
+ //add ctrl et al
+ std::map<std::string, UnitT::ActuatorData> devs;
+ for (const ControlDevicePtr& controlDevice : _module<Devices>().getControlDevices().values())
+ {
+ ARMARX_CHECK_EXPRESSION(controlDevice);
+ const auto& controlDeviceName = controlDevice->getDeviceName();
+ if (!controlDevice->hasTag(ControlDeviceTags::CreateNoDefaultController))
+ {
+ UnitT::ActuatorData ad;
+ ad.name = controlDeviceName;
+ ad.sensorDeviceIndex =
+ _module<Devices>().getSensorDevices().has(controlDeviceName) ?
+ _module<Devices>().getSensorDevices().index(controlDeviceName) :
+ std::numeric_limits<std::size_t>::max();
+ /// TODO use better kinematic unit controllers (posThroughVel + ramps)
#define initializeKinematicUnit_tmp_helper(mode, CtargT, ctrl) \
{ \
const auto& controlMode = mode; \
@@ -231,392 +222,391 @@ namespace armarx
NJointKinematicUnitPassThroughControllerConfigPtr config = new NJointKinematicUnitPassThroughControllerConfig; \
config->controlMode=controlMode; \
config->deviceName=controlDeviceName; \
- const NJointControllerPtr& nJointCtrl = _module<ControllerManagement>().createNJointController( \
- "NJointKinematicUnitPassThroughController", \
- getName() + "_" + "NJointKU_PTCtrl_"+controlDeviceName+"_"+controlMode, \
- config, \
- false, true); \
+ const NJointControllerBasePtr& nJointCtrl = _module<ControllerManagement>().createNJointController( \
+ "NJointKinematicUnitPassThroughController", \
+ getName() + "_" + "NJointKU_PTCtrl_"+controlDeviceName+"_"+controlMode, \
+ config, \
+ false, true); \
pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl); \
ARMARX_CHECK_EXPRESSION(pt); \
} \
}
- initializeKinematicUnit_tmp_helper(positionControlMode, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
- initializeKinematicUnit_tmp_helper(velocityControlMode, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
- initializeKinematicUnit_tmp_helper(torqueControlMode, ControlTarget1DoFActuatorTorque, ad.ctrlTor)
+ initializeKinematicUnit_tmp_helper(positionControlMode, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
+ initializeKinematicUnit_tmp_helper(velocityControlMode, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
+ initializeKinematicUnit_tmp_helper(torqueControlMode, ControlTarget1DoFActuatorTorque, ad.ctrlTor)
#undef initializeKinematicUnit_tmp_helper
- if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
- {
- devs[controlDeviceName] = std::move(ad);
- }
+ if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
+ {
+ devs[controlDeviceName] = std::move(ad);
}
}
- if (devs.empty())
- {
- ARMARX_IMPORTANT << "No joint devices found - skipping adding of KinematicUnit";
- return nullptr;
- }
- ARMARX_CHECK_EXPRESSION_W_HINT(!devs.empty(), "no 1DoF ControlDevice found with matching SensorDevice");
- //add it
- const std::string configName = getProperty<std::string>("KinematicUnitName");
- const std::string confPre = getConfigDomain() + "." + configName + ".";
- //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
- //fill properties
- properties->setProperty(confPre + "RobotNodeSetName", _module<RobotData>().getRobotNodetSeName());
- properties->setProperty(confPre + "RobotFileName", _module<RobotData>().getRobotFileName());
- properties->setProperty(confPre + "RobotFileNameProject", _module<RobotData>().getRobotProjectName());
- properties->setProperty(confPre + "TopicPrefix", getProperty<std::string>("KinematicUnitNameTopicPrefix"));
-
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
-
- //fill devices (sensor + controller)
- unit->setupData(
- getProperty<std::string>("RobotFileName").getValue(),
- _module<RobotData>().cloneRobot(),
- std::move(devs),
- _module<Devices>().getControlModeGroups().groups,
- _module<Devices>().getControlModeGroups().groupsMerged,
- dynamic_cast<RobotUnit*>(this)
- );
- return unit;
- }
-
- void Units::initializePlatformUnit()
- {
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- using UnitT = PlatformSubUnit;
- using IfaceT = PlatformUnitInterface;
-
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- //check if unit is already added
- if (getUnit(IfaceT::ice_staticId()))
- {
- return;
- }
- //is there a platform dev?
- if (_module<RobotData>().getRobotPlatformName().empty())
- {
- ARMARX_INFO << "no platform unit created since no platform name was given";
- return;
- }
- if (!_module<Devices>().getControlDevices().has(_module<RobotData>().getRobotPlatformName()))
- {
- ARMARX_WARNING << "no platform unit created since the platform control device with name '"
- << _module<RobotData>().getRobotPlatformName() << "' was not found";
- return;
- }
- const ControlDevicePtr& controlDevice = _module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
- const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
- JointController* jointVel = controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
- ARMARX_CHECK_EXPRESSION(jointVel);
- ARMARX_CHECK_EXPRESSION(sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
- //add it
- const std::string configName = getProperty<std::string>("PlatformUnitName");
- const std::string confPre = getConfigDomain() + "." + configName + ".";
- Ice::PropertiesPtr properties = getIceProperties()->clone();
- //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
- //fill properties
- properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
- //config
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config = new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
- config->initialVelocityX = 0;
- config->initialVelocityY = 0;
- config->initialVelocityRotation = 0;
- config->platformName = _module<RobotData>().getRobotPlatformName();
- auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformUnitVelocityPassThroughController",
- getName() + "_" + configName + "_VelPTContoller",
- config, false, true
- )
- );
- ARMARX_CHECK_EXPRESSION(ctrl);
- unit->pt = ctrl;
-
- NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg = new NJointHolonomicPlatformRelativePositionControllerConfig;
- configRelativePositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
- auto ctrlRelativePosition = NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformRelativePositionController",
- getName() + "_" + configName + "_RelativePositionContoller",
- configRelativePositionCtrlCfg, false, true
- )
- );
- ARMARX_CHECK_EXPRESSION(ctrlRelativePosition);
- unit->pt = ctrl;
- unit->relativePosCtrl = ctrlRelativePosition;
-
- unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
-
-
- NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg = new NJointHolonomicPlatformGlobalPositionControllerConfig;
- configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
- auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformGlobalPositionController",
- getName() + "_" + configName + "_GlobalPositionContoller",
- configGlobalPositionCtrlCfg, false, true
- )
- );
- ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
- unit->pt = ctrl;
- unit->globalPosCtrl = ctrlGlobalPosition;
-
- unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
- //add
- addUnit(std::move(unit));
}
-
- void Units::initializeForceTorqueUnit()
+ if (devs.empty())
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- using UnitT = ForceTorqueSubUnit;
- using IfaceT = ForceTorqueUnitInterface;
-
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- //check if unit is already added
- if (getUnit(IfaceT::ice_staticId()))
- {
- return;
- }
- //check if it is required
- std::vector<ForceTorqueSubUnit::DeviceData> ftdevs;
- for (const SensorDevicePtr& ftSensorDevice : _module<Devices>().getSensorDevices().values())
- {
- if (ftSensorDevice->getSensorValue()->isA<SensorValueForceTorque>())
- {
- ForceTorqueSubUnit::DeviceData ftSensorData;
- ftSensorData.sensorIndex = _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName());
- ftSensorData.deviceName = ftSensorDevice->getDeviceName();
- ftSensorData.frame = ftSensorDevice->getReportingFrame();
- ARMARX_CHECK_EXPRESSION_W_HINT(
- !ftSensorData.frame.empty(),
- "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex
- << ") reports force torque values but returns an empty string as reporting frame"
- );
- ARMARX_CHECK_EXPRESSION_W_HINT(
- unitCreateRobot->hasRobotNode(ftSensorData.frame),
- "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex
- << ") reports force torque values but returns a reporting frame '" << ftSensorData.frame
- << "' which is not present in the robot '" << _module<RobotData>().getRobotName() << "'"
- );
- ftdevs.emplace_back(std::move(ftSensorData));
- }
- }
- if (ftdevs.empty())
- {
- ARMARX_IMPORTANT << "no force torque unit created since there are no force torque sensor devices";
- return;
- }
- //add it
- const std::string configName = getProperty<std::string>("ForceTorqueUnitName");
- const std::string confPre = getConfigDomain() + "." + configName + ".";
- Ice::PropertiesPtr properties = getIceProperties()->clone();
- //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
- //fill properties
- properties->setProperty(confPre + "AgentName", _module<RobotData>().getRobotName());
- properties->setProperty(confPre + "ForceTorqueTopicName", getProperty<std::string>("ForceTorqueTopicName").getValue());
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
- unit->devs = ftdevs;
- //add
- addUnit(std::move(unit));
+ ARMARX_IMPORTANT << "No joint devices found - skipping adding of KinematicUnit";
+ return nullptr;
}
+ ARMARX_CHECK_EXPRESSION_W_HINT(!devs.empty(), "no 1DoF ControlDevice found with matching SensorDevice");
+ //add it
+ const std::string configName = getProperty<std::string>("KinematicUnitName");
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ properties->setProperty(confPre + "RobotNodeSetName", _module<RobotData>().getRobotNodetSeName());
+ properties->setProperty(confPre + "RobotFileName", _module<RobotData>().getRobotFileName());
+ properties->setProperty(confPre + "RobotFileNameProject", _module<RobotData>().getRobotProjectName());
+ properties->setProperty(confPre + "TopicPrefix", getProperty<std::string>("KinematicUnitNameTopicPrefix"));
+
+ ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+
+ //fill devices (sensor + controller)
+ unit->setupData(
+ getProperty<std::string>("RobotFileName").getValue(),
+ _module<RobotData>().cloneRobot(),
+ std::move(devs),
+ _module<Devices>().getControlModeGroups().groups,
+ _module<Devices>().getControlModeGroups().groupsMerged,
+ dynamic_cast<RobotUnit*>(this)
+ );
+ return unit;
+ }
- void Units::initializeInertialMeasurementUnit()
+ void Units::initializePlatformUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ using UnitT = PlatformSubUnit;
+ using IfaceT = PlatformUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- using UnitT = InertialMeasurementSubUnit;
- using IfaceT = InertialMeasurementUnitInterface;
+ return;
+ }
+ //is there a platform dev?
+ if (_module<RobotData>().getRobotPlatformName().empty())
+ {
+ ARMARX_INFO << "no platform unit created since no platform name was given";
+ return;
+ }
+ if (!_module<Devices>().getControlDevices().has(_module<RobotData>().getRobotPlatformName()))
+ {
+ ARMARX_WARNING << "no platform unit created since the platform control device with name '"
+ << _module<RobotData>().getRobotPlatformName() << "' was not found";
+ return;
+ }
+ const ControlDevicePtr& controlDevice = _module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
+ const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
+ JointController* jointVel = controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
+ ARMARX_CHECK_EXPRESSION(jointVel);
+ ARMARX_CHECK_EXPRESSION(sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
+ //add it
+ const std::string configName = getProperty<std::string>("PlatformUnitName");
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
+ ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ //config
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config = new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
+ config->initialVelocityX = 0;
+ config->initialVelocityY = 0;
+ config->initialVelocityRotation = 0;
+ config->platformName = _module<RobotData>().getRobotPlatformName();
+ auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformUnitVelocityPassThroughController",
+ getName() + "_" + configName + "_VelPTContoller",
+ config, false, true
+ )
+ );
+ ARMARX_CHECK_EXPRESSION(ctrl);
+ unit->pt = ctrl;
+
+ NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg = new NJointHolonomicPlatformRelativePositionControllerConfig;
+ configRelativePositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
+ auto ctrlRelativePosition = NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformRelativePositionController",
+ getName() + "_" + configName + "_RelativePositionContoller",
+ configRelativePositionCtrlCfg, false, true
+ )
+ );
+ ARMARX_CHECK_EXPRESSION(ctrlRelativePosition);
+ unit->pt = ctrl;
+ unit->relativePosCtrl = ctrlRelativePosition;
+
+ unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
+
+
+ NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg = new NJointHolonomicPlatformGlobalPositionControllerConfig;
+ configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
+ auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformGlobalPositionController",
+ getName() + "_" + configName + "_GlobalPositionContoller",
+ configGlobalPositionCtrlCfg, false, true
+ )
+ );
+ ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
+ unit->pt = ctrl;
+ unit->globalPosCtrl = ctrlGlobalPosition;
+
+ unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
+ //add
+ addUnit(std::move(unit));
+ }
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- //check if unit is already added
- if (getUnit(IfaceT::ice_staticId()))
- {
- return;
- }
- //check if it is required
- std::map<std::string, std::size_t> imudevs;
- for (auto i : getIndices(_module<Devices>().getSensorDevices().keys()))
- {
- const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(i);
- if (sensorDevice->getSensorValue()->isA<SensorValueIMU>())
- {
- imudevs[sensorDevice->getDeviceName()] = i;
- }
- }
- if (imudevs.empty())
- {
- ARMARX_IMPORTANT << "no inertial measurement unit created since there are no imu sensor devices";
- return;
+ void Units::initializeForceTorqueUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ using UnitT = ForceTorqueSubUnit;
+ using IfaceT = ForceTorqueUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+ //check if it is required
+ std::vector<ForceTorqueSubUnit::DeviceData> ftdevs;
+ for (const SensorDevicePtr& ftSensorDevice : _module<Devices>().getSensorDevices().values())
+ {
+ if (ftSensorDevice->getSensorValue()->isA<SensorValueForceTorque>())
+ {
+ ForceTorqueSubUnit::DeviceData ftSensorData;
+ ftSensorData.sensorIndex = _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName());
+ ftSensorData.deviceName = ftSensorDevice->getDeviceName();
+ ftSensorData.frame = ftSensorDevice->getReportingFrame();
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ !ftSensorData.frame.empty(),
+ "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex
+ << ") reports force torque values but returns an empty string as reporting frame"
+ );
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ unitCreateRobot->hasRobotNode(ftSensorData.frame),
+ "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex
+ << ") reports force torque values but returns a reporting frame '" << ftSensorData.frame
+ << "' which is not present in the robot '" << _module<RobotData>().getRobotName() << "'"
+ );
+ ftdevs.emplace_back(std::move(ftSensorData));
}
- //add it
- const std::string configName = getProperty<std::string>("InertialMeasurementUnitName");
- const std::string confPre = getConfigDomain() + "." + configName + ".";
- Ice::PropertiesPtr properties = getIceProperties()->clone();
- //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
- //fill properties
- properties->setProperty(confPre + "IMUTopicName", getProperty<std::string>("IMUTopicName").getValue());
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
- unit->devs = imudevs;
- //add
- addUnit(std::move(unit));
}
+ if (ftdevs.empty())
+ {
+ ARMARX_IMPORTANT << "no force torque unit created since there are no force torque sensor devices";
+ return;
+ }
+ //add it
+ const std::string configName = getProperty<std::string>("ForceTorqueUnitName");
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ properties->setProperty(confPre + "AgentName", _module<RobotData>().getRobotName());
+ properties->setProperty(confPre + "ForceTorqueTopicName", getProperty<std::string>("ForceTorqueTopicName").getValue());
+ ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ unit->devs = ftdevs;
+ //add
+ addUnit(std::move(unit));
+ }
- void Units::initializeTrajectoryControllerUnit()
+ void Units::initializeInertialMeasurementUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ using UnitT = InertialMeasurementSubUnit;
+ using IfaceT = InertialMeasurementUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- using UnitT = TrajectoryControllerSubUnit;
- if (!getProperty<bool>("CreateTrajectoryPlayer").getValue())
+ return;
+ }
+ //check if it is required
+ std::map<std::string, std::size_t> imudevs;
+ for (auto i : getIndices(_module<Devices>().getSensorDevices().keys()))
+ {
+ const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(i);
+ if (sensorDevice->getSensorValue()->isA<SensorValueIMU>())
{
- return;
+ imudevs[sensorDevice->getDeviceName()] = i;
}
+ }
+ if (imudevs.empty())
+ {
+ ARMARX_IMPORTANT << "no inertial measurement unit created since there are no imu sensor devices";
+ return;
+ }
+ //add it
+ const std::string configName = getProperty<std::string>("InertialMeasurementUnitName");
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ properties->setProperty(confPre + "IMUTopicName", getProperty<std::string>("IMUTopicName").getValue());
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ unit->devs = imudevs;
+ //add
+ addUnit(std::move(unit));
+ }
- //check if unit is already added
- if (getUnit(UnitT::ice_staticId()) || !trajectoryControllerSubUnit)
- {
- return;
- }
- (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))->setup(_modulePtr<RobotUnit>());
- addUnit(trajectoryControllerSubUnit);
- trajectoryControllerSubUnit = nullptr;
+ void Units::initializeTrajectoryControllerUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ using UnitT = TrajectoryControllerSubUnit;
+ if (!getProperty<bool>("CreateTrajectoryPlayer").getValue())
+ {
+ return;
}
- void Units::initializeTcpControllerUnit()
+ //check if unit is already added
+ if (getUnit(UnitT::ice_staticId()) || !trajectoryControllerSubUnit)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ return;
+ }
+ (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))->setup(_modulePtr<RobotUnit>());
+ addUnit(trajectoryControllerSubUnit);
+ trajectoryControllerSubUnit = nullptr;
+ }
- if (!getProperty<bool>("CreateTCPControlUnit").getValue())
- {
- return;
- }
- using UnitT = TCPControllerSubUnit;
+ void Units::initializeTcpControllerUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- //check if unit is already added
- if (getUnit(UnitT::ice_staticId()))
- {
- return;
- }
- (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))->setup(_modulePtr<RobotUnit>(), _module<RobotData>().cloneRobot());
- addUnit(tcpControllerSubUnit);
- tcpControllerSubUnit = nullptr;
+ if (!getProperty<bool>("CreateTCPControlUnit").getValue())
+ {
+ return;
}
+ using UnitT = TCPControllerSubUnit;
- void Units::addUnit(ManagedIceObjectPtr unit)
+ //check if unit is already added
+ if (getUnit(UnitT::ice_staticId()))
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_NOT_NULL(unit);
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- getArmarXManager()->addObjectAsync(unit, "", true, false);
- //maybe add it to the sub units
- RobotUnitSubUnitPtr rsu = RobotUnitSubUnitPtr::dynamicCast(unit);
- if (rsu)
- {
- subUnits.emplace_back(std::move(rsu));
- }
- units.emplace_back(std::move(unit));
+ return;
}
+ (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))->setup(_modulePtr<RobotUnit>(), _module<RobotData>().cloneRobot());
+ addUnit(tcpControllerSubUnit);
+ tcpControllerSubUnit = nullptr;
+ }
- void Units::_icePropertiesInitialized()
+ void Units::addUnit(ManagedIceObjectPtr unit)
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_CHECK_NOT_NULL(unit);
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ getArmarXManager()->addObjectAsync(unit, "", true, false);
+ //maybe add it to the sub units
+ RobotUnitSubUnitPtr rsu = RobotUnitSubUnitPtr::dynamicCast(unit);
+ if (rsu)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- Ice::PropertiesPtr properties = getIceProperties()->clone();
- const std::string& configDomain = "ArmarX";
- // create TCPControlSubUnit
-
- {
- const std::string configNameTCPControlUnit = getProperty<std::string>("TCPControlUnitName");
- tcpControllerSubUnit = Component::create<TCPControllerSubUnit, TCPControllerSubUnitPtr>(properties, configNameTCPControlUnit, configDomain);
- addPropertyUser(PropertyUserPtr::dynamicCast(tcpControllerSubUnit));
- }
+ subUnits.emplace_back(std::move(rsu));
+ }
+ units.emplace_back(std::move(unit));
+ }
- // create TrajectoryControllerSubUnit
+ void Units::_icePropertiesInitialized()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ const std::string& configDomain = "ArmarX";
+ // create TCPControlSubUnit
- {
- const std::string configNameTrajectoryControllerUnit = getProperty<std::string>("TrajectoryControllerUnitName");
- const std::string confPre = configDomain + "." + configNameTrajectoryControllerUnit + ".";
- properties->setProperty(confPre + "KinematicUnitName", getProperty<std::string>("KinematicUnitName").getValue());
- trajectoryControllerSubUnit = Component::create<TrajectoryControllerSubUnit, TrajectoryControllerSubUnitPtr>(properties, configNameTrajectoryControllerUnit, configDomain);
- addPropertyUser(PropertyUserPtr::dynamicCast(trajectoryControllerSubUnit));
- }
+ {
+ const std::string configNameTCPControlUnit = getProperty<std::string>("TCPControlUnitName");
+ tcpControllerSubUnit = Component::create<TCPControllerSubUnit, TCPControllerSubUnitPtr>(properties, configNameTCPControlUnit, configDomain);
+ addPropertyUser(PropertyUserPtr::dynamicCast(tcpControllerSubUnit));
}
- void Units::_preFinishRunning()
+ // create TrajectoryControllerSubUnit
+
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //remove all units
- subUnits.clear();
- for (ManagedIceObjectPtr& unit : units)
- {
- getArmarXManager()->removeObjectBlocking(unit->getName());
- }
- units.clear();
+ const std::string configNameTrajectoryControllerUnit = getProperty<std::string>("TrajectoryControllerUnitName");
+ const std::string confPre = configDomain + "." + configNameTrajectoryControllerUnit + ".";
+ properties->setProperty(confPre + "KinematicUnitName", getProperty<std::string>("KinematicUnitName").getValue());
+ trajectoryControllerSubUnit = Component::create<TrajectoryControllerSubUnit, TrajectoryControllerSubUnitPtr>(properties, configNameTrajectoryControllerUnit, configDomain);
+ addPropertyUser(PropertyUserPtr::dynamicCast(trajectoryControllerSubUnit));
}
+ }
- void Units::_preOnInitRobotUnit()
+ void Units::_preFinishRunning()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //remove all units
+ subUnits.clear();
+ for (ManagedIceObjectPtr& unit : units)
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- unitCreateRobot = _module<RobotData>().cloneRobot();
- ARMARX_DEBUG << "add emergency stop master";
- {
- emergencyStopMaster = new RobotUnitEmergencyStopMaster {&_module<ControlThread>(), getProperty<std::string>("EmergencyStopTopic").getValue()};
- ManagedIceObjectPtr obj = ManagedIceObjectPtr::dynamicCast(emergencyStopMaster);
- ARMARX_CHECK_EXPRESSION_W_HINT(obj, "RobotUnitEmergencyStopMaster");
- getArmarXManager()->addObject(obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true);
- auto prx = obj->getProxy(-1);
- try
- {
- getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->addObject(prx);
- }
- catch (const IceGrid::ObjectExistsException&)
- {
- getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->updateObject(prx);
- }
- catch (std::exception& e)
- {
- ARMARX_WARNING << "add the robot unit as emergency stop master to the ice grid admin!\nThere was an exception:\n" << e.what();
- }
- }
- ARMARX_DEBUG << "add emergency stop master...done!";
+ getArmarXManager()->removeObjectBlocking(unit->getName());
}
+ units.clear();
+ }
- void Units::_postOnExitRobotUnit()
+ void Units::_preOnInitRobotUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ unitCreateRobot = _module<RobotData>().cloneRobot();
+ ARMARX_DEBUG << "add emergency stop master";
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "remove EmergencyStopMaster";
+ emergencyStopMaster = new RobotUnitEmergencyStopMaster {&_module<ControlThread>(), getProperty<std::string>("EmergencyStopTopic").getValue()};
+ ManagedIceObjectPtr obj = ManagedIceObjectPtr::dynamicCast(emergencyStopMaster);
+ ARMARX_CHECK_EXPRESSION_W_HINT(obj, "RobotUnitEmergencyStopMaster");
+ getArmarXManager()->addObject(obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true);
+ auto prx = obj->getProxy(-1);
try
{
- getArmarXManager()->removeObjectBlocking(ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
- getArmarXManager()->getIceManager()->removeObject(getProperty<std::string>("EmergencyStopMasterName").getValue());
+ getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->addObject(prx);
+ }
+ catch (const IceGrid::ObjectExistsException&)
+ {
+ getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->updateObject(prx);
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_WARNING << "add the robot unit as emergency stop master to the ice grid admin!\nThere was an exception:\n" << e.what();
}
- catch (...) {}
- ARMARX_DEBUG << "remove EmergencyStopMaster...done!";
}
+ ARMARX_DEBUG << "add emergency stop master...done!";
+ }
- const ManagedIceObjectPtr& Units::getUnit(const std::string& staticIceId) const
+ void Units::_postOnExitRobotUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ ARMARX_DEBUG << "remove EmergencyStopMaster";
+ try
{
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- auto guard = getGuard();
- for (const ManagedIceObjectPtr& unit : units)
+ getArmarXManager()->removeObjectBlocking(ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
+ getArmarXManager()->getIceManager()->removeObject(getProperty<std::string>("EmergencyStopMasterName").getValue());
+ }
+ catch (...) {}
+ ARMARX_DEBUG << "remove EmergencyStopMaster...done!";
+ }
+
+ const ManagedIceObjectPtr& Units::getUnit(const std::string& staticIceId) const
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ auto guard = getGuard();
+ for (const ManagedIceObjectPtr& unit : units)
+ {
+ if (unit->ice_isA(staticIceId))
{
- if (unit->ice_isA(staticIceId))
- {
- return unit;
- }
+ return unit;
}
- return ManagedIceObject::NullPtr;
}
+ return ManagedIceObject::NullPtr;
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index 54d6c2dcc75ad6b33f9f1a865676da86d2d9833c..c099939eb3ed59f09d14e75b44adff91fbde0b71 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -27,276 +27,273 @@
#include "RobotUnitModuleBase.h"
#include "../Units/RobotUnitSubUnit.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
+ class UnitsPropertyDefinitions: public ModuleBasePropertyDefinitions
{
- class UnitsPropertyDefinitions: public ModuleBasePropertyDefinitions
+ public:
+ UnitsPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
{
- public:
- UnitsPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>(
- "KinematicUnitName", "KinematicUnit",
- "The name of the created kinematic unit");
- defineOptionalProperty<std::string>(
- "KinematicUnitNameTopicPrefix", "",
- "Prefix for the kinematic sensor values topic");
+ defineOptionalProperty<std::string>(
+ "KinematicUnitName", "KinematicUnit",
+ "The name of the created kinematic unit");
+ defineOptionalProperty<std::string>(
+ "KinematicUnitNameTopicPrefix", "",
+ "Prefix for the kinematic sensor values topic");
- defineOptionalProperty<std::string>(
- "PlatformUnitName", "PlatformUnit",
- "The name of the created platform unit");
+ defineOptionalProperty<std::string>(
+ "PlatformUnitName", "PlatformUnit",
+ "The name of the created platform unit");
- defineOptionalProperty<std::string>(
- "ForceTorqueUnitName", "ForceTorqueUnit",
- "The name of the created force troque unit (empty = default)");
- defineOptionalProperty<std::string>(
- "ForceTorqueTopicName", "ForceTorqueValues",
- "Name of the topic on which the force torque sensor values are provided");
+ defineOptionalProperty<std::string>(
+ "ForceTorqueUnitName", "ForceTorqueUnit",
+ "The name of the created force troque unit (empty = default)");
+ defineOptionalProperty<std::string>(
+ "ForceTorqueTopicName", "ForceTorqueValues",
+ "Name of the topic on which the force torque sensor values are provided");
- defineOptionalProperty<std::string>(
- "InertialMeasurementUnitName", "InertialMeasurementUnit",
- "The name of the created inertial measurement unit (empty = default)");
- defineOptionalProperty<std::string>(
- "IMUTopicName", "IMUValues",
- "Name of the IMU Topic.");
+ defineOptionalProperty<std::string>(
+ "InertialMeasurementUnitName", "InertialMeasurementUnit",
+ "The name of the created inertial measurement unit (empty = default)");
+ defineOptionalProperty<std::string>(
+ "IMUTopicName", "IMUValues",
+ "Name of the IMU Topic.");
- defineOptionalProperty<bool>(
- "CreateTCPControlUnit", false,
- "If true the TCPControl SubUnit is created and added");
- defineOptionalProperty<std::string>(
- "TCPControlUnitName", "TCPControlUnit",
- "Name of the TCPControlUnit.");
+ defineOptionalProperty<bool>(
+ "CreateTCPControlUnit", false,
+ "If true the TCPControl SubUnit is created and added");
+ defineOptionalProperty<std::string>(
+ "TCPControlUnitName", "TCPControlUnit",
+ "Name of the TCPControlUnit.");
- defineOptionalProperty<bool>(
- "CreateTrajectoryPlayer", true,
- "If true the TrajectoryPlayer SubUnit is created and added");
- defineOptionalProperty<std::string>(
- "TrajectoryControllerUnitName", "TrajectoryPlayer",
- "Name of the TrajectoryControllerUnit. The respective component outside of the RobotUnit is TrajectoryPlayer");
+ defineOptionalProperty<bool>(
+ "CreateTrajectoryPlayer", true,
+ "If true the TrajectoryPlayer SubUnit is created and added");
+ defineOptionalProperty<std::string>(
+ "TrajectoryControllerUnitName", "TrajectoryPlayer",
+ "Name of the TrajectoryControllerUnit. The respective component outside of the RobotUnit is TrajectoryPlayer");
- defineOptionalProperty<std::string>(
- "EmergencyStopMasterName", "EmergencyStopMaster",
- "The name used to register as an EmergencyStopMaster");
- defineOptionalProperty<std::string>(
- "EmergencyStopTopic", "EmergencyStop",
- "The name of the topic over which changes of the emergencyStopState are sent.");
- }
- };
+ defineOptionalProperty<std::string>(
+ "EmergencyStopMasterName", "EmergencyStopMaster",
+ "The name used to register as an EmergencyStopMaster");
+ defineOptionalProperty<std::string>(
+ "EmergencyStopTopic", "EmergencyStop",
+ "The name of the topic over which changes of the emergencyStopState are sent.");
+ }
+ };
+ /**
+ * @ingroup Library-RobotUnit-Modules
+ * @brief This \ref ModuleBase "Module" manages all Units of a \ref RobotUnit
+ *
+ * These are the managed Units:
+ * - \ref getKinematicUnit "KinematicUnit"
+ * - \ref getForceTorqueUnit "ForceTorqueUnit"
+ * - \ref getInertialMeasurementUnit "InertialMeasurementUnit"
+ * - \ref getPlatformUnit "PlatformUnit"
+ * - \ref getTCPControlUnit "TCPControlUnit"
+ * - \ref getTrajectoryPlayer "TrajectoryPlayer"
+ *
+ * @see ModuleBase
+ */
+ class Units : virtual public ModuleBase, virtual public RobotUnitUnitInterface
+ {
+ friend class ModuleBase;
+ public:
/**
- * @ingroup Library-RobotUnit-Modules
- * @brief This \ref ModuleBase "Module" manages all Units of a \ref RobotUnit
- *
- * These are the managed Units:
- * - \ref getKinematicUnit "KinematicUnit"
- * - \ref getForceTorqueUnit "ForceTorqueUnit"
- * - \ref getInertialMeasurementUnit "InertialMeasurementUnit"
- * - \ref getPlatformUnit "PlatformUnit"
- * - \ref getTCPControlUnit "TCPControlUnit"
- * - \ref getTrajectoryPlayer "TrajectoryPlayer"
- *
- * @see ModuleBase
+ * @brief Returns the singleton instance of this class
+ * @return The singleton instance of this class
*/
- class Units : virtual public ModuleBase, virtual public RobotUnitUnitInterface
- {
- friend class ModuleBase;
- public:
- /**
- * @brief Returns the singleton instance of this class
- * @return The singleton instance of this class
- */
- static Units& Instance();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @see ModuleBase::_preOnInitRobotUnit
- void _preOnInitRobotUnit();
- /// @see ModuleBase::_icePropertiesInitialized
- void _icePropertiesInitialized();
- /// @see ModuleBase::_preFinishRunning
- void _preFinishRunning();
- /// @see ModuleBase::_postOnExitRobotUnit
- void _postOnExitRobotUnit();
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////// ice interface //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- /**
- * @brief Returns proxies to all units
- * @return Proxies to all units
- */
- Ice::ObjectProxySeq getUnits(const Ice::Current&) const override;
- /**
- * @brief Returns a proxy to the Unit with the given ice id (or null if there is none)
- * @param staticIceId The Unit's ice id
- * @return A proxy to the Unit with the given ice id (or null if there is none)
- */
- Ice::ObjectPrx getUnit(const std::string& staticIceId, const Ice::Current&) const override;
+ static Units& Instance();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @see ModuleBase::_preOnInitRobotUnit
+ void _preOnInitRobotUnit();
+ /// @see ModuleBase::_icePropertiesInitialized
+ void _icePropertiesInitialized();
+ /// @see ModuleBase::_preFinishRunning
+ void _preFinishRunning();
+ /// @see ModuleBase::_postOnExitRobotUnit
+ void _postOnExitRobotUnit();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns proxies to all units
+ * @return Proxies to all units
+ */
+ Ice::ObjectProxySeq getUnits(const Ice::Current&) const override;
+ /**
+ * @brief Returns a proxy to the Unit with the given ice id (or null if there is none)
+ * @param staticIceId The Unit's ice id
+ * @return A proxy to the Unit with the given ice id (or null if there is none)
+ */
+ Ice::ObjectPrx getUnit(const std::string& staticIceId, const Ice::Current&) const override;
- /**
- * @brief Returns a proxy to the KinematicUnit
- * @return A proxy to the KinematicUnit
- */
- KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current&) const override;
- /**
- * @brief Returns a proxy to the ForceTorqueUnit
- * @return A proxy to the ForceTorqueUnit
- */
- ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current&) const override;
- /**
- * @brief Returns a proxy to the InertialMeasurementUnit
- * @return A proxy to the InertialMeasurementUnit
- */
- InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current&) const override;
- /**
- * @brief Returns a proxy to the PlatformUnit
- * @return A proxy to the PlatformUnit
- */
- PlatformUnitInterfacePrx getPlatformUnit(const Ice::Current&) const override;
- /**
- * @brief Returns a proxy to the TCPControlUnit
- * @return A proxy to the TCPControlUnit
- */
- TCPControlUnitInterfacePrx getTCPControlUnit(const Ice::Current&) const override;
- /**
- * @brief Returns a proxy to the TrajectoryPlayer
- * @return A proxy to the TrajectoryPlayer
- */
- TrajectoryPlayerInterfacePrx getTrajectoryPlayer(const Ice::Current&) const override;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////// Module interface /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- public:
- //general getters
- /**
- * @brief Returns a pointer to the Unit with the given ice id (or null if there is none)
- * @param staticIceId The Unit's ice id
- * @return A pointer to the Unit with the given ice id (or null if there is none)
- */
- const ManagedIceObjectPtr& getUnit(const std::string& staticIceId) const;
+ /**
+ * @brief Returns a proxy to the KinematicUnit
+ * @return A proxy to the KinematicUnit
+ */
+ KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current&) const override;
+ /**
+ * @brief Returns a proxy to the ForceTorqueUnit
+ * @return A proxy to the ForceTorqueUnit
+ */
+ ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current&) const override;
+ /**
+ * @brief Returns a proxy to the InertialMeasurementUnit
+ * @return A proxy to the InertialMeasurementUnit
+ */
+ InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current&) const override;
+ /**
+ * @brief Returns a proxy to the PlatformUnit
+ * @return A proxy to the PlatformUnit
+ */
+ PlatformUnitInterfacePrx getPlatformUnit(const Ice::Current&) const override;
+ /**
+ * @brief Returns a proxy to the TCPControlUnit
+ * @return A proxy to the TCPControlUnit
+ */
+ TCPControlUnitInterfacePrx getTCPControlUnit(const Ice::Current&) const override;
+ /**
+ * @brief Returns a proxy to the TrajectoryPlayer
+ * @return A proxy to the TrajectoryPlayer
+ */
+ TrajectoryPlayerInterfacePrx getTrajectoryPlayer(const Ice::Current&) const override;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// Module interface /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ //general getters
+ /**
+ * @brief Returns a pointer to the Unit with the given ice id (or null if there is none)
+ * @param staticIceId The Unit's ice id
+ * @return A pointer to the Unit with the given ice id (or null if there is none)
+ */
+ const ManagedIceObjectPtr& getUnit(const std::string& staticIceId) const;
- /**
- * @brief Returns a pointer to the Unit for the given type (or null if there is none)
- * @return A pointer to the Unit for the given type (or null if there is none)
- */
- template<class T> typename T::PointerType getUnit() const;
- /**
- * @brief Returns a proxy to the Unit for the given type (or null if there is none)
- * @return A proxy to the Unit for the given type (or null if there is none)
- */
- template<class T> typename T::ProxyType getUnitPrx() const;
+ /**
+ * @brief Returns a pointer to the Unit for the given type (or null if there is none)
+ * @return A pointer to the Unit for the given type (or null if there is none)
+ */
+ template<class T> typename T::PointerType getUnit() const;
+ /**
+ * @brief Returns a proxy to the Unit for the given type (or null if there is none)
+ * @return A proxy to the Unit for the given type (or null if there is none)
+ */
+ template<class T> typename T::ProxyType getUnitPrx() const;
- //specific getters
- /**
- * @brief Returns a pointer to the KinematicUnit
- * @return A pointer to the KinematicUnit
- */
- KinematicUnitInterfacePtr getKinematicUnit() const;
- /**
- * @brief Returns a pointer to the ForceTorqueUnit
- * @return A pointer to the ForceTorqueUnit
- */
- ForceTorqueUnitInterfacePtr getForceTorqueUnit() const;
- /**
- * @brief Returns a pointer to the InertialMeasurementUnit
- * @return A pointer to the InertialMeasurementUnit
- */
- InertialMeasurementUnitInterfacePtr getInertialMeasurementUnit() const;
- /**
- * @brief Returns a pointer to the PlatformUnit
- * @return A pointer to the PlatformUnit
- */
- PlatformUnitInterfacePtr getPlatformUnit() const;
- /**
- * @brief Returns a pointer to the TCPControlUnit
- * @return A pointer to the TCPControlUnit
- */
- TCPControlUnitInterfacePtr getTCPControlUnit() const;
- /**
- * @brief Returns a pointer to the TrajectoryPlayer
- * @return A pointer to the TrajectoryPlayer
- */
- TrajectoryPlayerInterfacePtr getTrajectoryPlayer() const;
- /**
- * @brief Returns a pointer to the EmergencyStopMaster
- * @return A pointer to the EmergencyStopMaster
- */
- const EmergencyStopMasterInterfacePtr& getEmergencyStopMaster() const;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////// implementation //////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- /**
- * @brief Calls all init hooks for all managed Units
- *
- * The called hooks are
- * - \ref initializeKinematicUnit
- * - \ref initializePlatformUnit
- * - \ref initializeForceTorqueUnit
- * - \ref initializeInertialMeasurementUnit
- * - \ref initializeTrajectoryControllerUnit
- * - \ref initializeTcpControllerUnit
- */
- virtual void initializeDefaultUnits();
- /// @brief Initializes the KinematicUnit
- virtual void initializeKinematicUnit();
- /// @brief Initializes the PlatformUnit
- virtual void initializePlatformUnit();
- /// @brief Initializes the ForceTorqueUnit
- virtual void initializeForceTorqueUnit();
- /// @brief Initializes the InertialMeasurementUnit
- virtual void initializeInertialMeasurementUnit();
- /// @brief Initializes the TrajectoryControllerUnit
- virtual void initializeTrajectoryControllerUnit();
- /// @brief Initializes the TcpControllerUnit
- virtual void initializeTcpControllerUnit();
- /// @brief Create the \ref KinematicUnit (this function should be called in \ref initializeKinematicUnit)
- ManagedIceObjectPtr createKinematicSubUnit(
- const Ice::PropertiesPtr& properties,
- const std::string& positionControlMode = ControlModes::Position1DoF,
- const std::string& velocityControlMode = ControlModes::Velocity1DoF,
- const std::string& torqueControlMode = ControlModes::Torque1DoF
- );
- /**
- * @brief Registers a unit for management (the Unit is added and removed to the \ref ArmarXManager
- * and updated with new values.
- * @param unit The Unit to add.
- */
- void addUnit(ManagedIceObjectPtr unit);
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////// Data ///////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /// @brief holds all units managed by the RobotUnit
- std::vector<ManagedIceObjectPtr> units;
- /// @brief holds a copy of all units managed by the RobotUnit derived from RobotUnitSubUnit
- /// this is done to prevent casting
- std::vector<RobotUnitSubUnitPtr> subUnits;
+ //specific getters
+ /**
+ * @brief Returns a pointer to the KinematicUnit
+ * @return A pointer to the KinematicUnit
+ */
+ KinematicUnitInterfacePtr getKinematicUnit() const;
+ /**
+ * @brief Returns a pointer to the ForceTorqueUnit
+ * @return A pointer to the ForceTorqueUnit
+ */
+ ForceTorqueUnitInterfacePtr getForceTorqueUnit() const;
+ /**
+ * @brief Returns a pointer to the InertialMeasurementUnit
+ * @return A pointer to the InertialMeasurementUnit
+ */
+ InertialMeasurementUnitInterfacePtr getInertialMeasurementUnit() const;
+ /**
+ * @brief Returns a pointer to the PlatformUnit
+ * @return A pointer to the PlatformUnit
+ */
+ PlatformUnitInterfacePtr getPlatformUnit() const;
+ /**
+ * @brief Returns a pointer to the TCPControlUnit
+ * @return A pointer to the TCPControlUnit
+ */
+ TCPControlUnitInterfacePtr getTCPControlUnit() const;
+ /**
+ * @brief Returns a pointer to the TrajectoryPlayer
+ * @return A pointer to the TrajectoryPlayer
+ */
+ TrajectoryPlayerInterfacePtr getTrajectoryPlayer() const;
+ /**
+ * @brief Returns a pointer to the EmergencyStopMaster
+ * @return A pointer to the EmergencyStopMaster
+ */
+ const EmergencyStopMasterInterfacePtr& getEmergencyStopMaster() const;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ /**
+ * @brief Calls all init hooks for all managed Units
+ *
+ * The called hooks are
+ * - \ref initializeKinematicUnit
+ * - \ref initializePlatformUnit
+ * - \ref initializeForceTorqueUnit
+ * - \ref initializeInertialMeasurementUnit
+ * - \ref initializeTrajectoryControllerUnit
+ * - \ref initializeTcpControllerUnit
+ */
+ virtual void initializeDefaultUnits();
+ /// @brief Initializes the KinematicUnit
+ virtual void initializeKinematicUnit();
+ /// @brief Initializes the PlatformUnit
+ virtual void initializePlatformUnit();
+ /// @brief Initializes the ForceTorqueUnit
+ virtual void initializeForceTorqueUnit();
+ /// @brief Initializes the InertialMeasurementUnit
+ virtual void initializeInertialMeasurementUnit();
+ /// @brief Initializes the TrajectoryControllerUnit
+ virtual void initializeTrajectoryControllerUnit();
+ /// @brief Initializes the TcpControllerUnit
+ virtual void initializeTcpControllerUnit();
+ /// @brief Create the \ref KinematicUnit (this function should be called in \ref initializeKinematicUnit)
+ ManagedIceObjectPtr createKinematicSubUnit(
+ const Ice::PropertiesPtr& properties,
+ const std::string& positionControlMode = ControlModes::Position1DoF,
+ const std::string& velocityControlMode = ControlModes::Velocity1DoF,
+ const std::string& torqueControlMode = ControlModes::Torque1DoF
+ );
+ /**
+ * @brief Registers a unit for management (the Unit is added and removed to the \ref ArmarXManager
+ * and updated with new values.
+ * @param unit The Unit to add.
+ */
+ void addUnit(ManagedIceObjectPtr unit);
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief holds all units managed by the RobotUnit
+ std::vector<ManagedIceObjectPtr> units;
+ /// @brief holds a copy of all units managed by the RobotUnit derived from RobotUnitSubUnit
+ /// this is done to prevent casting
+ std::vector<RobotUnitSubUnitPtr> subUnits;
- /// @brief Pointer to the EmergencyStopMaster used by the \ref RobotUnit
- EmergencyStopMasterInterfacePtr emergencyStopMaster;
+ /// @brief Pointer to the EmergencyStopMaster used by the \ref RobotUnit
+ EmergencyStopMasterInterfacePtr emergencyStopMaster;
- /// @brief Temporary ptr to the TCPControlUnit
- ManagedIceObjectPtr tcpControllerSubUnit;
- /// @brief Temporary ptr to the TrajectoryPlayer
- ManagedIceObjectPtr trajectoryControllerSubUnit;
+ /// @brief Temporary ptr to the TCPControlUnit
+ ManagedIceObjectPtr tcpControllerSubUnit;
+ /// @brief Temporary ptr to the TrajectoryPlayer
+ ManagedIceObjectPtr trajectoryControllerSubUnit;
- /// @brief A copy of the VirtualRobot to be used inside of this Module
- VirtualRobot::RobotPtr unitCreateRobot;
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////////////////// //
- private:
- /**
- * \brief This class allows minimal access to private members of \ref Units in a sane fashion for \ref Publisher.
- * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
- */
- friend class UnitsAttorneyForPublisher;
- };
- }
+ /// @brief A copy of the VirtualRobot to be used inside of this Module
+ VirtualRobot::RobotPtr unitCreateRobot;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * \brief This class allows minimal access to private members of \ref Units in a sane fashion for \ref Publisher.
+ * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
+ */
+ friend class UnitsAttorneyForPublisher;
+ };
}
#include "RobotUnitModuleUnits.ipp"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp
index a2521740895d4849c0193e7cfb0f9b13a53d86e3..069afdfaccc7dc67c6b07f6475e91b34e7a787fe 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp
@@ -24,9 +24,7 @@
#include "RobotUnitModuleUnits.h"
-namespace armarx
-{
- namespace RobotUnitModule
+namespace armarx::RobotUnitModule
{
inline Units& Units::Instance()
{
@@ -109,4 +107,3 @@ namespace armarx
return getUnit<TrajectoryPlayerInterface>();
}
}
-}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
index 173d6f616c41438d52850e24cae16ab7965e7e17..728d1682cba7f127b003c070179dc1ef0a52ada9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
@@ -4,13 +4,13 @@
#include "util.h"
-namespace armarx
+namespace armarx::RobotUnitModule
{
- namespace RobotUnitModule
- {
- class Publisher;
- }
+ class Publisher;
+}
+namespace armarx
+{
TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
class RobotUnitObserver : public Observer
@@ -38,4 +38,3 @@ namespace armarx
}
};
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp b/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
index 377f180e28ee14404829854cadca2733a02295f5..7902e41d3ce8023a4dba6f7699086b79a7f04e07 100644
--- a/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
@@ -20,110 +20,8 @@
* GNU General Public License
*/
-
-//#include "RobotUnit.h"
-
-//#include "NJointControllers/NJointController.h"
-
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-
-///////////////////////////////////
-
-//#include "RobotUnit.h"
-//#include "NJointControllers/NJointController.h"
-
-//#include "NJointControllers/NJointController.h"
-//#include "RobotUnit.h"
-
-//#include "NJointControllers/NJointController.h"
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-//#include "NJointControllers/NJointController.h"
-
-//#include "RobotUnit.h"
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-//#include "RobotUnit.h"
-
-////////////////////////////////////
-
-
-//#include "RobotUnit.h"
-//#include "NJointControllers/NJointController.h"
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-
#include "NJointControllers/NJointController.h"
#include "RobotUnit.h"
#include "RobotUnitModules/RobotUnitModuleBase.h"
-//#include "NJointControllers/NJointController.h"
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-//#include "RobotUnit.h"
-
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-//#include "NJointControllers/NJointController.h"
-//#include "RobotUnit.h"
-
-//#include "RobotUnit.h"
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-//#include "NJointControllers/NJointController.h"
-
-//#include "RobotUnitModules/RobotUnitModuleBase.h"
-//#include "RobotUnit.h"
-//#include "NJointControllers/NJointController.h"
-
-
-///////////////////////////////////
-
-//#include <atomic>
-//#include <chrono>
-//#include <mutex>
-
-//#include <boost/preprocessor/variadic/to_seq.hpp>
-//#include <boost/preprocessor/seq/for_each.hpp>
-//#include <boost/current_function.hpp>
-
-//#include <VirtualRobot/Robot.h>
-
-//#include <ArmarXCore/core/Component.h>
-
-//#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-
-
-
-/*
-#include "BasicControllers.h"
-#include "Constants.h"
-#include "ControlModes.h"
-#include "ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
-#include "ControlTargets/ControlTarget1DoFActuator.h"
-#include "ControlTargets/ControlTargetBase.h"
-#include "DefaultWidgetDescriptions.h"
-#include "Devices/RTThreadTimingsSensorDevice.h"
-#include "Devices/ControlDevice.h"
-#include "Devices/SensorDevice.h"
-#include "Devices/DeviceBase.h"
-#include "JointControllers/JointController.h"
-#include "NJointControllers/NJointController.h"
-#include "NJointControllers/NJointKinematicUnitPassThroughController.h"
-#include "NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h"
-#include "NJointControllers/NJointTrajectoryController.h"
-#include "RobotUnit.h"
-#include "SensorValues/SensorValueRTThreadTimings.h"
-#include "SensorValues/SensorValueIMU.h"
-#include "SensorValues/SensorValue1DoFActuator.h"
-#include "SensorValues/SensorValueHolonomicPlatform.h"
-#include "SensorValues/SensorValueBase.h"
-#include "SensorValues/SensorValueForceTorque.h"
-#include "util/JointAndNJointControllers.h"
-#include "Units/KinematicSubUnit.h"
-#include "Units/RobotUnitSubUnit.h"
-#include "Units/PlatformSubUnit.h"
-#include "Units/InertialMeasurementSubUnit.h"
-#include "Units/ForceTorqueSubUnit.h"
-#include "util.h"
-
//this file is only for syntax checks
-*/
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index ec6d2c5fb2d93b99a716cc1062fc4c39552f084b..66fbc66b5949598ba09ed4b328603cad95ddb357 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -93,7 +93,7 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
bool statusesAvalueChanged = false;
long timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
- std::set<NJointController*> nJointCtrls {c.nJointControllers.begin(), c.nJointControllers.end()};
+ std::set<NJointControllerBase*> nJointCtrls {c.nJointControllers.begin(), c.nJointControllers.end()};
std::vector<std::string> actuatorsWithoutSensor;
actuatorsWithoutSensor.reserve(devs.size());
for (const auto& name2actuatorData : devs)
@@ -243,7 +243,7 @@ void armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torqu
void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm, const Ice::Current&)
{
- std::map<std::string, NJointControllerPtr> toActivate;
+ std::map<std::string, NJointControllerBasePtr> toActivate;
{
std::lock_guard<std::mutex> guard {dataMutex};
for (const auto& n2c : ncm)
@@ -282,7 +282,7 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
{
const auto& name = n2NJointCtrl.first;
ARMARX_DEBUG << "checking group of '" << name << "'";
- const NJointControllerPtr& nJointCtrl = n2NJointCtrl.second;
+ const NJointControllerBasePtr& nJointCtrl = n2NJointCtrl.second;
if (!controlDeviceHardwareControlModeGroupsMerged.count(name))
{
continue;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index b65d6f298ce60565c1c0ad2b94182cf0e449b71f..f978876e538d9b8420af57d8c35b8f8f8063a9bd 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -125,7 +125,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
bool nodeSetAlreadyControlled = false;
for (auto& name : NJointControllers)
{
- NJointControllerPtr controller;
+ NJointControllerBasePtr controller;
try
{
controller = robotUnit->getNJointControllerNotNull(name);
@@ -203,7 +203,7 @@ void armarx::TCPControllerSubUnit::componentPropertiesUpdated(const std::set<std
{
float avoidJointLimitsKp = getProperty<float>("AvoidJointLimitsKp").getValue();
auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
- for (NJointControllerPtr controller : activeNJointControllers)
+ for (NJointControllerBasePtr controller : activeNJointControllers)
{
auto tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
if (tcpController)
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index ab9646af6869fa73a97b2925db8bbd6a6404a4f8..0a3669091152c2de2ec2caad4865a0e3d6b6c08d 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -428,7 +428,7 @@ bool TrajectoryControllerSubUnit::controllerExists()
auto allNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
NJointTrajectoryControllerPtr trajController;
- for (NJointControllerPtr controller : allNJointControllers)
+ for (const auto& controller : allNJointControllers)
{
trajController = NJointTrajectoryControllerPtr::dynamicCast(controller);
if (!trajController)
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index c2c4459d03c31fdc67a2a973608eb4796acbb53d..bca3c8214e327b59584bc091491289cc2f3dc690 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -40,192 +40,182 @@
namespace armarx
{
- namespace detail
+ class RobotUnit;
+ struct ControlThreadOutputBuffer;
+}
+
+namespace armarx::RobotUnitModule
+{
+ class Logging;
+}
+
+namespace armarx::detail
+{
+ inline std::string armarx_sprintf_helper(boost::format& f)
{
- inline std::string armarx_sprintf_helper(boost::format& f)
- {
- return boost::str(f);
- }
+ return boost::str(f);
+ }
+
+ template<class T, class... Args>
+ inline std::string armarx_sprintf_helper(boost::format& f, T&& t, Args&& ... args)
+ {
+ return armarx_sprintf_helper(f % std::forward<T>(t), std::forward<Args>(args)...);
+ }
+ template<class... Args>
+ inline std::string armarx_sprintf(const std::string& formatString, Args&& ... args)
+ {
+ boost::format f(formatString);
+ return ::armarx::detail::armarx_sprintf_helper(f, std::forward<Args>(args)...);
+ }
- template<class T, class... Args>
- inline std::string armarx_sprintf_helper(boost::format& f, T&& t, Args&& ... args)
+ struct RtMessageLogEntryBase
+ {
+ RtMessageLogEntryBase():
+ time {IceUtil::Time::now()}
+ {}
+ virtual ~RtMessageLogEntryBase() = default;
+
+ RtMessageLogEntryBase& setLoggingLevel(MessageType lvl);
+ RtMessageLogEntryBase& deactivateSpam(float sec);
+ RtMessageLogEntryBase& deactivateSpamTag(std::uint64_t tag);
+
+ void print(Ice::Int controlThreadId) const;
+ virtual std::size_t line() const = 0;
+ virtual std::string file() const = 0;
+ virtual std::string func() const = 0;
+ virtual std::string format() const = 0;
+ IceUtil::Time getTime() const;
+
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(RtMessageLogEntryBase)
+ protected:
+ friend struct RtMessageLogBuffer;
+ private:
+ MessageType loggingLevel {eUNDEFINED};
+ float deactivateSpamSec {0};
+ bool printMsg {false};
+ std::uint64_t deactivateSpamTag_;
+ IceUtil::Time time;
+ };
+
+ struct RtMessageLogEntryDummy : RtMessageLogEntryBase
+ {
+ static RtMessageLogEntryDummy Instance;
+ protected:
+ std::string format() const final override;
+ std::size_t line() const final override;
+ std::string file() const final override;
+ std::string func() const final override;
+
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER
+ };
+
+ struct RtMessageLogEntryNull : RtMessageLogEntryBase
+ {
+ static RtMessageLogEntryNull Instance;
+ protected:
+ std::string format() const final override
{
- return armarx_sprintf_helper(f % std::forward<T>(t), std::forward<Args>(args)...);
+ return "";
}
- template<class... Args>
- inline std::string armarx_sprintf(const std::string& formatString, Args&& ... args)
+ std::size_t line() const final override
{
- boost::format f(formatString);
- return ::armarx::detail::armarx_sprintf_helper(f, std::forward<Args>(args)...);
+ return 0;
}
-
- struct RtMessageLogEntryBase
+ std::string file() const final override
{
- RtMessageLogEntryBase():
- time {IceUtil::Time::now()}
- {}
- virtual ~RtMessageLogEntryBase() = default;
-
- RtMessageLogEntryBase& setLoggingLevel(MessageType lvl);
- RtMessageLogEntryBase& deactivateSpam(float sec);
- RtMessageLogEntryBase& deactivateSpamTag(std::uint64_t tag);
-
- void print(Ice::Int controlThreadId) const;
- virtual std::size_t line() const = 0;
- virtual std::string file() const = 0;
- virtual std::string func() const = 0;
- virtual std::string format() const = 0;
- IceUtil::Time getTime() const;
-
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(RtMessageLogEntryBase)
- protected:
- friend struct RtMessageLogBuffer;
- private:
- MessageType loggingLevel {eUNDEFINED};
- float deactivateSpamSec {0};
- bool printMsg {false};
- std::uint64_t deactivateSpamTag_;
- IceUtil::Time time;
- };
-
- struct RtMessageLogEntryDummy : RtMessageLogEntryBase
+ return "";
+ }
+ std::string func() const final override
{
- static RtMessageLogEntryDummy Instance;
- protected:
- std::string format() const final override;
- std::size_t line() const final override;
- std::string file() const final override;
- std::string func() const final override;
-
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER
- };
+ return "";
+ }
- struct RtMessageLogEntryNull : RtMessageLogEntryBase
- {
- static RtMessageLogEntryNull Instance;
- protected:
- std::string format() const final override
- {
- return "";
- }
- std::size_t line() const final override
- {
- return 0;
- }
- std::string file() const final override
- {
- return "";
- }
- std::string func() const final override
- {
- return "";
- }
-
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER
- };
- }
-}
-namespace armarx
-{
- class RobotUnit;
- struct ControlThreadOutputBuffer;
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER
+ };
- namespace RobotUnitModule
+ struct RtMessageLogBuffer
{
- class Logging;
- }
+ RtMessageLogBuffer(const RtMessageLogBuffer& other, bool minimize = false);
+ RtMessageLogBuffer(
+ std::size_t bufferSize, std::size_t numEntries,
+ std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries);
+ ~RtMessageLogBuffer();
- namespace detail
- {
- struct RtMessageLogBuffer
- {
- RtMessageLogBuffer(const RtMessageLogBuffer& other, bool minimize = false);
- RtMessageLogBuffer(
- std::size_t bufferSize, std::size_t numEntries,
- std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries);
- ~RtMessageLogBuffer();
+ RtMessageLogBuffer() = delete;
+ RtMessageLogBuffer(RtMessageLogBuffer&&) = delete;
+ RtMessageLogBuffer& operator=(RtMessageLogBuffer&&) = delete;
+ RtMessageLogBuffer& operator=(const RtMessageLogBuffer&) = delete;
- RtMessageLogBuffer() = delete;
- RtMessageLogBuffer(RtMessageLogBuffer&&) = delete;
- RtMessageLogBuffer& operator=(RtMessageLogBuffer&&) = delete;
- RtMessageLogBuffer& operator=(const RtMessageLogBuffer&) = delete;
+ void reset(std::size_t& bufferSize, std::size_t& numEntries, std::size_t iterationCount);
- void reset(std::size_t& bufferSize, std::size_t& numEntries, std::size_t iterationCount);
+ const std::vector<const RtMessageLogEntryBase*>& getEntries() const;
- const std::vector<const RtMessageLogEntryBase*>& getEntries() const;
+ std::size_t getMaximalBufferSize() const;
+ std::size_t getMaximalNumberOfBufferEntries() const;
+ private:
+ void deleteAll();
- std::size_t getMaximalBufferSize() const;
- std::size_t getMaximalNumberOfBufferEntries() const;
- private:
- void deleteAll();
+ friend struct ControlThreadOutputBufferEntry;
+ friend struct ::armarx::ControlThreadOutputBuffer;
+ friend class ::armarx::RobotUnit;
+ friend class ::armarx::RobotUnitModule::Logging;
- friend struct ControlThreadOutputBufferEntry;
- friend struct ::armarx::ControlThreadOutputBuffer;
- friend class ::armarx::RobotUnit;
- friend class ::armarx::RobotUnitModule::Logging;
+ const std::size_t initialBufferSize;
+ const std::size_t initialBufferEntryNumbers;
+ const std::size_t bufferMaxSize;
+ const std::size_t bufferMaxNumberEntries;
- const std::size_t initialBufferSize;
- const std::size_t initialBufferEntryNumbers;
- const std::size_t bufferMaxSize;
- const std::size_t bufferMaxNumberEntries;
+ std::vector<std::uint8_t> buffer;
+ std::size_t bufferSpace;
+ void* bufferPlace;
- std::vector<std::uint8_t> buffer;
- std::size_t bufferSpace;
- void* bufferPlace;
+ std::vector<const RtMessageLogEntryBase*> entries;
+ std::size_t entriesWritten {0};
- std::vector<const RtMessageLogEntryBase*> entries;
- std::size_t entriesWritten {0};
+ std::size_t requiredAdditionalBufferSpace {0};
+ std::size_t requiredAdditionalEntries {0};
+ std::size_t messagesLost {0};
- std::size_t requiredAdditionalBufferSpace {0};
- std::size_t requiredAdditionalEntries {0};
- std::size_t messagesLost {0};
+ std::size_t maxAlign {1};
+ };
- std::size_t maxAlign {1};
- };
+ struct ControlThreadOutputBufferEntry
+ {
+ //default ctors / ops
- struct ControlThreadOutputBufferEntry
- {
- //default ctors / ops
-
- ControlThreadOutputBufferEntry(
- const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
- const KeyValueVector<std::string, SensorDevicePtr >& sensorDevices,
- std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
- std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries
- );
- ControlThreadOutputBufferEntry(const ControlThreadOutputBufferEntry& other, bool minimize = false);
-
- ControlThreadOutputBufferEntry() = delete;
- ControlThreadOutputBufferEntry(ControlThreadOutputBufferEntry&&) = delete;
- ControlThreadOutputBufferEntry& operator=(ControlThreadOutputBufferEntry&&) = delete;
- ControlThreadOutputBufferEntry& operator=(const ControlThreadOutputBufferEntry&) = delete;
-
- std::size_t getDataBufferSize() const;
-
- //data
- /// @brief Timestamp in wall time (never use the virtual time for this)
- IceUtil::Time writeTimestamp;
- IceUtil::Time sensorValuesTimestamp;
- IceUtil::Time timeSinceLastIteration;
- std::vector<PropagateConst<SensorValueBase*>> sensors;
- std::vector<std::vector<PropagateConst<ControlTargetBase*>>> control;
- std::size_t iteration {0};
-
- RtMessageLogBuffer messages;
- private:
- std::vector<std::uint8_t> buffer;
- };
- }
+ ControlThreadOutputBufferEntry(
+ const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
+ const KeyValueVector<std::string, SensorDevicePtr >& sensorDevices,
+ std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
+ std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries
+ );
+ ControlThreadOutputBufferEntry(const ControlThreadOutputBufferEntry& other, bool minimize = false);
+
+ ControlThreadOutputBufferEntry() = delete;
+ ControlThreadOutputBufferEntry(ControlThreadOutputBufferEntry&&) = delete;
+ ControlThreadOutputBufferEntry& operator=(ControlThreadOutputBufferEntry&&) = delete;
+ ControlThreadOutputBufferEntry& operator=(const ControlThreadOutputBufferEntry&) = delete;
+
+ std::size_t getDataBufferSize() const;
+
+ //data
+ /// @brief Timestamp in wall time (never use the virtual time for this)
+ IceUtil::Time writeTimestamp;
+ IceUtil::Time sensorValuesTimestamp;
+ IceUtil::Time timeSinceLastIteration;
+ std::vector<PropagateConst<SensorValueBase*>> sensors;
+ std::vector<std::vector<PropagateConst<ControlTargetBase*>>> control;
+ std::size_t iteration {0};
+
+ RtMessageLogBuffer messages;
+ private:
+ std::vector<std::uint8_t> buffer;
+ };
}
namespace armarx
{
- namespace RobotUnitModule
- {
- class Logging;
- }
-
struct ControlThreadOutputBuffer
{
using Entry = detail::ControlThreadOutputBufferEntry;
@@ -282,25 +272,18 @@ namespace armarx
std::size_t messageBufferSize {0};
std::size_t messageBufferEntries {0};
};
-
using SensorAndControl = ControlThreadOutputBuffer::Entry;
}
-
-// ARMARX_INFO << deactivateSpam(1) << "Redirected RT Logging:\n" << ::armarx::detail::armarx_printf_helper(f, __VA_ARGS__);
-
#define _detail_ARMARX_RT_REDIRECT( ...) ([&]{ \
::armarx::detail::armarx_sprintf(__VA_ARGS__); \
ARMARX_INFO << deactivateSpam(1) << "Redirected RT Logging:\n";\
return &::armarx::detail::RtMessageLogEntryNull::Instance;}())
-
-
-
#define ARMARX_RT_LOGF(...) (*((::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance())? _detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION,__LINE__, __VA_ARGS__, true) : _detail_ARMARX_RT_REDIRECT(__VA_ARGS__)))
#define _detail_ARMARX_RT_LOGF(file_, func_, line_, FormatString, ...) \
- ([&]{ \
+ ([&]{ \
using namespace ::armarx; \
using RtMessageLogEntryBase = ControlThreadOutputBuffer::RtMessageLogEntryBase; \
struct RtMessageLogEntry : RtMessageLogEntryBase \
@@ -333,73 +316,70 @@ namespace armarx
#define ARMARX_RT_LOGF_FATAL(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::eFATAL)
//impl
-namespace armarx
+namespace armarx::detail
{
- namespace detail
+ inline IceUtil::Time RtMessageLogEntryBase::getTime() const
{
- inline IceUtil::Time RtMessageLogEntryBase::getTime() const
- {
- return time;
- }
-
- inline std::string RtMessageLogEntryDummy::format() const
- {
- throw std::logic_error {"called 'format' of RtMessageLogEntryDummy"};
- }
+ return time;
+ }
- inline std::size_t RtMessageLogEntryDummy::line() const
- {
- throw std::logic_error {"called 'line' of RtMessageLogEntryDummy"};
- }
+ inline std::string RtMessageLogEntryDummy::format() const
+ {
+ throw std::logic_error {"called 'format' of RtMessageLogEntryDummy"};
+ }
- inline std::string RtMessageLogEntryDummy::file() const
- {
- throw std::logic_error {"called 'file' of RtMessageLogEntryDummy"};
- }
+ inline std::size_t RtMessageLogEntryDummy::line() const
+ {
+ throw std::logic_error {"called 'line' of RtMessageLogEntryDummy"};
+ }
- inline std::string RtMessageLogEntryDummy::func() const
- {
- throw std::logic_error {"called 'func' of RtMessageLogEntryDummy"};
- }
+ inline std::string RtMessageLogEntryDummy::file() const
+ {
+ throw std::logic_error {"called 'file' of RtMessageLogEntryDummy"};
+ }
- inline RtMessageLogBuffer::RtMessageLogBuffer(
- std::size_t bufferSize, std::size_t numEntries,
- std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries
- ):
- initialBufferSize {bufferSize},
- initialBufferEntryNumbers {numEntries},
- bufferMaxSize {bufferMaxSize},
- bufferMaxNumberEntries {bufferMaxNumberEntries},
- buffer(bufferSize, 0),
- bufferSpace {buffer.size()},
- bufferPlace {buffer.data()},
- entries(numEntries, nullptr)
- {}
+ inline std::string RtMessageLogEntryDummy::func() const
+ {
+ throw std::logic_error {"called 'func' of RtMessageLogEntryDummy"};
+ }
- inline RtMessageLogBuffer::~RtMessageLogBuffer()
- {
- deleteAll();
- }
+ inline RtMessageLogBuffer::RtMessageLogBuffer(
+ std::size_t bufferSize, std::size_t numEntries,
+ std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries
+ ):
+ initialBufferSize {bufferSize},
+ initialBufferEntryNumbers {numEntries},
+ bufferMaxSize {bufferMaxSize},
+ bufferMaxNumberEntries {bufferMaxNumberEntries},
+ buffer(bufferSize, 0),
+ bufferSpace {buffer.size()},
+ bufferPlace {buffer.data()},
+ entries(numEntries, nullptr)
+ {}
+
+ inline RtMessageLogBuffer::~RtMessageLogBuffer()
+ {
+ deleteAll();
+ }
- inline const std::vector<const RtMessageLogEntryBase*>& RtMessageLogBuffer::getEntries() const
- {
- return entries;
- }
+ inline const std::vector<const RtMessageLogEntryBase*>& RtMessageLogBuffer::getEntries() const
+ {
+ return entries;
+ }
- inline std::size_t RtMessageLogBuffer::getMaximalBufferSize() const
- {
- return bufferMaxSize;
- }
+ inline std::size_t RtMessageLogBuffer::getMaximalBufferSize() const
+ {
+ return bufferMaxSize;
+ }
- inline std::size_t RtMessageLogBuffer::getMaximalNumberOfBufferEntries() const
- {
- return bufferMaxNumberEntries;
- }
+ inline std::size_t RtMessageLogBuffer::getMaximalNumberOfBufferEntries() const
+ {
+ return bufferMaxNumberEntries;
+ }
- inline std::size_t ControlThreadOutputBufferEntry::getDataBufferSize() const
- {
- return buffer.size();
- }
+ inline std::size_t ControlThreadOutputBufferEntry::getDataBufferSize() const
+ {
+ return buffer.size();
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
index 8168e747ee11a30f376545d5e12229cf9b44147f..3a98c100b79bef43a55b1dd12475a41c8016c5b5 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
@@ -51,92 +51,92 @@ namespace std
}
#endif
-namespace armarx
+namespace armarx::detail
{
- namespace detail
+ template<class Base>
+ class HeterogenousContinuousContainerBase
{
- template<class Base>
- class HeterogenousContinuousContainerBase
+ public:
+ bool empty() const
{
- public:
- bool empty() const
- {
- return begin_ == current_;
- }
- bool owning() const
- {
- return storage_ != nullptr;
- }
- std::size_t getRemainingStorageCapacity() const
- {
- return static_cast<const std::uint8_t*>(end_) -
- static_cast<const std::uint8_t*>(current_);
- }
- std::size_t getStorageCapacity() const
- {
- return static_cast<const std::uint8_t*>(end_) -
- static_cast<const std::uint8_t*>(begin_);
- }
- std::size_t getUsedStorageCapacity() const
- {
- return static_cast<const std::uint8_t*>(current_) -
- static_cast<const std::uint8_t*>(begin_);
- }
- void assignStorage(void* begin, void* end)
+ return begin_ == current_;
+ }
+ bool owning() const
+ {
+ return storage_ != nullptr;
+ }
+ std::size_t getRemainingStorageCapacity() const
+ {
+ return static_cast<const std::uint8_t*>(end_) -
+ static_cast<const std::uint8_t*>(current_);
+ }
+ std::size_t getStorageCapacity() const
+ {
+ return static_cast<const std::uint8_t*>(end_) -
+ static_cast<const std::uint8_t*>(begin_);
+ }
+ std::size_t getUsedStorageCapacity() const
+ {
+ return static_cast<const std::uint8_t*>(current_) -
+ static_cast<const std::uint8_t*>(begin_);
+ }
+ void assignStorage(void* begin, void* end)
+ {
+ ARMARX_CHECK_EXPRESSION(empty());
+ ARMARX_CHECK_LESS_EQUAL(begin, end);
+ storage_ = nullptr;
+ begin_ = begin;
+ current_ = begin;
+ end_ = end;
+ }
+ void setStorageCapacity(std::size_t sz)
+ {
+ ARMARX_CHECK_EXPRESSION(empty());
+ if (!sz)
{
- ARMARX_CHECK_EXPRESSION(empty());
- ARMARX_CHECK_LESS_EQUAL(begin, end);
- storage_ = nullptr;
- begin_ = begin;
- current_ = begin;
- end_ = end;
+ assignStorage(nullptr, nullptr);
+ return;
}
- void setStorageCapacity(std::size_t sz)
- {
- ARMARX_CHECK_EXPRESSION(empty());
- if (!sz)
- {
- assignStorage(nullptr, nullptr);
- return;
- }
- //replache with void* aligned_alloc( std::size_t alignment, std::size_t size ) @c++17
- sz += 63;
- storage_.reset(new std::uint8_t[sz]);
- begin_ = static_cast<void*>(storage_.get());
- current_ = begin_;
- end_ = static_cast<void*>(storage_.get() + sz);
- }
- protected:
- template<class Derived>
- Base* pushBack(const Derived* d)
- {
- static_assert(
- std::is_base_of<Base, Derived>::value,
- "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
- );
- std::size_t space_ = getRemainingStorageCapacity();
+ //replache with void* aligned_alloc( std::size_t alignment, std::size_t size ) @c++17
+ sz += 63;
+ storage_.reset(new std::uint8_t[sz]);
+ begin_ = static_cast<void*>(storage_.get());
+ current_ = begin_;
+ end_ = static_cast<void*>(storage_.get() + sz);
+ }
+ protected:
+ template<class Derived>
+ Base* pushBack(const Derived* d)
+ {
+ static_assert(
+ std::is_base_of<Base, Derived>::value,
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
+ );
+ std::size_t space_ = getRemainingStorageCapacity();
- void* ptr = std::align(d->_alignof(), d->_sizeof(), current_, space_);
+ void* ptr = std::align(d->_alignof(), d->_sizeof(), current_, space_);
- if (!ptr)
- {
- return nullptr;
- }
- current_ = static_cast<void*>(static_cast<std::uint8_t*>(ptr) + d->_sizeof());
- return d->_placementCopyConstruct(ptr);
- }
- void clear()
+ if (!ptr)
{
- current_ = begin_;
+ return nullptr;
}
- private:
- std::unique_ptr<std::uint8_t[]> storage_ {nullptr};
- void* begin_ {nullptr};
- void* current_ {nullptr};
- void* end_ {nullptr};
- };
- }
+ current_ = static_cast<void*>(static_cast<std::uint8_t*>(ptr) + d->_sizeof());
+ return d->_placementCopyConstruct(ptr);
+ }
+ void clear()
+ {
+ current_ = begin_;
+ }
+ private:
+ std::unique_ptr<std::uint8_t[]> storage_ {nullptr};
+ void* begin_ {nullptr};
+ void* current_ {nullptr};
+ void* end_ {nullptr};
+ };
+}
+namespace armarx
+{
template<class Base, bool UsePropagateConst = true>
class HeterogenousContinuousContainer : public detail::HeterogenousContinuousContainerBase<Base>
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h b/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
index faee01fc2c8f714adef111b3ef3d483d54079e81..84809506f96ca0f0d48211fe2b7511f75b08d8f9 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
@@ -26,7 +26,7 @@
namespace armarx
{
class JointController;
- class NJointController;
+ class NJointControllerBase;
/// @brief Structure used by the RobotUnit to swap lists of Joint and NJoint controllers
struct JointAndNJointControllers
{
@@ -42,7 +42,7 @@ namespace armarx
}
std::vector<JointController*> jointControllers;
- std::vector<NJointController*> nJointControllers;
+ std::vector<NJointControllerBase*> nJointControllers;
/// @brief this is set by RobotUnit::writeRequestedControllers
std::vector<std::size_t> jointToNJointControllerAssignement;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/Time.h b/source/RobotAPI/components/units/RobotUnit/util/Time.h
index 919ea91a7bfb378487614b8997dd044efa72e395..9199a9714d09951ef6ae4df3f9b0f82f335a9845 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/Time.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/Time.h
@@ -26,101 +26,99 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-namespace armarx
+namespace armarx::detail
{
- namespace detail
+ template<class TimeT = std::chrono::microseconds>
+ struct TimerTag
{
- template<class TimeT = std::chrono::microseconds>
- struct TimerTag
- {
- //assume it is std::chrono
-
- using ClockT = std::chrono::high_resolution_clock;
- const ClockT::time_point beg;
+ //assume it is std::chrono
- TimerTag() : beg {ClockT::now()} {}
+ using ClockT = std::chrono::high_resolution_clock;
+ const ClockT::time_point beg;
- TimeT stop()
- {
- return std::chrono::duration_cast<TimeT>(ClockT::now() - beg);
- }
- };
+ TimerTag() : beg {ClockT::now()} {}
- template<>
- struct TimerTag<long>
+ TimeT stop()
{
- //return micro seconds as long
+ return std::chrono::duration_cast<TimeT>(ClockT::now() - beg);
+ }
+ };
- using ClockT = std::chrono::high_resolution_clock;
- const ClockT::time_point beg;
+ template<>
+ struct TimerTag<long>
+ {
+ //return micro seconds as long
- TimerTag() : beg {ClockT::now()} {}
+ using ClockT = std::chrono::high_resolution_clock;
+ const ClockT::time_point beg;
- long stop()
- {
- return std::chrono::duration_cast<std::chrono::microseconds>(ClockT::now() - beg).count();
- }
- };
+ TimerTag() : beg {ClockT::now()} {}
- template<>
- struct TimerTag<TimestampVariant> : TimerTag<>
+ long stop()
{
- TimestampVariant stop()
- {
- return {TimerTag<std::chrono::microseconds>::stop().count()};
- }
- };
-
- template<>
- struct TimerTag<IceUtil::Time> : TimerTag<>
+ return std::chrono::duration_cast<std::chrono::microseconds>(ClockT::now() - beg).count();
+ }
+ };
+
+ template<>
+ struct TimerTag<TimestampVariant> : TimerTag<>
+ {
+ TimestampVariant stop()
{
- TimestampVariant stop()
- {
- return IceUtil::Time::microSeconds(TimerTag<std::chrono::microseconds>::stop().count());
- }
- };
-
- template <class Fun, class TimeT>
- TimeT operator*(TimerTag<TimeT>&& t, Fun&& fn)
+ return {TimerTag<std::chrono::microseconds>::stop().count()};
+ }
+ };
+
+ template<>
+ struct TimerTag<IceUtil::Time> : TimerTag<>
+ {
+ TimestampVariant stop()
{
- fn();
- return t.stop();
+ return IceUtil::Time::microSeconds(TimerTag<std::chrono::microseconds>::stop().count());
}
+ };
- //for virtual time
+ template <class Fun, class TimeT>
+ TimeT operator*(TimerTag<TimeT>&& t, Fun&& fn)
+ {
+ fn();
+ return t.stop();
+ }
- template<class TimeT = IceUtil::Time>
- struct VirtualTimerTag;
+ //for virtual time
- template<>
- struct VirtualTimerTag<TimestampVariant>
- {
- const IceUtil::Time beg;
- VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
- TimestampVariant stop()
- {
- return {TimeUtil::GetTime() - beg};
- }
- };
-
- template<>
- struct VirtualTimerTag<IceUtil::Time>
+ template<class TimeT = IceUtil::Time>
+ struct VirtualTimerTag;
+
+ template<>
+ struct VirtualTimerTag<TimestampVariant>
+ {
+ const IceUtil::Time beg;
+ VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
+ TimestampVariant stop()
{
- const IceUtil::Time beg;
- VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
- TimestampVariant stop()
- {
- return TimeUtil::GetTime() - beg;
- }
- };
-
- template <class Fun, class TimeT>
- TimeT operator*(VirtualTimerTag<TimeT>&& t, Fun&& fn)
+ return {TimeUtil::GetTime() - beg};
+ }
+ };
+
+ template<>
+ struct VirtualTimerTag<IceUtil::Time>
+ {
+ const IceUtil::Time beg;
+ VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
+ TimestampVariant stop()
{
- fn();
- return t.stop();
+ return TimeUtil::GetTime() - beg;
}
+ };
+
+ template <class Fun, class TimeT>
+ TimeT operator*(VirtualTimerTag<TimeT>&& t, Fun&& fn)
+ {
+ fn();
+ return t.stop();
}
}
+
#define ARMARX_STOPWATCH(...) ::armarx::detail::TimerTag<__VA_ARGS__>{} *[&]
#define ARMARX_VIRTUAL_STOPWATCH(...) ::armarx::detail::VirtualTimerTag<__VA_ARGS__>{} *[&]