diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index ff3d42340eb8804e6d472ad44fcb76f648a03428..dd03919fe258401049146b5578db8dd12d34f456 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -29,7 +29,9 @@ set(LIB_FILES
RobotUnitObserver.cpp
BasicControllers.cpp
DefaultWidgetDescriptions.cpp
- RobotSynchronization.cpp
+
+ #robot unit modules need to be added to the list below (but not here)
+ RobotUnitModules/RobotUnitModuleBase.cpp
ControlTargets/ControlTargetBase.cpp
@@ -60,6 +62,8 @@ set(LIB_FILES
set(LIB_HEADERS
util.h
util/EigenForwardDeclarations.h
+ util/HeterogenousContinuousContainer.h
+ util/HeterogenousContinuousContainerMacros.h
util/KeyValueVector.h
util/AtomicWrapper.h
util/Time.h
@@ -70,15 +74,18 @@ set(LIB_HEADERS
util/introspection/ClassMemberInfo.h
util/RtLogging.h
+ #robot unit modules need to be added to the list below (but not here)
+ RobotUnitModules/RobotUnitModuleBase.h
+ RobotUnitModules/RobotUnitModuleBase.ipp
+ RobotUnitModules/RobotUnitModules.h
+
PDController.h
Constants.h
ControlModes.h
RobotUnit.h
- RobotUnit.ipp
RobotUnitObserver.h
BasicControllers.h
DefaultWidgetDescriptions.h
- RobotSynchronization.h
ControlTargets/ControlTargetBase.h
ControlTargets/ControlTarget1DoFActuator.h
@@ -102,7 +109,7 @@ set(LIB_HEADERS
JointControllers/JointController.h
NJointControllers/NJointController.h
- NJointControllers/NJointController.hpp
+ NJointControllers/NJointController.ipp
NJointControllers/NJointTrajectoryController.h
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -117,6 +124,85 @@ set(LIB_HEADERS
Devices/RTThreadTimingsSensorDevice.h
)
+###########################################################################
+#since the robot unit is a complex class, it is split into several modules.
+#one reason behind this is stopping developers from using datastructures in the wrong way (eg causing racing conditions)
+#for the robot unit to work, ALL modules are required (a class has to derive all of them)
+#modules are autodetected in the RobotUnitModules folder, but should be listed here anyways
+#(since glob does not autodetect new files)
+#(see: https://cmake.org/cmake/help/v3.0/command/file.html)
+# > If no CMakeLists.txt file changes when a source is added or removed then the generated build system cannot know when to ask CMake to regenerate.
+set(RobotUnitModules
+ Management
+ ControllerManagement
+ ControlThread
+ ControlThreadDataBuffer
+ Devices
+ Logging
+ Publisher
+ RobotData
+ SelfCollisionChecker
+ Units
+)
+file(GLOB_RECURSE files_full_path "${CMAKE_CURRENT_SOURCE_DIR}/RobotUnitModules/*")
+set(files)
+foreach(file_full_path ${files_full_path})
+ #remove prefix
+ string(REGEX REPLACE "${CMAKE_CURRENT_SOURCE_DIR}/" "" file_rel_path "${file_full_path}")
+ list(APPEND files ${file_rel_path})
+endforeach()
+#remove base module / collection header
+list(REMOVE_ITEM files RobotUnitModules/RobotUnitModuleBase.h)
+list(REMOVE_ITEM files RobotUnitModules/RobotUnitModuleBase.cpp)
+list(REMOVE_ITEM files RobotUnitModules/RobotUnitModuleBase.ipp)
+list(REMOVE_ITEM files RobotUnitModules/RobotUnitModules.h)
+
+#remove all expected files
+foreach(module ${RobotUnitModules})
+ set(ModuleName RobotUnitModule${module})
+ set(ModuleFileBase "RobotUnitModules/${ModuleName}")
+
+ #h
+ list(FIND files "${ModuleFileBase}.h" idx)
+
+ if(idx EQUAL -1)
+ message(STATUS "Files:")
+ printlist(" " "${files}")
+ message(FATAL_ERROR "MODULE FILE NOT FOUND: '${ModuleFileBase}.h'")
+ endif()
+ list(REMOVE_ITEM files ${ModuleFileBase}.h)
+ list(APPEND LIB_HEADERS ${ModuleFileBase}.h)
+
+ #cpp
+ list(FIND files ${ModuleFileBase}.cpp idx)
+ if(idx EQUAL -1)
+ message(STATUS "Files:")
+ printlist(" " "${files}")
+ message(FATAL_ERROR "MODULE FILE NOT FOUND: '${ModuleFileBase}.cpp'")
+ endif()
+ list(REMOVE_ITEM files ${ModuleFileBase}.cpp)
+ list(APPEND LIB_FILES ${ModuleFileBase}.cpp)
+
+ #ipp
+ list(FIND files ${ModuleFileBase}.ipp idx)
+ if(NOT idx EQUAL -1)
+ list(APPEND LIB_HEADERS ${ModuleFileBase}.ipp)
+ list(REMOVE_ITEM files ${ModuleFileBase}.ipp)
+ endif()
+ message(STATUS "Found RobotUnitModule ${ModuleName}")
+endforeach()
+#check remaining files
+foreach(f ${files})
+ if(f MATCHES ".*/RobotUnitModule[a-zA-Z]*.*" )
+ message(FATAL_ERROR "MODULE NOT ADDED TO LIST OF MODULES: ${f}" )
+ endif()
+endforeach()
+
+list_to_string(RobotUnitModules "," ${RobotUnitModules})
+
+message(STATUS "collected these names for RobotUnitModules: ${RobotUnitModules}")
+#done detecting modules!
+###########################################################################
@@ -124,3 +210,4 @@ add_subdirectory(test)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+target_compile_definitions("${LIB_NAME}" PUBLIC "-DRobotUnitModules=${RobotUnitModules}")
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
index af8fa62e0e08a2c967c442c3601337a239278d26..5ca733da695eb174ae9b69d0c86abefdbe5df0e1 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -32,6 +32,7 @@
#include "../util.h"
#include "../Constants.h"
#include "../ControlModes.h"
+#include "../util/HeterogenousContinuousContainerMacros.h"
namespace armarx
{
@@ -86,74 +87,48 @@ namespace armarx
virtual std::string getDataFieldAsString(std::size_t i) const = 0;
//management functions
- virtual void _copyTo(ControlTargetBase* target) const = 0;
template<class T, class = typename std::enable_if<std::is_base_of<ControlTargetBase, T>::value>::type>
void _copyTo(std::unique_ptr<T>& target) const
{
_copyTo(target.get());
}
- virtual std::unique_ptr<ControlTargetBase> _clone() const = 0;
-
- virtual std::size_t _sizeInBytes() const = 0;
- virtual std::size_t _alignof() const = 0;
- virtual ControlTargetBase* _placementConstruct(void* place) const = 0;
-
ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
ControlTargetHasGetClassMemberInfo,
GetClassMemberInfo, ControlTargetInfo<T>(*)(void));
+
+ ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(ControlTargetBase)
};
-#define DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
- virtual ControlTargetBase* _placementConstruct(void* place) const override \
- { \
- return new(place) std::decay<decltype(*this)>::type; \
- } \
- virtual std::size_t _sizeInBytes() const override \
- { \
- return sizeof(std::decay<decltype(*this)>::type); \
- } \
- virtual std::size_t _alignof() const override \
- { \
- return alignof(std::decay<decltype(*this)>::type); \
- } \
- virtual void _copyTo(ControlTargetBase* target) const override \
- { \
- const auto targAsThis = target->asA<std::decay<decltype(*this)>::type>(); \
- ARMARX_CHECK_NOT_NULL(targAsThis); \
- *targAsThis = *this; \
- } \
- virtual std::unique_ptr<ControlTargetBase> _clone() const override \
- { \
- std::unique_ptr<ControlTargetBase> c {new std::decay<decltype(*this)>::type}; \
- ControlTargetBase::_copyTo(c); \
- return c; \
- } \
- virtual std::string getControlTargetType(bool withoutNamespaceSpecifier = false) const override \
- { \
- return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
- } \
- void _check_for_static_GetClassMemberInfo_overload() \
- { \
- static_assert(ControlTargetHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
- "This class has to implement GetClassMemberInfo() returning " \
- "an instance of ControlTargetInfo<THIS_CLASS_TYPE>"); \
- } \
- virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp,this); \
- } \
- virtual std::size_t getNumberOfDataFields() const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
- } \
- virtual std::string getDataFieldAsString(std::size_t i) const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAsString(this, i); \
- } \
- virtual std::vector<std::string> getDataFieldNames() const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
+#define DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
+ ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ using ControlTargetBase = ::armarx::ControlTargetBase; \
+ using VariantBasePtr = ::armarx::VariantBasePtr; \
+ std::string getControlTargetType(bool withoutNamespaceSpecifier = false) const override \
+ { \
+ return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
+ } \
+ void _check_for_static_GetClassMemberInfo_overload() \
+ { \
+ static_assert(ControlTargetHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
+ "This class has to implement GetClassMemberInfo() returning " \
+ "an instance of ControlTargetInfo<THIS_CLASS_TYPE>"); \
+ } \
+ std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp,this); \
+ } \
+ std::size_t getNumberOfDataFields() const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
+ } \
+ std::string getDataFieldAsString(std::size_t i) const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAsString(this, i); \
+ } \
+ std::vector<std::string> getDataFieldNames() const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
}
#define make_DummyControlTarget(Suffix,ControlMode) \
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
index 734907c05b46755a12ae06def73b07727e56fa98..b7b0778f647f95c6a14d1e34fd82fedec1a2efe3 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
@@ -27,6 +27,10 @@
namespace armarx
{
+ namespace RobotUnitModule
+ {
+ TYPEDEF_PTRS_HANDLE(ControlThread);
+ }
TYPEDEF_PTRS_SHARED(RTThreadTimingsSensorDevice);
class RTThreadTimingsSensorDevice: virtual public SensorDevice
@@ -42,6 +46,7 @@ namespace armarx
virtual void rtMarkRtBusSendReceiveEnd() = 0;
protected:
friend class RobotUnit;
+ friend class RobotUnitModule::ControlThread;
virtual void rtMarkRtSwitchControllerSetupStart() = 0;
virtual void rtMarkRtSwitchControllerSetupEnd() = 0;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
index b41543c9f389983f01ac52f99c5895e2523020e2..28ade519086d07f71aa03673a41972ebf3919a2e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
@@ -37,19 +37,17 @@ namespace armarx
return "NJointCartesianTorqueController";
}
- NJointCartesianTorqueController::NJointCartesianTorqueController(NJointControllerDescriptionProviderInterfacePtr prov, NJointCartesianTorqueControllerConfigPtr config, const boost::shared_ptr<VirtualRobot::Robot>& r)
+ NJointCartesianTorqueController::NJointCartesianTorqueController(RobotUnitPtr, NJointCartesianTorqueControllerConfigPtr config, const boost::shared_ptr<VirtualRobot::Robot>& r)
{
- ARMARX_CHECK_EXPRESSION(prov);
- RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- ARMARX_CHECK_EXPRESSION(robotUnit);
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(config->nodeSetName);
+ useSynchronizedRtRobot();
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(config->nodeSetName);
ARMARX_CHECK_EXPRESSION_W_HINT(rns, config->nodeSetName);
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
- //const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
+ //const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
/*const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
@@ -66,7 +64,7 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);*/
};
- tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : robotUnit->getRtRobot()->getRobotNode(config->tcpName);
+ tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName);
nodeSetName = config->nodeSetName;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
index fd2e86457ef8f5bf5d15b04a99f53f4b8bb5fe3f..e52f3df118b8924cb281c8163a95acd1d0903a98 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
@@ -73,7 +73,7 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCartesianTorqueControllerConfigPtr;
- NJointCartesianTorqueController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCartesianTorqueController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
@@ -89,7 +89,7 @@ namespace armarx
static NJointCartesianTorqueControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
NJointCartesianTorqueController(
- NJointControllerDescriptionProviderInterfacePtr prov,
+ RobotUnitPtr prov,
NJointCartesianTorqueControllerConfigPtr config,
const boost::shared_ptr<VirtualRobot::Robot>& r);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index 8e97708c59029a5f511e6b044dd8120ca8d4e933..7d2264e65314ffe5023cc98d9855936596d268eb 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
@@ -37,19 +37,17 @@ namespace armarx
return "NJointCartesianVelocityController";
}
- NJointCartesianVelocityController::NJointCartesianVelocityController(NJointControllerDescriptionProviderInterfacePtr prov, NJointCartesianVelocityControllerConfigPtr config, const boost::shared_ptr<VirtualRobot::Robot>& r)
+ NJointCartesianVelocityController::NJointCartesianVelocityController(RobotUnitPtr robotUnit, NJointCartesianVelocityControllerConfigPtr config, const boost::shared_ptr<VirtualRobot::Robot>& r)
{
- ARMARX_CHECK_EXPRESSION(prov);
- RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- ARMARX_CHECK_EXPRESSION(robotUnit);
+ useSynchronizedRtRobot();
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(config->nodeSetName);
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(config->nodeSetName);
ARMARX_CHECK_EXPRESSION_W_HINT(rns, config->nodeSetName);
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
@@ -66,7 +64,7 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);
};
- VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : robotUnit->getRtRobot()->getRobotNode(config->tcpName);
+ VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName);
tcpController.reset(new CartesianVelocityController(rns, tcp));
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index bc7744f4064e6a351f235f99b3eeff1bde445fe1..7c792c309119b66a07e9c13983e75c3e177eac4d 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
@@ -86,7 +86,7 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCartesianVelocityControllerConfigPtr;
- NJointCartesianVelocityController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCartesianVelocityController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
@@ -102,7 +102,7 @@ namespace armarx
static NJointCartesianVelocityControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
NJointCartesianVelocityController(
- NJointControllerDescriptionProviderInterfacePtr prov,
+ RobotUnitPtr prov,
NJointCartesianVelocityControllerConfigPtr config,
const boost::shared_ptr<VirtualRobot::Robot>& r);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index 9b87ed517a71f0a6ae1dad5fc897bbc67559bec6..c2f632008576b93d774acdd6ab22fc1c8a2ed508 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -26,205 +26,295 @@
#include "../RobotUnit.h"
-using namespace armarx;
+#include <atomic>
-const NJointControllerPtr NJointController::NullPtr
+namespace armarx
{
- nullptr
-};
+ namespace RobotUnitModule
+ {
-NJointControllerDescription NJointController::getControllerDescription(const Ice::Current&) const
-{
- NJointControllerDescription d;
- d.className = getClassName();
- ARMARX_CHECK_EXPRESSION(getProxy(-1));
- d.controller = NJointControllerInterfacePrx::uncheckedCast(getProxy(-1));
- d.controlModeAssignment = controlDeviceControlModeMap;
- d.instanceName = getInstanceName();
- d.deletable = deletable;
- d.internal = internal;
- return d;
+ /**
+ * \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
+ {
+ friend class ::armarx::NJointController;
+ static JointController* GetJointController(Devices& d, const std::string &deviceName, const std::string &controlMode)
+ {
+ if(d.controlDevices.has(deviceName))
+ {
+ 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);
+ }
+ }
+ return nullptr;
+ }
+ };
+ }
}
-boost::optional<std::vector<char> > NJointController::isNotInConflictWith(const std::vector<char>& used) const
+
+thread_local bool armarx::NJointControllerRegistryEntry::ConstructorIsRunning_ = false;
+
+namespace armarx
{
- ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
- auto result = used;
- for (std::size_t i = 0; i < controlDeviceUsedBitmap.size(); ++i)
+ const NJointControllerPtr NJointController::NullPtr
+ {
+ nullptr
+ };
+
+ NJointControllerDescription NJointController::getControllerDescription(const Ice::Current&) const
+ {
+ NJointControllerDescription d;
+ d.className = getClassName();
+ ARMARX_CHECK_EXPRESSION(getProxy(-1));
+ d.controller = NJointControllerInterfacePrx::uncheckedCast(getProxy(-1));
+ d.controlModeAssignment = controlDeviceControlModeMap;
+ d.instanceName = getInstanceName();
+ d.deletable = deletable;
+ d.internal = internal;
+ return d;
+ }
+
+ boost::optional<std::vector<char> > NJointController::isNotInConflictWith(const std::vector<char>& used) const
{
- if (controlDeviceUsedBitmap.at(i))
+ ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
+ auto result = used;
+ for (std::size_t i = 0; i < controlDeviceUsedBitmap.size(); ++i)
{
- if (used.at(i))
+ if (controlDeviceUsedBitmap.at(i))
{
- return boost::optional<std::vector<char>>();
+ if (used.at(i))
+ {
+ return boost::optional<std::vector<char>>();
+ }
+ result.at(i) = true;
}
- result.at(i) = true;
- }
+ }
+ return {true, std::move(result)};
}
- return {true, std::move(result)};
-}
-NJointControllerStatus NJointController::getControllerStatus(const Ice::Current&) const
-{
- NJointControllerStatus s;
- s.active = isActive;
- s.instanceName = getInstanceName();
- s.requested = isRequested;
- s.error = deactivatedBecauseOfError;
- s.timestampUSec = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch()).count();
- return s;
-}
+ NJointControllerStatus NJointController::getControllerStatus(const Ice::Current&) const
+ {
+ NJointControllerStatus s;
+ s.active = isActive;
+ s.instanceName = getInstanceName();
+ s.requested = isRequested;
+ s.error = deactivatedBecauseOfError;
+ s.timestampUSec = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch()).count();
+ return s;
+ }
-NJointControllerDescriptionWithStatus NJointController::getControllerDescriptionWithStatus(const Ice::Current&) const
-{
- NJointControllerDescriptionWithStatus ds;
- ds.description = getControllerDescription();
- ds.status = getControllerStatus();
- return ds;
-}
+ NJointControllerDescriptionWithStatus NJointController::getControllerDescriptionWithStatus(const Ice::Current&) const
+ {
+ NJointControllerDescriptionWithStatus ds;
+ ds.description = getControllerDescription();
+ ds.status = getControllerStatus();
+ return ds;
+ }
-RobotUnitInterfacePrx NJointController::getRobotUnit(const Ice::Current&) const
-{
- ARMARX_CHECK_EXPRESSION(robotUnit);
- return RobotUnitInterfacePrx::uncheckedCast(robotUnit->getProxy(-1));
-}
+ RobotUnitInterfacePrx NJointController::getRobotUnit(const Ice::Current&) const
+ {
+ return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
+ }
-void NJointController::activateController(const Ice::Current&) const
-{
- ARMARX_CHECK_EXPRESSION(robotUnit);
- robotUnit->activateNJointController(getInstanceName());
-}
+ void NJointController::activateController(const Ice::Current&) const
+ {
+ robotUnit.activateNJointController(getInstanceName());
+ }
-void NJointController::deactivateController(const Ice::Current&) const
-{
- ARMARX_CHECK_EXPRESSION(robotUnit);
- robotUnit->deactivateNJointController(getInstanceName());
-}
+ void NJointController::deactivateController(const Ice::Current&) const
+ {
+ robotUnit.deactivateNJointController(getInstanceName());
+ }
-void NJointController::deleteController(const Ice::Current&) const
-{
- ARMARX_CHECK_EXPRESSION(robotUnit);
- robotUnit->deleteNJointController(getInstanceName());
-}
+ void NJointController::deleteController(const Ice::Current&) const
+ {
+ robotUnit.deleteNJointController(getInstanceName());
+ }
-void NJointController::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
-{
- const bool active = isActive;
- if (publishActive.exchange(active) != active)
+ void NJointController::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
{
- if (active)
+ const bool active = isActive;
+ if (publishActive.exchange(active) != active)
{
- ARMARX_DEBUG << "activating publishing for " << getInstanceName();
- try
+ if (active)
{
- onPublishActivation(draw, observer);
+ ARMARX_DEBUG << "activating publishing for " << getInstanceName();
+ try
+ {
+ onPublishActivation(draw, observer);
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_WARNING << "onPublishActivation of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "onPublishActivation of '" << getInstanceName() << "' threw an exception!";
+ }
}
- catch (std::exception& e)
- {
- ARMARX_WARNING << "onPublishActivation of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
- }
- catch (...)
+ else
{
- ARMARX_WARNING << "onPublishActivation of '" << getInstanceName() << "' threw an exception!";
+ ARMARX_DEBUG << "deactivating publishing for " << getInstanceName();
+ try
+ {
+ onPublishDeactivation(draw, observer);
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName() << "' threw an exception!";
+ }
}
}
- else
+ if (active)
{
- ARMARX_DEBUG << "deactivating publishing for " << getInstanceName();
try
{
- onPublishDeactivation(draw, observer);
+ onPublish(sac, draw, observer);
}
catch (std::exception& e)
{
- ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ ARMARX_WARNING << "onPublish of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
}
catch (...)
{
- ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName() << "' threw an exception!";
+ ARMARX_WARNING << "onPublish of '" << getInstanceName() << "' threw an exception!";
}
}
}
- if (active)
+
+ void NJointController::deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
{
- try
+ if (publishActive.exchange(false))
{
- onPublish(sac, draw, observer);
+ ARMARX_DEBUG << "forced deactivation of publishing for " << getInstanceName();
+ onPublishDeactivation(draw, observer);
}
- catch (std::exception& e)
+ }
+
+ void NJointController::rtActivateController()
+ {
+ if (!isActive)
{
- ARMARX_WARNING << "onPublish of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ deactivatedBecauseOfError = false;
+ rtPreActivateController();
+ isActive = true;
}
- catch (...)
+ }
+
+ void NJointController::rtDeactivateController()
+ {
+ if (isActive)
{
- ARMARX_WARNING << "onPublish of '" << getInstanceName() << "' threw an exception!";
+ isActive = false;
+ rtPostDeactivateController();
}
}
-}
-void NJointController::deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
-{
- if (publishActive.exchange(false))
+ void NJointController::rtDeactivateControllerBecauseOfError()
{
- ARMARX_DEBUG << "forced deactivation of publishing for " << getInstanceName();
- onPublishDeactivation(draw, observer);
+ deactivatedBecauseOfError = true;
+ rtDeactivateController();
}
-}
-void NJointController::rtActivateController()
-{
- if (!isActive)
+ armarx::ConstSensorDevicePtr armarx::NJointController::peekSensorDevice(const std::string &deviceName) const
{
- deactivatedBecauseOfError = false;
- rtPreActivateController();
- isActive = true;
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ NJointControllerRegistryEntry::ConstructorIsRunning(),
+ "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."
+ );
+ return RobotUnitModule::Devices::Instance().getSensorDevice(deviceName);
}
-}
-void NJointController::rtDeactivateController()
-{
- if (isActive)
+ ConstControlDevicePtr NJointController::peekControlDevice(const std::string &deviceName) const
{
- isActive = false;
- rtPostDeactivateController();
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ NJointControllerRegistryEntry::ConstructorIsRunning(),
+ "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."
+ );
+ return RobotUnitModule::Devices::Instance().getControlDevice(deviceName);
}
-}
-void NJointController::rtDeactivateControllerBecauseOfError()
-{
- deactivatedBecauseOfError = true;
- rtDeactivateController();
-}
+ void NJointController::useSynchronizedRtRobot(bool updateCollisionModel)
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ NJointControllerRegistryEntry::ConstructorIsRunning(),
+ "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."
+ );
+ ARMARX_CHECK_IS_NULL_W_HINT(rtRobot, "useSynchronizedRtRobot was already called");
+ rtRobot = RobotUnitModule::RobotData::Instance().cloneRobot(updateCollisionModel);
+ rtRobotNodes = rtRobot->getRobotNodes();
+ }
-void NJointController::robotUnitInit(
- RobotUnit* ru,
- std::size_t ctrlId,
- std::map<std::string, const JointController*> ctrlDeviceUsedJointController,
- std::vector<char> ctrlDeviceUsedBitmap,
- std::vector<std::size_t> ctrlDeviceUsedIndices,
- bool ctrlDeletable,
- bool ctrlInternal)
-{
- ARMARX_CHECK_EXPRESSION(ru);
- ARMARX_CHECK_EXPRESSION_W_HINT(ctrlDeviceUsedJointController.size(), "Controller requested zero joints");
- ARMARX_CHECK_EXPRESSION(ctrlId != std::numeric_limits<std::size_t>::max());
- ARMARX_CHECK_EXPRESSION_W_HINT(ru->getNumberOfControlDevices() == ctrlDeviceUsedBitmap.size(),
- "ru->getNumberOfControlDevices() == ctrlDeviceUsedBitmap.size() ->" +
- to_string(ru->getNumberOfControlDevices()) + " > " + to_string(ctrlDeviceUsedBitmap.size()));
- ARMARX_CHECK_EXPRESSION(ctrlDeviceUsedJointController.size() == ctrlDeviceUsedIndices.size());
-
- robotUnit = ru;
- id = ctrlId;
- controlDeviceUsedJointController = std::move(ctrlDeviceUsedJointController);
- for (const auto& pair : controlDeviceUsedJointController)
- {
- ARMARX_CHECK_EXPRESSION(pair.second);
- controlDeviceControlModeMap[pair.first] = pair.second->getControlMode();
- }
- controlDeviceUsedBitmap = std::move(ctrlDeviceUsedBitmap);
- controlDeviceUsedIndices = std::move(ctrlDeviceUsedIndices);
- deletable = ctrlDeletable;
- internal = ctrlInternal;
-}
+ const SensorValueBase* NJointController::useSensorValue(const std::string &deviceName) const
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ NJointControllerRegistryEntry::ConstructorIsRunning(),
+ "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."
+ );
+ auto dev = peekSensorDevice(deviceName);
+ if(!dev)
+ {
+ ARMARX_DEBUG << "No sensor device for " << deviceName;
+ return nullptr;
+ }
+ return dev->getSensorValue();
+ }
+
+ NJointController::NJointController() :
+ robotUnit(RobotUnitModule::ModuleBase::Instance< ::armarx::RobotUnit>()),
+ controlDeviceUsedBitmap(robotUnit.getNumberOfControlDevices(), 0)
+ {
+ controlDeviceUsedIndices.reserve(robotUnit.getNumberOfControlDevices());
+ }
+ ControlTargetBase *NJointController::useControlTarget(const std::string &deviceName, const std::string &controlMode)
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ NJointControllerRegistryEntry::ConstructorIsRunning(),
+ "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."
+ );
+ ARMARX_CHECK_EQUAL_W_HINT(
+ controlDeviceControlModeMap.count(deviceName),0,
+ BOOST_CURRENT_FUNCTION << ": Must not request two ControlTargets for the same device. (got = "
+ << controlDeviceControlModeMap.at(deviceName) << ", requested " + controlMode + ") "
+ << "(You can only have a ControlDevice in one ControlMode. "
+ << "To check all available ControlModes for this device, get the ControlDevice via peekControlDevice(" + deviceName + ") and querry it)"
+ );
+
+ //there is a device and a target was requested:
+ JointController* const jointCtrl = RobotUnitModule::DevicesAttorneyForNJointController::GetJointController(RobotUnitModule::Devices::Instance(), deviceName, controlMode);
+ if (!jointCtrl)
+ {
+ return nullptr;
+ }
+
+ //update meta data structured
+ const std::size_t devIndex = robotUnit.getControlDeviceIndex(deviceName);
+ controlDeviceUsedJointController[deviceName] = jointCtrl;
+
+ controlDeviceControlModeMap[deviceName] = controlMode;
+
+ ARMARX_CHECK_EQUAL(0, controlDeviceUsedBitmap.at(devIndex));
+ controlDeviceUsedBitmap.at(devIndex) = 1;
+ //we want this vector to be sorted
+ controlDeviceUsedIndices.insert
+ (
+ std::upper_bound( controlDeviceUsedIndices.begin(), controlDeviceUsedIndices.end(), devIndex),
+ devIndex
+ );
+
+ return jointCtrl->getControlTarget();
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index b9fbd1980f0097db0d22db4b74fbfd32f11c28aa..ce5845ae37aeac6f58c0f1ecd990a52db8a1af4f 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -54,32 +54,27 @@
namespace VirtualRobot
{
class Robot;
+ class RobotNode;
+ typedef boost::shared_ptr<RobotNode> RobotNodePtr;
+ typedef boost::shared_ptr<Robot> RobotPtr;
}
namespace armarx
{
- TYPEDEF_PTRS_HANDLE(NJointController);
- TYPEDEF_PTRS_HANDLE(NJointControllerDescriptionProviderInterface);
-
- class RobotUnit;
-
- class NJointControllerDescriptionProviderInterface: virtual public Ice::Object
+ namespace RobotUnitModule
{
- public:
- ~NJointControllerDescriptionProviderInterface() override = default;
-
- virtual ConstSensorDevicePtr getSensorDevice(const std::string& sensorDeviceName) const = 0;
- virtual const SensorValueBase* getSensorValue(const std::string& sensorDeviceName) const;
- virtual ConstControlDevicePtr getControlDevice(const std::string& deviceName) const = 0;
- virtual ControlTargetBase* getControlTarget(const std::string& deviceName, const std::string& controlMode) = 0;
+ class NJointControllerAttorneyForPublisher;
+ class NJointControllerAttorneyForControlThread;
+ class NJointControllerAttorneyForControllerManagement;
+ }
+ namespace detail
+ {
+ template<class> class NJointControllerRegistryEntryHelperBase;
+ }
- virtual std::string getName() const = 0;
+ TYPEDEF_PTRS_HANDLE(NJointController);
- template<class T>
- const T* getSensorValue(const std::string& sensorDeviceName) const;
- template<class T>
- T* getControlTarget(const std::string& deviceName, const std::string& controlMode);
- };
+ TYPEDEF_PTRS_HANDLE(RobotUnit);
/**
* @defgroup Library-RobotUnit RobotUnit
@@ -158,6 +153,51 @@ namespace armarx
virtual public NJointControllerInterface,
virtual public ManagedIceObject
{
+ //firends
+ /**
+ * \brief This class allows minimal access to private members of \ref NJointController 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.
+ * \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.
+ * \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;
+ /**
+ * \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;
+ public:
+
+ ConstSensorDevicePtr peekSensorDevice(const std::string& deviceName) const;
+ ConstControlDevicePtr peekControlDevice(const std::string& deviceName) const;
+
+ ControlTargetBase* useControlTarget(const std::string& deviceName, const std::string& controlMode);
+ template<class T>
+ T* 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;
+ }
+ const SensorValueBase* useSensorValue(const std::string& sensorDeviceName) const;
+ template<class T>
+ const T* 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;
+ }
+
+ void useSynchronizedRtRobot(bool updateCollisionModel = false);
+
protected:
std::string getDefaultName() const override;
void onInitComponent() override;
@@ -189,6 +229,8 @@ namespace armarx
static const NJointControllerPtr NullPtr;
+ NJointController();
+
~NJointController() override = default;
//ice interface (must not be called in the rt thread)
bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final override;
@@ -211,11 +253,22 @@ namespace armarx
WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = GlobalIceCurrent) const override;
void callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current& = GlobalIceCurrent) override;
- std::size_t getId() const;
+// std::size_t getId() const;
//c++ interface (local calls) (must be called in the rt thread)
virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ const VirtualRobot::RobotPtr& rtGetRobot()
+ {
+ return rtRobot;
+ }
+
+ const std::vector<VirtualRobot::RobotNodePtr>& rtGetRobotNodes()
+ {
+ return rtRobotNodes;
+ }
+
protected:
/**
* @brief This function is called before the controller is activated.
@@ -230,7 +283,7 @@ namespace armarx
public:
bool rtUsesControlDevice(std::size_t deviceIndex) const;
- std::size_t rtGetId() const;
+// std::size_t rtGetId() const;
//used control devices
StringStringDictionary getControlDeviceUsedControlModeMap(const Ice::Current& = GlobalIceCurrent) const final override;
@@ -259,19 +312,8 @@ namespace armarx
void rtDeactivateController();
void rtDeactivateControllerBecauseOfError();
- //firends
- friend class RobotUnitNJointControllerAccess;
-
- void robotUnitInit(RobotUnit* ru,
- std::size_t ctrlId,
- std::map<std::string, const JointController*> ctrlDeviceControlModeMap,
- std::vector<char> ctrlDeviceUsedBitmap,
- std::vector<std::size_t> ctrlDeviceUsedIndices,
- bool ctrlDeletable,
- bool ctrlInternal);
-
- RobotUnit* robotUnit;
- std::size_t id = std::numeric_limits<std::size_t>::max();
+ RobotUnit& robotUnit;
+// const std::size_t id;
StringStringDictionary controlDeviceControlModeMap;
std::map<std::string, const JointController*> controlDeviceUsedJointController;
std::vector<char> controlDeviceUsedBitmap;
@@ -289,22 +331,38 @@ namespace armarx
std::atomic_bool statusReportedActive {false};
std::atomic_bool statusReportedRequested {false};
- public:
- struct IdComp
- {
- template<class PtrT1, class PtrT2>
- bool operator()(PtrT1 l, PtrT2 r) const;
- };
+ VirtualRobot::RobotPtr rtRobot;
+ std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
};
+}
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ class ControllerManagement;
+ }
class NJointControllerRegistryEntry
{
private:
- friend class RobotUnit;
- virtual NJointControllerPtr create(NJointControllerDescriptionProviderInterfacePtr, const NJointControllerConfigPtr&, const boost::shared_ptr<VirtualRobot::Robot>&) const = 0;
+ friend class RobotUnitModule::ControllerManagement;
+ virtual NJointControllerPtr create(
+ RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr&,
+ const boost::shared_ptr<VirtualRobot::Robot>&,
+ bool deletable,
+ bool internal) const = 0;
virtual WidgetDescription::WidgetPtr GenerateConfigDescription(const boost::shared_ptr<VirtualRobot::Robot>&, const std::map<std::string, ConstControlDevicePtr>&, const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
virtual NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
virtual bool hasRemoteConfiguration() const = 0;
+ protected:
+ static thread_local bool ConstructorIsRunning_;
+ public:
+ static bool ConstructorIsRunning()
+ {
+ return ConstructorIsRunning_;
+ }
};
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
@@ -348,31 +406,6 @@ namespace armarx
//inline functions
namespace armarx
{
- //NJointControllerDescriptionProviderInterface
- inline const SensorValueBase* NJointControllerDescriptionProviderInterface::getSensorValue(const std::string& sensorDeviceName) const
- {
- auto sd = getSensorDevice(sensorDeviceName);
- return sd ? sd->getSensorValue() : nullptr;
- }
-
- template<class T>
- inline const T* NJointControllerDescriptionProviderInterface::getSensorValue(const std::string& sensorDeviceName) const
- {
- return dynamic_cast<const T*>(getSensorValue(sensorDeviceName));
- }
-
- template<class T>
- inline T* NJointControllerDescriptionProviderInterface::getControlTarget(const std::string& deviceName, const std::string& controlMode)
- {
- return dynamic_cast<T*>(getControlTarget(deviceName, controlMode));
- }
-
- //NJointController
- inline std::size_t NJointController::getId() const
- {
- return id;
- }
-
inline void NJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
@@ -383,11 +416,6 @@ namespace armarx
return controlDeviceUsedBitmap.at(deviceIndex);
}
- inline std::size_t NJointController::rtGetId() const
- {
- return id;
- }
-
inline StringStringDictionary NJointController::getControlDeviceUsedControlModeMap(const Ice::Current&) const
{
return controlDeviceControlModeMap;
@@ -465,21 +493,6 @@ namespace armarx
{
}
- template<class PtrT1, class PtrT2>
- inline bool NJointController::IdComp::operator()(PtrT1 l, PtrT2 r) const
- {
- //null is bigger than any other
- if (!l)
- {
- return false;
- }
- if (!r)
- {
- return true;
- }
- return l->id < r->id;
- }
-
//NJointControllerWithTripleBuffer<ControlDataStruct>
template <typename ControlDataStruct>
void NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
@@ -532,4 +545,4 @@ namespace armarx
controlDataTripleBuffer.reinitAllBuffers(initial);
}
}
-#include "NJointController.hpp"
+#include "NJointController.ipp"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.hpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
similarity index 80%
rename from source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.hpp
rename to source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
index f1579a7483a6ba25dfb6875832ea0614c2492565..41cce1faf33e0395e450c19572ed8bfbcd253956 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.hpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
@@ -23,8 +23,13 @@
#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
{
@@ -52,20 +57,39 @@ namespace armarx
namespace detail
{
- template<class NJointControllerT, class = void, class = void>
- class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
+ template<class NJointControllerT>
+ class NJointControllerRegistryEntryHelperBase : public NJointControllerRegistryEntry
{
- NJointControllerPtr create(NJointControllerDescriptionProviderInterfacePtr prov,
- const NJointControllerConfigPtr& config,
- const boost::shared_ptr<VirtualRobot::Robot>& rob) const final override
+ NJointControllerPtr create(RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr& config,
+ const boost::shared_ptr<VirtualRobot::Robot>& rob,
+ bool deletable,
+ bool internal) const final override
{
- ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider 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>());
- return new NJointControllerT(prov, cfg, rob);
+ 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>());
+
+ 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;
+ return ptr;
}
+ };
+
+
+ template<class NJointControllerT, class = void, class = void>
+ class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntryHelperBase<NJointControllerT>
+ {
WidgetDescription::WidgetPtr GenerateConfigDescription(const boost::shared_ptr<VirtualRobot::Robot>&,
const std::map<std::string, ConstControlDevicePtr>&,
const std::map<std::string, ConstSensorDevicePtr>&) const final override
@@ -95,23 +119,10 @@ namespace armarx
template<class NJointControllerT>
class NJointControllerRegistryEntryHelper <
NJointControllerT,
- // meta::void_t<decltype(NJointControllerT::GenerateConfigDescription)>,
typename std::enable_if < hasGenerateConfigDescription<NJointControllerT>::value >::type,
- // meta::void_t<decltype(NJointControllerT::GenerateConfigFromVariants)
typename std::enable_if < hasGenerateConfigFromVariants<NJointControllerT>::value >::type
- > : public NJointControllerRegistryEntry
+ > : public NJointControllerRegistryEntryHelperBase<NJointControllerT>
{
- NJointControllerPtr create(NJointControllerDescriptionProviderInterfacePtr prov,
- const NJointControllerConfigPtr& config,
- const boost::shared_ptr<VirtualRobot::Robot>& rob) const final override
- {
- ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider 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>());
- return new NJointControllerT(prov, cfg, rob);
- }
WidgetDescription::WidgetPtr GenerateConfigDescription(
const boost::shared_ptr<VirtualRobot::Robot>& robot,
const std::map<std::string, ConstControlDevicePtr>& controlDevices,
@@ -191,4 +202,6 @@ namespace armarx
}
};
}
+
+
#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index f5bc30a8521b64942a5d8940328531f8b1b88905..148090ede09cbaf44fe80ccc2c581f6bc50b9668 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -27,11 +27,11 @@
using namespace armarx;
NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatformUnitVelocityPassThroughController(
- NJointControllerDescriptionProviderInterfacePtr prov,
+ RobotUnitPtr prov,
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr
cfg, const VirtualRobot::RobotPtr&)
{
- target = prov->getControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity);
initialSettings.velocityX = cfg->initialVelocityX;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index 8cc71e9189a91e84d3f6a2965a129932e0cff580..313f95abefef29bc3f020c3bd1a6a9b965b3976e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -58,7 +58,7 @@ namespace armarx
using ConfigPtrT = NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr;
NJointHolonomicPlatformUnitVelocityPassThroughController(
- NJointControllerDescriptionProviderInterfacePtr prov,
+ RobotUnitPtr prov,
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config,
const VirtualRobot::RobotPtr&);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index 02da648c7dffcee50611d9928ce6478a9b0ac34d..61e768884245fc6a1601b3a867d160d2504037db 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -26,11 +26,11 @@ using namespace armarx;
NJointControllerRegistration<NJointKinematicUnitPassThroughController> registrationControllerNJointKinematicUnitPassThroughController("NJointKinematicUnitPassThroughController");
NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughController(
- NJointControllerDescriptionProviderInterfacePtr prov,
+ RobotUnitPtr prov,
const NJointKinematicUnitPassThroughControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&)
{
- const SensorValueBase* sv = prov->getSensorValue(cfg->deviceName);
+ const SensorValueBase* sv = useSensorValue(cfg->deviceName);
std::string controlMode = cfg->controlMode;
std::string deviceName = cfg->deviceName;
@@ -41,7 +41,7 @@ NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughControll
controlMode = ControlModes::VelocityTorque;
}*/
- ControlTargetBase* ct = prov->getControlTarget(cfg->deviceName, controlMode);
+ ControlTargetBase* ct = useControlTarget(cfg->deviceName, controlMode);
//get sensor
if (controlMode == ControlModes::Position1DoF)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index 6936d394c847beb6f64409914a9471820bb0ff41..3119a0fc23a89029515d6829087798f358ed49b5 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -53,7 +53,7 @@ namespace armarx
using ConfigPtrT = NJointKinematicUnitPassThroughControllerConfigPtr;
inline NJointKinematicUnitPassThroughController(
- NJointControllerDescriptionProviderInterfacePtr prov,
+ RobotUnitPtr prov,
const NJointKinematicUnitPassThroughControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index d2df87384c5dac6572a462f8ab10b87e892b08c9..6350540cb8fce388944f31dc2b94359bd2a7d657 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -154,24 +154,23 @@ namespace armarx
return new NJointTCPControllerConfig {values.at("nodeSetName")->getString(), values.at("tcpName")->getString()};
}
- NJointTCPController::NJointTCPController(NJointControllerDescriptionProviderInterfacePtr prov, NJointTCPControllerConfigPtr config, const boost::shared_ptr<VirtualRobot::Robot>& r)
+ NJointTCPController::NJointTCPController(RobotUnitPtr robotUnit, NJointTCPControllerConfigPtr config, const boost::shared_ptr<VirtualRobot::Robot>& r)
{
- ARMARX_CHECK_EXPRESSION(prov);
- RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
ARMARX_CHECK_EXPRESSION(robotUnit);
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
- auto nodeset = robotUnit->getRtRobot()->getRobotNodeSet(config->nodeSetName);
+ useSynchronizedRtRobot();
+ auto nodeset = rtGetRobot()->getRobotNodeSet(config->nodeSetName);
ARMARX_CHECK_EXPRESSION_W_HINT(nodeset, config->nodeSetName);
for (size_t i = 0; i < nodeset->getSize(); ++i)
{
auto jointName = nodeset->getNode(i)->getName();
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
};
- ik.reset(new VirtualRobot::DifferentialIK(nodeset, robotUnit->getRtRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? nodeset->getTCP() : robotUnit->getRtRobot()->getRobotNode(config->tcpName);
+ ik.reset(new VirtualRobot::DifferentialIK(nodeset, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? nodeset->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName);
nodeSetName = config->nodeSetName;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index 46eff8bfb2003a4ed9216c86a57c23faaf7a2367..d0fadce61ccc38c35cf307dbd6b8d30dfe3227f7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -68,7 +68,7 @@ namespace armarx
{
public:
using ConfigPtrT = NJointTCPControllerConfigPtr;
- NJointTCPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTCPController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
@@ -84,7 +84,7 @@ namespace armarx
static NJointTCPControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
NJointTCPController(
- NJointControllerDescriptionProviderInterfacePtr prov,
+ RobotUnitPtr prov,
NJointTCPControllerConfigPtr config,
const boost::shared_ptr<VirtualRobot::Robot>& r);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index c988cec09e2d59ad93e63eb38dde3d7eeb3d5306..9071e66b2e4cb6b01d6cce20bff7dd2a3ff0db19 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -7,15 +7,15 @@ namespace armarx
{
NJointControllerRegistration<NJointTrajectoryController> registrationControllerNJointTrajectoryController("NJointTrajectoryController");
- NJointTrajectoryController::NJointTrajectoryController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
+ NJointTrajectoryController::NJointTrajectoryController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
{
cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointTrajectoryControllerConfigPtr");
for (std::string jointName : cfg->jointNames)
{
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
sensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index 5de238702fdc2b06dc40b84d6a9315a0e0b1fc9b..755428ec0b82b271338bf7c8ba3c9dedb0e41eb2 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -24,7 +24,7 @@ namespace armarx
public NJointTrajectoryControllerInterface
{
public:
- NJointTrajectoryController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTrajectoryController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.cpp b/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.cpp
deleted file mode 100644
index 5f8857e1a34184fd3617fdd97065299aa96a6728..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::RobotUnit::RobotSynchronization
- * @author Stefan Reither ( stef dot reither at web dot de )
- * @date 2017
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "RobotSynchronization.h"
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "SensorValues/SensorValue1DoFActuator.h"
-
-#include "RobotUnit.h"
-
-using namespace armarx;
-
-RobotSynchronization::RobotSynchronization(RobotUnit* robotUnit, VirtualRobot::RobotPtr& robot, const std::vector<std::string>& synchronizingJoints)
-{
- this->robotUnit = robotUnit;
- this->robot = robot;
- Ice::StringSeq names = robotUnit->getSensorDeviceNames(GlobalIceCurrent);
-
- for (std::string name : names)
- {
- // skip, if the current sensor device does not correspond to a joint.
- if (!this->robot->hasRobotNode(name))
- {
- continue;
- }
-
- ConstSensorDevicePtr device = robotUnit->getSensorDevice(name);
-
- // skip, if the sensor value of the current sensor device can't provide a position.
- if (!device->getSensorValue()->isA<SensorValue1DoFActuatorPosition>())
- {
- continue;
- }
-
- std::vector<std::string>::const_iterator it = std::find(synchronizingJoints.begin(), synchronizingJoints.end(), name);
- if (it == synchronizingJoints.end())
- {
- continue;
- }
- else
- {
- jointSensorDeviceMap[*it] = device;
- }
- }
-}
-
-void RobotSynchronization::sync()
-{
- if (robotUnit->getRobotUnitState() == RobotUnitState::InitializingUnits || robotUnit->getRobotUnitState() == RobotUnitState::Running)
- {
- for (auto const& entry : jointSensorDeviceMap)
- {
- const SensorValueBase* sv = entry.second->getSensorValue();
-
- ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
- robot->getRobotNode(entry.first)->setJointValueNoUpdate(sv->asA<SensorValue1DoFActuatorPosition>()->position);
- }
- robot->applyJointValues();
- }
-}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.h b/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.h
deleted file mode 100644
index 2b65c5f57414e5ef8ba15d1d8ed2230325814090..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::RobotUnit::RobotSynchronization
- * @author Stefan Reither ( stef dot reither at web dot de )
- * @date 2017
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-#include "Devices/SensorDevice.h"
-
-#include <string>
-#include <vector>
-
-namespace armarx
-{
- class RobotUnit;
- class SensorValue1DoFActuatorPosition;
-
- TYPEDEF_PTRS_SHARED(RobotSynchronization);
- /**
- * @brief The RobotSynchronization class
- */
- class RobotSynchronization
- {
- public:
- RobotSynchronization(RobotUnit* robotUnit, VirtualRobot::RobotPtr& robot, const std::vector<std::string>& synchronizingJoints);
- ~RobotSynchronization() {}
- void sync();
-
- private:
- std::map<std::string, ConstSensorDevicePtr> jointSensorDeviceMap;
- RobotUnit* robotUnit = NULL;
- VirtualRobot::RobotPtr robot;
- };
-}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 5791b8bcfacce0fe292dfd0778b6ac9610f27bc5..e3ad39c38ffba9e7241da8ed69b85a8028ca7dc9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -22,3276 +22,6 @@
#include "RobotUnit.h"
-#include <algorithm>
-
-#include <boost/algorithm/string/predicate.hpp>
-
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Visualization/VisualizationNode.h>
-#include <VirtualRobot/Obstacle.h>
-#include <VirtualRobot/SceneObjectSet.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <ArmarXCore/core/IceManager.h>
-#include <ArmarXCore/core/IceGridAdmin.h>
-#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-
-#include "Units/ForceTorqueSubUnit.h"
-#include "Units/InertialMeasurementSubUnit.h"
-#include "Units/KinematicSubUnit.h"
-#include "Units/PlatformSubUnit.h"
-#include "SensorValues/SensorValue1DoFActuator.h"
-#include "Units/TCPControllerSubUnit.h"
-#include "Units/TrajectoryControllerSubUnit.h"
-
-namespace std
-{
- std::string to_string(armarx::RobotUnitState s)
- {
- switch (s)
- {
- case armarx::RobotUnitState::PreComponentInitialization:
- return "PreComponentInitialization";
- case armarx::RobotUnitState::InitializingDevices:
- return "InitializingDevices";
- case armarx::RobotUnitState::InitializingUnits:
- return "InitializingUnits";
- case armarx::RobotUnitState::WaitingForRTThreadInitialization:
- return "WaitingForRTThreadInitialization";
- case armarx::RobotUnitState::Running:
- return "Running";
- case armarx::RobotUnitState::Exiting:
- return "Exiting";
- }
- throw std::invalid_argument {"Unknown state " + std::to_string(static_cast<std::size_t>(s))};
- }
-}
-
namespace armarx
{
- class RobotUnitEmergencyStopMaster :
- virtual public ManagedIceObject,
- virtual public EmergencyStopMasterInterface
- {
- public:
- RobotUnitEmergencyStopMaster(RobotUnit* robotUnit, std::string emergencyStopTopicName) :
- robotUnit {robotUnit},
- emergencyStopTopicName {emergencyStopTopicName}
- {}
-
- void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = GlobalIceCurrent) final override
- {
- robotUnit->setEmergencyStopState(state);
- emergencyStopTopic->reportEmergencyStopState(state);
- }
- EmergencyStopState getEmergencyStopState(const Ice::Current& = GlobalIceCurrent) const final override
- {
- return robotUnit->getEmergencyStopState();
- }
-
- 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";
- }
-
- protected:
- RobotUnit* const robotUnit;
- const std::string emergencyStopTopicName;
- EmergencyStopListenerPrx emergencyStopTopic;
- };
-
- namespace detail
- {
- std::function<std::string(const std::string&)> makeDebugObserverNameFixer(const std::string& prefix)
- {
- return
- [prefix](const std::string & name)
- {
- std::string s = prefix + name;
- std::replace(s.begin(), s.end(), '.', '_');
- std::replace(s.begin(), s.end(), ':', '_');
- return s;
- };
- }
- }
-
- const std::string armarx::RobotUnit::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
-
- const std::set<RobotUnitState> armarx::RobotUnit::devicesReadyStates
- {
- RobotUnitState::InitializingUnits,
- RobotUnitState::WaitingForRTThreadInitialization,
- RobotUnitState::Running
- };
- armarx::RobotUnit::GuardType armarx::RobotUnit::getGuard() const
- {
- if (std::this_thread::get_id() == rtThreadId)
- {
- throw std::logic_error {"Attempted to lock mutex in RT thread"};
- }
- return GuardType {dataMutex};
- }
-
- void armarx::RobotUnit::checkNJointControllerClassName(const std::string& className) const
- {
- 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()};
- }
- }
-
- void armarx::RobotUnit::addControlDevice(const ControlDevicePtr& cd)
- {
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
- controlDevices.add(cd->getDeviceName(), cd);
- ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
- }
-
- void armarx::RobotUnit::addSensorDevice(const SensorDevicePtr& sd)
- {
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
- 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()};
- }
- if (sd->getDeviceName() == 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)
- sensorDevices.add(sd->getDeviceName(), sd);
- rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
- }
- else
- {
- sensorDevices.add(sd->getDeviceName(), sd);
- }
- ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";
- }
-
- void armarx::RobotUnit::finishDeviceInitialization()
- {
- auto beg = MicroNow();
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
- if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
- {
- addSensorDevice(createRTThreadTimingSensorDevice());
- }
- state = RobotUnitState::InitializingUnits;
- //check added devices
- ARMARX_DEBUG << "checking ControlDevices:";
- for (const ControlDevicePtr& controlDevice : controlDevices.values())
- {
- ARMARX_CHECK_EXPRESSION(controlDevice);
- ARMARX_DEBUG << "----" << controlDevice->getDeviceName();
- if (!controlDevice->hasJointController(ControlModes::EmergencyStop))
- {
- 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 (!controlDevice->hasJointController(ControlModes::StopMovement))
- {
- 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()};
- }
- }
- ARMARX_DEBUG << "checking sensor devices:";
- for (const SensorDevicePtr& sensorDevice : sensorDevices.values())
- {
- ARMARX_CHECK_EXPRESSION(sensorDevice);
- ARMARX_DEBUG << "----" << sensorDevice->getDeviceName();
- if (!sensorDevice->getSensorValue())
- {
- std::stringstream s;
- s << "SensorDevice " << sensorDevice->getSensorValue() << " has null SensorValue";
- ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
- }
- ARMARX_CHECK_EXPRESSION(sensorDevice);
- }
-
- ARMARX_DEBUG << "initializing buffers:";
- {
- ARMARX_DEBUG << "----initializing controller buffers";
- controllersActivated.reinitAllBuffers(JointAndNJointControllers {getNumberOfControlDevices()});
- JointAndNJointControllers ctrlinitReq(getNumberOfControlDevices());
- for (std::size_t i = 0 ; i < getNumberOfControlDevices(); ++i)
- {
- ctrlinitReq.jointControllers.at(i) = controlDevices.at(i)->rtGetJointStopMovementController();
- }
- controllersRequested.reinitAllBuffers(ctrlinitReq);
- controllersRequested.commitWrite();//to make sure the diry bit is activated (to trigger rt switch)
- }
- 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;
- const SensorValue1DoFActuatorPosition* sv = dev->getSensorValue()->asA<SensorValue1DoFActuatorPosition>();
- if (sv && rtRobot->hasRobotNode(dev->getDeviceName()))
- {
- nodePositionSensorMap[rtRobot->getRobotNode(dev->getDeviceName())] = sv;
- }
- }
- }
-
- ARMARX_VERBOSE << "ControlDevices:\n" << controlDevices.keys();
- ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
- ctrlModeGroups.groupIndices.assign(getNumberOfControlDevices(), IndexSentinel());
-
- if (!ctrlModeGroups.groupsMerged.empty())
- {
- ARMARX_DEBUG << "Remove control devs from ControlDeviceHardwareControlModeGroups ("
- << ctrlModeGroups.groups.size() << ")";
- const auto groupsMerged = ctrlModeGroups.groupsMerged;
- for (const auto& dev : groupsMerged)
- {
- if (controlDevices.has(dev))
- {
- continue;
- }
- ctrlModeGroups.groupsMerged.erase(dev);
- for (auto& group : ctrlModeGroups.groups)
- {
- group.erase(dev);
- }
- 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)
- {
- 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_INFO << "finishDeviceInitialization...done! " << (MicroNow() - beg).count() << " us";
- }
-
- void armarx::RobotUnit::initializeDefaultUnits()
- {
- auto beg = MicroNow();
- {
- 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! " << (MicroNow() - beg).count() << " us";
- }
-
- void armarx::RobotUnit::initializeKinematicUnit()
- {
- using UnitT = KinematicSubUnit;
- using IfaceT = KinematicUnitInterface;
-
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- //check if unit is already added
- if (getUnit(IfaceT::ice_staticId()))
- {
- return;
- }
- //add ctrl et al
- std::map<std::string, UnitT::ActuatorData> devs;
- for (const ControlDevicePtr& controlDevice : controlDevices.values())
- {
- ARMARX_CHECK_EXPRESSION(controlDevice);
- const auto& controlDeviceName = controlDevice->getDeviceName();
- if (!controlDevice->hasTag(ControlDeviceTags::CreateNoDefaultController))
- {
- UnitT::ActuatorData ad;
- ad.name = controlDeviceName;
- ad.sensorDeviceIndex =
- sensorDevices.has(controlDeviceName) ?
- sensorDevices.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; \
- NJointKinematicUnitPassThroughControllerPtr& pt = ctrl; \
- JointController* jointCtrl = controlDevice->getJointController(controlMode); \
- if (jointCtrl && jointCtrl->getControlTarget()->isA<CtargT>()) \
- { \
- NJointKinematicUnitPassThroughControllerConfigPtr config = new NJointKinematicUnitPassThroughControllerConfig; \
- config->controlMode=controlMode; \
- config->deviceName=controlDeviceName; \
- const NJointControllerPtr& nJointCtrl = createNJointController( \
- "NJointKinematicUnitPassThroughController", \
- "NJointKU_PTCtrl_"+controlDeviceName+"_"+controlMode, \
- config, \
- false, true); \
- pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl); \
- ARMARX_CHECK_EXPRESSION(pt); \
- } \
- }
- initializeKinematicUnit_tmp_helper(ControlModes::Position1DoF, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
- initializeKinematicUnit_tmp_helper(ControlModes::Velocity1DoF, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
- initializeKinematicUnit_tmp_helper(ControlModes::Torque1DoF, ControlTarget1DoFActuatorTorque, ad.ctrlTor)
-#undef initializeKinematicUnit_tmp_helper
-
-
- 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;
- }
- 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 + ".";
- Ice::PropertiesPtr properties = getIceProperties()->clone();
- //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
- //fill properties
- properties->setProperty(confPre + "RobotNodeSetName", robotNodeSetName);
- properties->setProperty(confPre + "RobotFileName", robotFileName);
- properties->setProperty(confPre + "RobotFileNameProject", robotProjectName);
- 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(),
- robot->clone(),
- std::move(devs),
- ctrlModeGroups.groups,
- ctrlModeGroups.groupsMerged,
- this
- );
- //add
- addUnit(std::move(unit));
- }
-
- void armarx::RobotUnit::initializePlatformUnit()
- {
- 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 (getRobotPlatformName().empty())
- {
- ARMARX_INFO << "no platform unit created since no platform name was given";
- return;
- }
- if (!controlDevices.has(getRobotPlatformName()))
- {
- ARMARX_WARNING << "no platform unit created since the platform control device with name '"
- << getRobotPlatformName() << "' was not found";
- return;
- }
- const ControlDevicePtr& controlDevice = controlDevices.at(robotPlatformName);
- const SensorDevicePtr& sensorDevice = sensorDevices.at(robotPlatformName);
- 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", robotPlatformName);
- 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 = robotPlatformName;
- auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
- createNJointController(
- "NJointHolonomicPlatformUnitVelocityPassThroughController", configName + "_VelPTContoller",
- config, false, true
- )
- );
- ARMARX_CHECK_EXPRESSION(ctrl);
- unit->pt = ctrl;
- unit->platformSensorIndex = sensorDevices.index(robotPlatformName);
- //add
- addUnit(std::move(unit));
- }
-
- void armarx::RobotUnit::initializeForceTorqueUnit()
- {
- 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 : sensorDevices.values())
- {
- if (ftSensorDevice->getSensorValue()->isA<SensorValueForceTorque>())
- {
- ForceTorqueSubUnit::DeviceData ftSensorData;
- ftSensorData.sensorIndex = sensorDevices.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(
- getRobot()->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 '" << getRobot()->getName() << "'"
- );
- 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", robot->getName());
- 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 RobotUnit::initializeTrajectoryControllerUnit()
- {
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- using UnitT = TrajectoryControllerSubUnit;
- if (!getProperty<bool>("CreateTrajectoryPlayer").getValue())
- {
- return;
- }
-
- //check if unit is already added
- if (getUnit(UnitT::ice_staticId()) || !trajectoryControllerSubUnit)
- {
- return;
- }
- (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))->setup(this);
- addUnit(trajectoryControllerSubUnit);
- }
-
- void RobotUnit::initializeTcpControllerUnit()
- {
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
-
- if (!getProperty<bool>("CreateTrajectoryPlayer").getValue())
- {
- return;
- }
- using UnitT = TCPControllerSubUnit;
-
- //check if unit is already added
- if (getUnit(UnitT::ice_staticId()))
- {
- return;
- }
- (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))->setup(this, robot->clone());
- addUnit(tcpControllerSubUnit);
- }
- void armarx::RobotUnit::initializeInertialMeasurementUnit()
- {
- using UnitT = InertialMeasurementSubUnit;
- using IfaceT = InertialMeasurementUnitInterface;
-
- 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(sensorDevices.keys()))
- {
- const SensorDevicePtr& sensorDevice = sensorDevices.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;
- }
- //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));
- }
-
- void armarx::RobotUnit::addUnit(ManagedIceObjectPtr 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));
- }
-
- void armarx::RobotUnit::finishUnitInitialization()
- {
-
- auto beg = MicroNow();
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
- state = RobotUnitState::WaitingForRTThreadInitialization;
- //start publisher
- std::size_t pubtimestep = getProperty<std::size_t>("PublishPeriodMs");
- if (!pubtimestep)
- {
- pubtimestep = 1;
- }
- publishNewSensorDataTime = TimeUtil::GetTime();
- publisherTask = new PublisherTaskT([&] {publish({});}, pubtimestep, false, getName() + "_PublisherTask");
- ARMARX_INFO << "starting publisher with timestep " << pubtimestep;
- publisherTask->setDelayWarningTolerance(100);
- publisherTask->start();
-
- std::size_t rtLoggingTimestep = getProperty<std::size_t>("RTLoggingPeriodMs");
- rtLogging.enabled = rtLoggingTimestep;
- rtLogging.loggingTaskTimestep = rtLoggingTimestep;
- ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
- if (isRtLoggingEnabled())
- {
- //init buffer
- {
- std::size_t messageBufferSize = getProperty<std::size_t>("RTLoggingMessageBufferSize");
- std::size_t messageBufferNumberEntries = getProperty<std::size_t>("RTLoggingMessageNumber");
- std::size_t messageBufferMaxSize = getProperty<std::size_t>("RTLoggingMaxMessageBufferSize");
- std::size_t messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLoggingMaxMessageNumber");
-
- rtLogging.keepBacklogFor = IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLoggingKeepIterationsForMs"));
-
- ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
- ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
-
- std::size_t cThreadPeriodUs = static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
- std::size_t lThreadPeriodUs = rtLoggingTimestep * 1000;
-
- std::size_t nBuffers = (lThreadPeriodUs / cThreadPeriodUs + 1) * 100;
- const auto bufferSize = controlThreadOutputBuffer.initialize(
- nBuffers, controlDevices, sensorDevices,
- messageBufferSize, messageBufferNumberEntries,
- messageBufferMaxSize, messageBufferMaxNumberEntries);
- ARMARX_INFO << "RTLogging activated! using "
- << nBuffers << "buffers "
- << "(buffersize = " << bufferSize << " bytes)";
- }
- //init logging names
- {
- //sensorDevices
- rtLogging.controlDeviceValueLoggingNames.reserve(controlDevices.size());
- for (const auto& cd : controlDevices.values())
- {
- rtLogging.controlDeviceValueLoggingNames.emplace_back();
- auto& namesForDev = rtLogging.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
- rtLogging.sensorDeviceValueLoggingNames.reserve(sensorDevices.size());
- for (const auto& sd : sensorDevices.values())
- {
- const auto names = sd->getSensorValue()->getDataFieldNames();
- rtLogging.sensorDeviceValueLoggingNames.emplace_back();
- auto& fullNames = rtLogging.sensorDeviceValueLoggingNames.back();
- fullNames.reserve(names.size());
- for (const auto& name : names)
- {
- fullNames.emplace_back(
- "sens." +
- sd->getDeviceName() + "." +
- name);
- }
- }
- }
- //start logging thread is done in rtinit
- //maybe add the default log
- {
- const auto loggingpath = getProperty<std::string>("RTLoggingDefaultLog").getValue();
- if (!loggingpath.empty())
- {
- rtLogging.defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
- }
- }
- }
- else
- {
- //no rt logging
- ARMARX_INFO << "RTLogging deactivated!";
- controlThreadOutputBuffer.initialize(0, controlDevices, sensorDevices, 0, 0, 0, 0);
- }
-
- startSelfCollisionAvoidance();
- if (isRtLoggingEnabled())
- {
- rtLogging.task = new RTLoggingTaskT([&] {rtLogging.doLogging();}, rtLogging.loggingTaskTimestep, false, getName() + "_RTLoggingTask");
- ARMARX_INFO << "starting rt logging with timestep " << rtLogging.loggingTaskTimestep;
- rtLogging.task->setDelayWarningTolerance(rtLogging.loggingTaskTimestep * 10);
- rtLogging.task->start();
- }
-
- ARMARX_INFO << "finishUnitInitialization...done! " << (MicroNow() - beg).count() << " us";
- }
-
- void armarx::RobotUnit::initRTThread()
- {
- auto guard = getGuard();
- throwIfStateIsNot(RobotUnitState::WaitingForRTThreadInitialization, __FUNCTION__);
- rtThreadId = std::this_thread::get_id();
- rtLogging.rtThreadId = LogSender::getThreadId();
- state = RobotUnitState::Running;
- ControlThreadOutputBuffer::RtLoggingInstance = &controlThreadOutputBuffer;
- }
-
- bool armarx::RobotUnit::rtSwitchControllerSetup()
- {
- rtThreadTimingsSensorDevice->rtMarkRtSwitchControllerSetupStart();
- ARMARX_ON_SCOPE_EXIT { rtThreadTimingsSensorDevice->rtMarkRtSwitchControllerSetupEnd(); };
- // !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)
- {
- return false;
- }
- rtIsInEmergencyStop = true;
- bool updatedActive = false;
- //deactivate all nJointCtrl
- for (auto& nJointCtrl : rtGetActivatedNJointControllers())
- {
- if (nJointCtrl)
- {
- RtSetNJointControllerDeactivatedStatus(nJointCtrl);
- nJointCtrl = nullptr;
- updatedActive = true;
- }
- }
- //set all JointCtrl to emergency stop (except stop movement)
- for (std::size_t i = 0; i < rtGetNumberOfControlDevices(); ++i)
- {
- ControlDevicePtr& controlDevice = controlDevices.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;
- updatedActive = true;
- }
- }
- if (updatedActive)
- {
- rtGetActivatedControllers().resetAssignement();
- rtWriteActivatedControllers();
- }
- return updatedActive;
- }
-
- if (!rtRequestedControllersChanged() && !rtIsInEmergencyStop)
- {
- return false;
- }
- rtIsInEmergencyStop = false;
-
- //handle nJointCtrl
- {
- //"merge"
- std::size_t n = rtGetNumberOfControlDevices();
- std::size_t idxAct = 0;
- std::size_t idxReq = 0;
- for (std::size_t i = 0; i < 2 * n; ++i)
- {
- //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
- std::size_t reqId = req ? req->rtGetId() : std::numeric_limits<std::size_t>::max();
- std::size_t actId = act ? act->rtGetId() : std::numeric_limits<std::size_t>::max();
-
- if (reqId < actId)
- {
- //new ctrl
- RtSetNJointControllerActivatedStatus(req);
- ++idxReq;
- }
- else if (reqId > actId)
- {
- RtSetNJointControllerDeactivatedStatus(act);
- ++idxAct;
- }
- else //if(reqId == actId)
- {
- //same ctrl or both null ctrl
- ++idxReq;
- ++idxAct;
- }
- if (idxAct >= n && !req)
- {
- break;
- }
- }
- rtGetActivatedNJointControllers() = rtGetRequestedNJointControllers();
- }
-
- //handle Joint Ctrl
- {
- for (std::size_t i = 0; i < rtGetNumberOfControlDevices(); ++i)
- {
- auto& controlDevice = controlDevices.at(i);
- const auto requestedJointCtrl = rtGetRequestedJointControllers().at(i);
- controlDevice->rtSetActiveJointController(requestedJointCtrl);
- rtGetActivatedJointControllers().at(i) = requestedJointCtrl;
- }
- }
- rtGetActivatedJointToNJointControllerAssignement() = rtGetRequestedControllers().jointToNJointControllerAssignement;
- rtWriteActivatedControllers();
- return true;
- }
-
- void armarx::RobotUnit::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex, bool writeActivatedControlers)
- {
- std::size_t nJointCtrlIndex = rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
- ARMARX_CHECK_EXPRESSION_W_HINT(
- nJointCtrlIndex < rtGetNumberOfControlDevices(),
- nJointCtrlIndex << " < " << rtGetNumberOfControlDevices() <<
- ": no NJoint controller controls this device (name = "
- << controlDevices.at(ctrlDevIndex)->getDeviceName()
- << ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
- << VAROUT(rtGetActivatedJointControllers()) << "\n"
- << VAROUT(rtGetActivatedNJointControllers())
- );
-
- rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false);
-
- if (writeActivatedControlers)
- {
- rtWriteActivatedControllers();
- }
- }
-
- void armarx::RobotUnit::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex, bool writeActivatedControlers)
- {
- const NJointControllerPtr& nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
- RtSetNJointControllerDeactivatedBecauseOfErrorStatus(nJointCtrl);
- for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
- {
- const ControlDevicePtr& controlDevice = controlDevices.at(ctrlDevIdx);
- JointController* es = controlDevice->rtGetJointEmergencyStopController();
- controlDevice->rtSetActiveJointController(es);
- rtGetActivatedJointControllers().at(ctrlDevIdx) = es;
- ARMARX_CHECK_EXPRESSION(rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) == nJointCtrlIndex);
- rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) = IndexSentinel();
- }
- rtGetActivatedNJointControllers().at(nJointCtrlIndex) = nullptr;
- //check ControlDeviceHardwareControlModeGroups
- {
- for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
- {
- const auto groupIdx = ctrlModeGroups.groupIndices.at(ctrlDevIdx);
- if (groupIdx == IndexSentinel())
- {
- continue;
- }
- ControlDevice* const dev = controlDevices.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 = controlDevices.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, false);
- }
- }
- }
- rtDeactivatedNJointControllerBecauseOfError(nJointCtrl);
-
- if (writeActivatedControlers)
- {
- rtWriteActivatedControllers();
- }
- }
-
- void armarx::RobotUnit::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- rtThreadTimingsSensorDevice->rtMarkRtRunNJointControllersStart();
- for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < rtGetActivatedNJointControllers().size(); ++nJointCtrlIndex)
- {
- NJointController* nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
- try
- {
- if (nJointCtrl)
- {
- auto start = MicroNow();
- nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
- auto duration = MicroNow() - start;
- if (duration.count() > 500)
- {
- ARMARX_ERROR << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
- }
- else if (duration.count() > 50)
- {
- ARMARX_RT_LOGF_WARN("An NJointController took %d µs to run!", duration.count()).deactivateSpam(5);
- }
- }
- }
- catch (...)
- {
- ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
- << " threw an exception and is now deactivated: "
- << GetHandledExceptionString();
- rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false);
- }
- }
- rtWriteActivatedControllers();
- rtThreadTimingsSensorDevice->rtMarkRtRunNJointControllersEnd();
- }
-
- void armarx::RobotUnit::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- rtThreadTimingsSensorDevice->rtMarkRtRunJointControllersStart();
- for (const ControlDevicePtr& device : this->controlDevices.values())
- {
- device->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
- rtThreadTimingsSensorDevice->rtMarkRtRunJointControllersEnd();
- }
-
- void armarx::RobotUnit::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- rtThreadTimingsSensorDevice->rtMarkRtReadSensorDeviceValuesStart();
- for (const SensorDevicePtr& device : this->sensorDevices.values())
- {
- device->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
- }
- rtThreadTimingsSensorDevice->rtMarkRtReadSensorDeviceValuesEnd();
- }
-
- void armarx::RobotUnit::rtHandleInvalidTargets()
- {
- rtThreadTimingsSensorDevice->rtMarkRtHandleInvalidTargetsStart();
- bool oneIsInvalid = 0;
- for (std::size_t i = 0; i < rtGetNumberOfControlDevices(); ++i)
- {
- if (!rtGetActivatedJointControllers().at(i)->rtIsTargetVaild())
- {
- rtDeactivateAssignedNJointControllerBecauseOfError(i, false);
- oneIsInvalid = true;
- }
- }
- if (oneIsInvalid)
- {
- rtWriteActivatedControllers();
- }
- rtThreadTimingsSensorDevice->rtMarkRtHandleInvalidTargetsEnd();
- }
-
- void armarx::RobotUnit::rtResetAllTargets()
- {
- rtThreadTimingsSensorDevice->rtMarkRtResetAllTargetsStart();
- for (const ControlDevicePtr& controlDev : controlDevices.values())
- {
- controlDev->rtGetActiveJointController()->rtResetTarget();
- }
- rtThreadTimingsSensorDevice->rtMarkRtResetAllTargetsEnd();
- }
-
- ConstSensorDevicePtr armarx::RobotUnit::getSensorDevice(const std::string& sensorDeviceName) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
- }
-
- ConstControlDevicePtr armarx::RobotUnit::getControlDevice(const std::string& deviceName) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return controlDevices.at(deviceName, ControlDevice::NullPtr);
- }
-
- ControlTargetBase* armarx::RobotUnit::getControlTarget(const std::string& deviceName, const std::string& controlMode)
- {
- static const std::string fname = __FUNCTION__;
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- if (std::this_thread::get_id() != initNJointThreadId)
- {
- throw std::logic_error
- {
- fname + ": Must be only called during instantiation of a NJoint controller from the instantiated controler " +
- "(Don't try to call this function outside of a NJointController's constructor)"
- };
- }
- if (initNJointControllerUsedJointController.find(deviceName) != initNJointControllerUsedJointController.end())
- {
- throw std::logic_error
- {
- fname + ": Must not request two ControlTargets for the same device. (got = " +
- initNJointControllerUsedJointController.at(deviceName)->getControlMode() + ", requested " + controlMode + ") " +
- "(You can only have a ControlDevice in one ControlMode. " +
- "To check all available ControlModes for this device, get the ControlDevice via getControlDevice(" + deviceName + ") and querry it)"
- };
- }
- // we should allow these modes for debugging reasons + there is no real reason to forbid them
- //if (controlMode == ControlModes::EmergencyStop)
- //{
- // throw std::invalid_argument {"armarx::RobotUnit::getControlTarget: Can't get target for the control mode " + ControlModes::EmergencyStop};
- //}
- //if (controlMode == ControlModes::StopMovement)
- //{
- // throw std::invalid_argument {"armarx::RobotUnit::getControlTarget: Can't get target for the control mode " + ControlModes::StopMovement};
- //}
- ControlDevicePtr controlDevice = controlDevices.at(deviceName, ControlDevice::NullPtr);
- if (!controlDevice)
- {
- return nullptr;
- }
- if (!controlDevice->hasJointController(controlMode))
- {
- return nullptr;
- }
- //there is a device and a target as was requested:
- JointController* jointCtrl = controlDevice->getJointController(controlMode);
- initNJointControllerUsedJointController[deviceName] = jointCtrl;
- return jointCtrl->getControlTarget();
- }
-
- NJointControllerInterfacePrx armarx::RobotUnit::createNJointControllerFromVariantConfig(
- const std::string& className, const std::string& instanceName, const StringVariantBaseMap& variants, const Ice::Current&)
- {
- //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),
- GlobalIceCurrent/*to select ice overload*/);
- }
-
- const NJointControllerPtr& armarx::RobotUnit::createNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- bool deletable,
- bool internal)
- {
- 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(initNJointControllerUsedJointController.empty());//in case someone broke things
- initNJointThreadId = std::this_thread::get_id();
- ARMARX_ON_SCOPE_EXIT
- {
- initNJointControllerUsedJointController.clear();
- initNJointThreadId = std::thread::id{};
- };
- ARMARX_CHECK_EXPRESSION(factory);
- NJointControllerPtr nJointCtrl {factory->create(NJointControllerDescriptionProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config, robot)};
-
- std::vector<char> ctrlDeviceUsedBitmap(getNumberOfControlDevices(), false);
- std::vector<std::size_t> ctrlDeviceUsedIndices;
- ctrlDeviceUsedIndices.reserve(getNumberOfControlDevices());
- for (const auto& cd2targ : initNJointControllerUsedJointController)
- {
- auto idx = controlDevices.index(cd2targ.first);
- ctrlDeviceUsedBitmap.at(idx) = true;
- ctrlDeviceUsedIndices.emplace_back(idx);
- }
- InitNJointControler(
- nJointCtrl,
- this,
- nJointControllerNextId++,
- initNJointControllerUsedJointController,
- std::move(ctrlDeviceUsedBitmap),
- std::move(ctrlDeviceUsedIndices),
- deletable, internal);
- getArmarXManager()->addObjectAsync(nJointCtrl, instanceName, false, false);
- nJointControllers[instanceName] = std::move(nJointCtrl);
- ARMARX_CHECK_EXPRESSION(listenerPrx);
- listenerPrx->nJointControllerCreated(instanceName);
- return nJointControllers.at(instanceName);
- }
-
- void armarx::RobotUnit::setActivateControllersRequest(std::set<NJointControllerPtr, NJointController::IdComp>&& ctrls)
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- //erase nullptr
- ctrls.erase(nullptr);
- ARMARX_CHECK_EXPRESSION(ctrls.size() <= getNumberOfControlDevices());
- const std::set<NJointControllerPtr, NJointController::IdComp> setOfRequestedCtrls
- {
- getRequestedNJointControllers().begin(),
- getRequestedNJointControllers().end()
- };
- if (setOfRequestedCtrls == ctrls)
- {
- //redirty the flag to swap in the same set again
- controllersRequested.commitWrite();
- return;
- }
- JointAndNJointControllers request {getNumberOfControlDevices()};
- std::size_t idx = 0;
- for (const NJointControllerPtr& nJointCtrl : ctrls)
- {
- request.nJointControllers.at(idx++) = nJointCtrl.get();
- //add Joint for this ctrl
- for (const auto& cd2cm : nJointCtrl->getControlDeviceUsedControlModeMap())
- {
- std::size_t jointCtrlIndex = controlDevices.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)
- {
- ss << "\n" << ctrl->getInstanceName();
- }
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- request.jointControllers.at(jointCtrlIndex) = controlDevices.at(jointCtrlIndex)->getJointController(cd2cm.second);
- }
- }
- for (auto i : getIndices(request.jointControllers))
- {
- JointController*& jointCtrl = request.jointControllers.at(i);
- if (!jointCtrl)
- {
- JointController* jointCtrlOld = getRequestedJointControllers().at(i);
- if (jointCtrlOld == controlDevices.at(i)->getJointStopMovementController())
- {
- //don't replace this controller with emergency stop
- jointCtrl = jointCtrlOld;
- }
- else
- {
- //no one controls this device -> emergency stop
- jointCtrl = controlDevices.at(i)->getJointEmergencyStopController();
- }
- }
- }
- writeRequestedControllers(std::move(request));
- ARMARX_INFO << "requested controllers:\n" << getRequestedNJointControllerNames();
- }
-
- void armarx::RobotUnit::deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
- {
- ARMARX_VERBOSE << "requesting controller deletion for:\n" << names;
- if (names.empty())
- {
- return;
- }
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- auto ctrlsToActVec = getNJointControllersNotNull(names); //also checks if these controllers exist
- ARMARX_DEBUG << "all controllers requested to delete exist" << std::flush;
- //check if all can be deleted
- for (const auto& nJointCtrl : ctrlsToActVec)
- {
- if (!nJointCtrl->isDeletable())
- {
- throw LogicError
- {
- "The NJointController '" + nJointCtrl->getInstanceName() +
- "' can't be deleted since this operation is not allowed for this controller! (no NJointController was deleted)"
- };
- }
- }
- for (const auto& nJointCtrl : ctrlsToActVec)
- {
- 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)"
- };
- }
- }
- for (const auto& nJointCtrl : ctrlsToActVec)
- {
- 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";
- }
- }
-
- void armarx::RobotUnit::activateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
- {
- ARMARX_VERBOSE << "requesting controller activation for:\n" << names;
- if (names.empty())
- {
- return;
- }
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- auto ctrlsToActVec = getNJointControllersNotNull(names); //also checks if these controllers exist
- ARMARX_DEBUG << "all requested controllers exist" << std::flush;
- std::set<NJointControllerPtr, NJointController::IdComp> ctrlsToAct {ctrlsToActVec.begin(), ctrlsToActVec.end()};
- ARMARX_CHECK_EXPRESSION(!ctrlsToAct.count(nullptr));
- std::set<NJointControllerPtr, NJointController::IdComp> ctrls {getRequestedNJointControllers().begin(), getRequestedNJointControllers().end()};
- ctrls.erase(nullptr);
- //check for conflict
- std::vector<char> inuse;
- //check requested controllers
- {
- auto r = NJointController::AreNotInConflict(ctrlsToAct.begin(), ctrlsToAct.end());
- if (!r)
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::activateNJointControllers: requested controllers are in conflict!\ncontrollers:\n" << names;
- 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)
- {
- out << (c ? 1 : 0);
- }
- };
- ARMARX_DEBUG << "inuse field (request)\n" << printInUse;
- //add already active controllers if they are conflict free
- {
- if (ctrls.empty())
- {
- ARMARX_DEBUG << "no already requested NJointControllers";
- }
- for (const NJointControllerPtr& nJointCtrl : ctrls)
- {
- 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_DEBUG << "inuse field (all)\n" << printInUse;
- }
- setActivateControllersRequest(std::move(ctrlsToAct));
- }
-
- void armarx::RobotUnit::deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
- {
- ARMARX_VERBOSE << "requesting controller deactivation for:\n" << names;
- if (names.empty())
- {
- return;
- }
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- auto ctrlsDeacVec = getNJointControllersNotNull(names); //also checks if these controllers exist
- std::set<NJointControllerPtr, NJointController::IdComp> ctrls {getRequestedNJointControllers().begin(), getRequestedNJointControllers().end()};
- for (const auto& nJointCtrlToDeactivate : ctrlsDeacVec)
- {
- ctrls.erase(nJointCtrlToDeactivate);
- }
- setActivateControllersRequest(std::move(ctrls));
- }
-
- void armarx::RobotUnit::switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current&)
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- auto ctrlsToActVec = getNJointControllersNotNull(newSetup); //also checks if these controllers exist
- setActivateControllersRequest({ctrlsToActVec.begin(), ctrlsToActVec.end()});
- }
-
- void armarx::RobotUnit::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
- {
- auto begPublish = Now();
- static const int spamdelay = 30;
-
- if (state != RobotUnitState::Running)
- {
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state " << state;
- return;
- }
- auto guard = getGuard();
-
- throwIfStateIsNot(RobotUnitState::Running, __FUNCTION__);
-
- //get batch proxies
- auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
- auto listenerBatchPrx = listenerPrx->ice_batchOneway();
- //delete NJoint queued for deletion
- {
- for (auto& pair : nJointControllersToBeDeleted)
- {
- listenerBatchPrx->nJointControllerDeleted(pair.first);
- getArmarXManager()->removeObjectNonBlocking(pair.second);
- ARMARX_VERBOSE << "deleted NJointController " << pair.first;
- }
- nJointControllersToBeDeleted.clear();
- }
- //swap buffers in
- const bool haveActivatedControllersChanged = activatedControllersChanged();
- const bool haveSensorAndControlValsChanged = sensorAndControlBufferChanged();
- //setup datastructures
- const auto& controlThreadOutputBuffer = getSensorAndControlBuffer();
- const auto& activatedControllers = getActivatedControllers();
- const auto& requestedControllers = getRequestedControllers();
-
- const auto timestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
-
- const auto numSensorDevices = getNumberOfSensorDevices();
- const auto numControlDevices = getNumberOfControlDevices();
-
- const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
- const bool debugObserverPublishControlTargetsThisIteration = ObserverPublishControlTargets;
- const bool debugObserverPublishSensorValuesThisIteration = ObserverPublishSensorValues;
- StringVariantBaseMap ctrlDevMap, sensorDevMap;
- //publish publishing meta state
- additionalMap["haveActivatedControllersChanged"] = new TimedVariant {haveActivatedControllersChanged, timestamp};
- additionalMap["haveSensorAndControlValsChanged"] = new TimedVariant {haveSensorAndControlValsChanged, timestamp};
- additionalMap["publishToObserver" ] = new TimedVariant {publishToObserver, timestamp};
- //update
- if (haveSensorAndControlValsChanged)
- {
- //update units
- timingMap["publishTimings_UnitUpdate"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
- publishNewSensorDataTime = TimeUtil::GetTime();
- for (RobotUnitSubUnitPtr& rsu : subUnits)
- {
- if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
- {
- timingMap["publishTimings_UnitUpdate_" + rsu->getName()] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant){rsu->update(controlThreadOutputBuffer, activatedControllers);},
- timestamp
- };
- }
- }
- },
- timestamp
- };
- //publish sensor updates
-
- timingMap["publishTimings_SensorUpdates"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- for (std::size_t sensidx = 0 ; sensidx < numSensorDevices; ++sensidx)
- {
- const SensorDevice& sensDev = *(sensorDevices.at(sensidx));
- const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
- auto variants = sensVal.toVariants(timestamp);
-
- if (!variants.empty())
- {
- if (debugObserverPublishSensorValuesThisIteration && publishToObserver)
- {
- transformMapKeys(
- variants, sensorDevMap,
- detail::makeDebugObserverNameFixer(sensDev.getDeviceName() + "_" + sensVal.getSensorValueType(true) + "_")
- );
- }
- SensorDeviceStatus status;
- status.deviceName = sensDev.getDeviceName();
- status.sensorValue = std::move(variants);
- status.timestampUSec = MicroNow().count();
- listenerBatchPrx->sensorDeviceStatusChanged(status);
- }
- }
- },
- timestamp
- };
- }
- 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)
- {
- timingMap["publishTimings_ControlUpdates"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- for (std::size_t ctrlidx = 0 ; ctrlidx < numControlDevices; ++ctrlidx)
- {
- const ControlDevice& ctrlDev = *(controlDevices.at(ctrlidx));
-
- StringToStringVariantBaseMapMap variants;
- for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
- {
- const auto& controlMode = ctrlVal->getControlMode();
- variants[controlMode] = ctrlVal->toVariants(timestamp);
- if (
- haveSensorAndControlValsChanged &&
- !variants[controlMode].empty() &&
- debugObserverPublishControlTargetsThisIteration &&
- publishToObserver
- )
- {
- transformMapKeys(
- variants[controlMode], ctrlDevMap,
- detail::makeDebugObserverNameFixer(ctrlDev.getDeviceName() + "_" + controlMode + "_")
- );
- }
- }
- ControlDeviceStatus status;
- const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
- status.deviceName = ctrlDev.getDeviceName();
- ARMARX_CHECK_EXPRESSION(requestedControllers.jointControllers.at(ctrlidx));
- status.requestedControlMode = requestedControllers.jointControllers.at(ctrlidx)->getControlMode();
- status.controlTargetValues = std::move(variants);
- status.timestampUSec = MicroNow().count();
- listenerBatchPrx->controlDeviceStatusChanged(status);
- }
- },
- timestamp
- };
- }
- //call publish hook + publish NJointController changes
- timingMap["publishTimings_NJointControllerUpdates"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- for (const auto& pair : nJointControllers)
- {
- const NJointControllerPtr& nJointCtrl = pair.second;
- timingMap["publishTimings_NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- //run some hook for active (used for visu)
- PublishNJointController(nJointCtrl, controlThreadOutputBuffer, debugDrawerPrx, debugObserverBatchPrx);
- if (
- GetNJointControllerStatusReportedActive(nJointCtrl) != nJointCtrl->isControllerActive() ||
- GetNJointControllerStatusReportedRequested(nJointCtrl) != nJointCtrl->isControllerRequested()
- )
- {
- UpdateNJointControllerStatusReported(nJointCtrl);
- listenerBatchPrx->nJointControllerStatusChanged(nJointCtrl->getControllerStatus());
- }
- },
- timestamp
- };
- }
- },
- timestamp
- };
-
- //report new class names
- timingMap["publishTimings_ClassNameUpdates"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- const auto classNames = NJointControllerRegistry::getKeys();
- if (lastReportedClasses.size() != classNames.size())
- {
- for (const auto& name : classNames)
- {
- if (!lastReportedClasses.count(name))
- {
- listenerBatchPrx->nJointControllerClassAdded(name);
- lastReportedClasses.emplace(name);
- }
- }
- }
- },
- timestamp
- };
- timingMap["publishTimings_RobotUnitListenerFlush"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant){listenerBatchPrx->ice_flushBatchRequests();}, timestamp
- };
-
- if (publishToObserver)
- {
- timingMap["publishTimings_LastRobotUnitObserverFlush"] =
- new TimedVariant {TimestampVariant{lastRobotUnitObserverFlush.count()}, timestamp};
- timingMap["publishTimings_LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop.count()}, timestamp};
- lastRobotUnitObserverFlush = ARMARX_STOPWATCH()
- {
- if (robotUnitObs->getState() >= eManagedIceObjectStarted)
- {
- robotUnitObs->offerOrUpdateDataFieldsFlatCopy(robotUnitObs->additionalChannel, additionalMap);
- robotUnitObs->updateChannel(robotUnitObs->additionalChannel);
- robotUnitObs->offerOrUpdateDataFieldsFlatCopy(robotUnitObs->timingChannel, timingMap);
- robotUnitObs->updateChannel(robotUnitObs->timingChannel);
- robotUnitObs->offerOrUpdateDataFieldsFlatCopy(robotUnitObs->controlDevicesChannel, ctrlDevMap);
- robotUnitObs->updateChannel(robotUnitObs->controlDevicesChannel);
- robotUnitObs->offerOrUpdateDataFieldsFlatCopy(robotUnitObs->sensorDevicesChannel, sensorDevMap);
- robotUnitObs->updateChannel(robotUnitObs->sensorDevicesChannel);
-
- }
- debugObserverBatchPrx->ice_flushBatchRequests();
- };
- }
- else
- {
- lastRobotUnitObserverFlush = std::chrono::microseconds {0};
- }
-
- //warn if there are unset jointCtrl controllers
- {
- const auto& jointCtrls = getActivatedJointControllers();
- if (std::any_of(jointCtrls.begin(), jointCtrls.end(), [](const JointController * jointCtrl)
- {
- return !jointCtrl;
- }))
- {
- ARMARX_WARNING << "armarx::RobotUnit::publish: Some activated JointControllers are reported to be nullptr! "
- << "(did you forget to call rtSwitchControllerSetup()?)";
- }
- }
- ++publishIterationCount;
- lastPublishLoop = MicroToNow(begPublish);
- }
-
- void armarx::RobotUnit::removeAllUnits()
- {
- subUnits.clear();
- for (ManagedIceObjectPtr& unit : units)
- {
- getArmarXManager()->removeObjectBlocking(unit->getName());
- }
- units.clear();
- }
-
- void armarx::RobotUnit::finishRunning()
- {
- ARMARX_DEBUG << "RobotUnit::finishRunning()";
- if (state == RobotUnitState::Exiting)
- {
- ARMARX_ERROR << "armarx::RobotUnit::finishRunning called multiple times!";
- }
-
- rtLogging.defaultLogHandle = nullptr;
- if (rtLogging.task)
- {
- ARMARX_DEBUG << "shutting down rt logging task";
- rtLogging.task->stop();
- while (rtLogging.task->isFunctionExecuting() || rtLogging.task->isRunning())
- {
- ARMARX_FATAL << deactivateSpam(0.1) << "RT LOGGING TASK IS RUNNING AFTER IT WAS STOPPED!";
- }
- rtLogging.task = nullptr;
- ARMARX_DEBUG << "shutting down rt logging task done";
- }
-
- if (publisherTask)
- {
- ARMARX_DEBUG << "shutting down publisher task";
- publisherTask->stop();
- auto tPublisherStopped = Now();
- 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_until(tPublisherStopped + std::chrono::milliseconds {100});
- ARMARX_DEBUG << "shutting down publisher task done";
- }
-
- auto guard = getGuard();
- try
- {
- throwIfStateIsNot(RobotUnitState::Running, __FUNCTION__, true);
- state = RobotUnitState::Exiting;
- //const ptr maps
- controlDevicesConstPtr.clear();
- sensorDevicesConstPtr.clear();
- //publisher
-
-
- //RT
- ARMARX_DEBUG << "joining rt thread";
- joinRTThread();
- ARMARX_DEBUG << "joining rt thread...done";
- //call deac publishing of all active NJoint
- for (const auto& pair : nJointControllers)
- {
- ARMARX_DEBUG << "forcing deactivation of publishing for " << pair.first;
- DeactivateNJointControllerPublishing(pair.second, debugDrawerPrx, debugObserverPrx);
- }
- //units
- ARMARX_DEBUG << "removing units";
- removeAllUnits();
- ARMARX_DEBUG << "removing units...done";
- //NJoint queued for deletion (some could still be in the queue)
- {
- ARMARX_DEBUG << "remove NJointControllers queued for deletion";
- for (auto& n2NJointCtrl : nJointControllersToBeDeleted)
- {
- NJointControllerPtr& nJointCtrl = n2NJointCtrl.second;
- getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
- }
- nJointControllersToBeDeleted.clear();
- ARMARX_DEBUG << "remove NJointControllers queued for deletion...done";
- }
- //NJoint
- {
- ARMARX_DEBUG << "remove NJointControllers";
- for (auto& n2NJointCtrl : nJointControllers)
- {
- NJointControllerPtr& nJointCtrl = n2NJointCtrl.second;
- getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
- }
- nJointControllers.clear();
- ARMARX_DEBUG << "remove NJointControllers...done";
- }
- //clear devices
- sensorDevices.clear();
- controlDevices.clear();
- //robot
- if (robot.use_count() != 1)
- {
- ARMARX_WARNING << "The robot's smart pointer use count is " << robot.use_count();
- }
- robot.reset();
- ARMARX_INFO << "RobotUnit finished running";
- }
- catch (Ice::Exception& e)
- {
- ARMARX_ERROR << "exception in RobotUnit::finishRunning!\nwhat:\n"
- << e.what()
- << "\n\tname: " << e.ice_name()
- << "\n\tfile: " << e.ice_file()
- << "\n\tline: " << e.ice_line()
- << "\n\tstack: " << e.ice_stackTrace();
- }
- catch (std::exception& e)
- {
- ARMARX_ERROR << "exception in RobotUnit::finishRunning!\nwhat:\n" << e.what();
- }
- catch (...)
- {
- ARMARX_ERROR << "exception in RobotUnit::finishRunning!";
- }
- ARMARX_DEBUG << "RobotUnit::finishRunning()...done";
- }
-
- NJointControllerClassDescription armarx::RobotUnit::getNJointControllerClassDescription(
- const std::string& className, const Ice::Current&) const
- {
- while (state == RobotUnitState::PreComponentInitialization || state == 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(robot, controlDevicesConstPtr, sensorDevicesConstPtr);
- }
- return data;
- }
-
- NJointControllerClassDescriptionSeq armarx::RobotUnit::getNJointControllerClassDescriptions(const Ice::Current&) const
- {
- std::size_t tries = 200;
- while (state == RobotUnitState::PreComponentInitialization || state == RobotUnitState::InitializingDevices)
- {
- //this phase should only last short so busy waiting is ok
- std::this_thread::sleep_for(std::chrono::milliseconds(50));
- if (! --tries)
- {
- throw RuntimeError {"RobotUnit::getNJointControllerClassDescriptions: it took too long to for the unit to get in a valid state"};
- }
- }
- auto guard = getGuard();
- NJointControllerClassDescriptionSeq r;
- r.reserve(NJointControllerRegistry::getKeys().size());
- for (const auto& key : NJointControllerRegistry::getKeys())
- {
- r.emplace_back(getNJointControllerClassDescription(key));
- }
- return r;
- }
-
- NJointControllerInterfacePrx armarx::RobotUnit::getNJointController(const std::string& name, const Ice::Current&) const
- {
- 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));
- }
-
- NJointControllerStatus armarx::RobotUnit::getNJointControllerStatus(const std::string& name, const Ice::Current&) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return getNJointControllerNotNull(name)->getControllerStatus();
- }
-
- NJointControllerStatusSeq armarx::RobotUnit::getNJointControllerStatuses(const Ice::Current&) const
- {
- auto guard = getGuard();
- if (!areDevicesReady())
- {
- return {};
- }
- NJointControllerStatusSeq r;
- r.reserve(nJointControllers.size());
- for (const auto& nJointCtrl : nJointControllers)
- {
- r.emplace_back(nJointCtrl.second->getControllerStatus());
- }
- return r;
- }
-
- NJointControllerDescription armarx::RobotUnit::getNJointControllerDescription(const std::string& name, const Ice::Current&) const
- {
- NJointControllerPtr nJointCtrl;
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- nJointCtrl = getNJointControllerNotNull(name);
- }
- return nJointCtrl->getControllerDescription();
- }
-
- NJointControllerDescriptionSeq armarx::RobotUnit::getNJointControllerDescriptions(const Ice::Current&) const
- {
- 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;
- }
-
- NJointControllerDescriptionWithStatus armarx::RobotUnit::getNJointControllerDescriptionWithStatus(
- const std::string& name, const Ice::Current&) const
- {
- NJointControllerPtr nJointCtrl;
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- nJointCtrl = getNJointControllerNotNull(name);
- }
- return nJointCtrl->getControllerDescriptionWithStatus();
- }
-
- NJointControllerDescriptionWithStatusSeq armarx::RobotUnit::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
- {
- std::map<std::string, NJointControllerPtr> nJointControllersCopy;
- {
- auto guard = getGuard();
- if (!areDevicesReady())
- {
- return {};
- }
- nJointControllersCopy = nJointControllers;
- }
- NJointControllerDescriptionWithStatusSeq r;
- r.reserve(nJointControllersCopy.size());
- for (const auto& nJointCtrl : nJointControllersCopy)
- {
- r.emplace_back(nJointCtrl.second->getControllerDescriptionWithStatus());
- }
- return r;
- }
-
- Ice::StringSeq armarx::RobotUnit::getControlDeviceNames(const Ice::Current&) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return controlDevices.keys();
- }
-
- ControlDeviceDescription armarx::RobotUnit::getControlDeviceDescription(std::size_t idx) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- 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();
- }
- return data;
- }
-
- ControlDeviceDescription armarx::RobotUnit::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- if (!controlDevices.has(name))
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::getControlDeviceDescription: There is no ControlDevice '" << name
- << "'. There are these ControlDevices: " << controlDevices.keys();
- throw InvalidArgumentException {ss.str()};
- }
- return getControlDeviceDescription(controlDevices.index(name));
- }
-
- ControlDeviceDescriptionSeq armarx::RobotUnit::getControlDeviceDescriptions(const Ice::Current&) const
- {
- auto guard = getGuard();
- if (!areDevicesReady())
- {
- return {};
- }
- ControlDeviceDescriptionSeq r;
- r.reserve(getNumberOfControlDevices());
- for (auto idx : getIndices(controlDevices.values()))
- {
- r.emplace_back(getControlDeviceDescription(idx));
- }
- return r;
- }
-
- ControlDeviceStatus armarx::RobotUnit::getControlDeviceStatus(std::size_t idx) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- const ControlDevicePtr& controlDevice = controlDevices.at(idx);
- ControlDeviceStatus status;
- const auto activeJointCtrl = getActivatedJointControllers().at(idx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
- status.deviceName = controlDevice->getDeviceName();
- ARMARX_CHECK_EXPRESSION(getRequestedJointControllers().at(idx));
- status.requestedControlMode = getRequestedJointControllers().at(idx)->getControlMode();
- for (const auto& targ : getSensorAndControlBuffer().control.at(idx))
- {
- status.controlTargetValues[targ->getControlMode()] = targ->toVariants(getSensorAndControlBuffer().sensorValuesTimestamp);
- }
- status.timestampUSec = MicroNow().count();
- return status;
- }
-
- ControlDeviceStatus armarx::RobotUnit::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- if (!controlDevices.has(name))
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::getControlDeviceStatus: There is no ControlDevice '" << name
- << "'. There are these ControlDevices: " << controlDevices.keys();
- throw InvalidArgumentException {ss.str()};
- }
- return getControlDeviceStatus(controlDevices.index(name));
- }
-
- ControlDeviceStatusSeq armarx::RobotUnit::getControlDeviceStatuses(const Ice::Current&) const
- {
- auto guard = getGuard();
- if (!areDevicesReady())
- {
- return {};
- }
- ControlDeviceStatusSeq r;
- r.reserve(getNumberOfControlDevices());
- for (auto idx : getIndices(controlDevices.values()))
- {
- r.emplace_back(getControlDeviceStatus(idx));
- }
- return r;
- }
-
- Ice::StringSeq armarx::RobotUnit::getSensorDeviceNames(const Ice::Current&) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return sensorDevices.keys();
- }
-
- SensorDeviceDescription armarx::RobotUnit::getSensorDeviceDescription(std::size_t idx) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- 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();
- return data;
- }
-
- SensorDeviceDescription armarx::RobotUnit::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- if (!sensorDevices.has(name))
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::getSensorDeviceDescription: There is no SensorDevice '" << name
- << "'. There are these SensorDevices: " << sensorDevices.keys();
- throw InvalidArgumentException {ss.str()};
- }
- return getSensorDeviceDescription(sensorDevices.index(name));
- }
-
- SensorDeviceDescriptionSeq armarx::RobotUnit::getSensorDeviceDescriptions(const Ice::Current&) const
- {
- auto guard = getGuard();
- if (!areDevicesReady())
- {
- return {};
- }
- SensorDeviceDescriptionSeq r;
- r.reserve(getNumberOfSensorDevices());
- for (auto idx : getIndices(sensorDevices.values()))
- {
- r.emplace_back(getSensorDeviceDescription(idx));
- }
- return r;
- }
-
- SensorDeviceStatus armarx::RobotUnit::getSensorDeviceStatus(std::size_t idx) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
- SensorDeviceStatus status;
- status.deviceName = sensorDevice->getDeviceName();
- status.sensorValue = getSensorAndControlBuffer().sensors.at(idx)->toVariants(getSensorAndControlBuffer().sensorValuesTimestamp);
- status.timestampUSec = MicroNow().count();
- return status;
- }
-
- SensorDeviceStatus armarx::RobotUnit::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- if (!sensorDevices.has(name))
- {
- std::stringstream ss;
- ss << "armarx::RobotUnit::getSensorDeviceStatus: There is no SensorDevice '" << name
- << "'. There are these SensorDevices: " << sensorDevices.keys();
- throw InvalidArgumentException {ss.str()};
- }
- return getSensorDeviceStatus(sensorDevices.index(name));
- }
-
- SensorDeviceStatusSeq armarx::RobotUnit::getSensorDeviceStatuses(const Ice::Current&) const
- {
- auto guard = getGuard();
- if (!areDevicesReady())
- {
- return {};
- }
- SensorDeviceStatusSeq r;
- r.reserve(getNumberOfSensorDevices());
- for (auto idx : getIndices(sensorDevices.values()))
- {
- r.emplace_back(getSensorDeviceStatus(idx));
- }
- return r;
- }
-
- std::vector<armarx::NJointControllerPtr> armarx::RobotUnit::getNJointControllersNotNull(const std::vector<std::string>& names) const
- {
- 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& armarx::RobotUnit::getNJointControllerNotNull(const std::string& name) const
- {
- 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;
- }
-
- StringNJointControllerPrxDictionary armarx::RobotUnit::getAllNJointControllers(const Ice::Current&) const
- {
- 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;
- }
-
- Ice::ObjectProxySeq armarx::RobotUnit::getUnits(const Ice::Current&) const
- {
- 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;
- }
-
- const ManagedIceObjectPtr& armarx::RobotUnit::getUnit(const std::string& staticIceId) const
- {
- auto guard = getGuard();
- for (const ManagedIceObjectPtr& unit : units)
- {
- if (unit->ice_isA(staticIceId))
- {
- return unit;
- }
- }
- return ManagedIceObject::NullPtr;
- }
-
- Ice::ObjectPrx armarx::RobotUnit::getUnit(const std::string& staticIceId, const Ice::Current&) const
- {
- //no lock required
- ManagedIceObjectPtr unit = getUnit(staticIceId);
- if (unit)
- {
- return unit->getProxy(-1, true);
- }
- return nullptr;
- }
-
- const VirtualRobot::RobotPtr& armarx::RobotUnit::getRobot() const
- {
- auto guard = getGuard();
- throwIfStateIsNot(
- {
- RobotUnitState::InitializingDevices,
- RobotUnitState::InitializingUnits,
- RobotUnitState::WaitingForRTThreadInitialization,
- RobotUnitState::Running
- },
- __FUNCTION__
- );
- ARMARX_CHECK_EXPRESSION(robot);
- return robot;
- }
-
- std::string armarx::RobotUnit::getRobotName() const
- {
- auto guard = getGuard();
- throwIfStateIsNot(
- {
- RobotUnitState::InitializingDevices,
- RobotUnitState::InitializingUnits,
- RobotUnitState::WaitingForRTThreadInitialization,
- RobotUnitState::Running
- },
- __FUNCTION__
- );
- return getRobot()->getName();
- }
-
- std::size_t armarx::RobotUnit::getNumberOfControlDevices() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return controlDevices.size();
- }
-
- std::size_t armarx::RobotUnit::getNumberOfSensorDevices() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return sensorDevices.size();
- }
-
- const DebugDrawerInterfacePrx& armarx::RobotUnit::getDebugDrawerProxy() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return debugDrawerPrx;
- }
-
- const RobotUnitListenerPrx& armarx::RobotUnit::getListenerProxy() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return listenerPrx;
- }
-
- void armarx::RobotUnit::icePropertiesInitialized()
- {
- robotUnitObs = Component::create<RobotUnitObserver>(getIceProperties(), "", getConfigDomain());
- addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObs));
-
- Ice::PropertiesPtr properties = getIceProperties()->clone();
- const std::string& configDomain = "ArmarX";
- // create TCPControlSubUnit
-
- {
- const std::string configNameTCPControlUnit = getProperty<std::string>("TCPControlUnitName");
- tcpControllerSubUnit = Component::create<TCPControllerSubUnit>(properties, configNameTCPControlUnit, configDomain);
- addPropertyUser(PropertyUserPtr::dynamicCast(tcpControllerSubUnit));
- }
-
- // create TrajectoryControllerSubUnit
-
- {
- 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>(properties, configNameTrajectoryControllerUnit, configDomain);
- addPropertyUser(PropertyUserPtr::dynamicCast(trajectoryControllerSubUnit));
- }
- }
-
-
- RemoteReferenceCounterBasePtr armarx::RobotUnit::startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current&)
- {
- if (!isRtLoggingEnabled())
- {
- throw LogicError {"RtLogging is disabled in the config! (set RTLoggingPeriodMs to a non zero value"};
- }
- FileSystemPathBuilder pb {formatString};
- std::lock_guard<std::mutex> guard {rtLogging.mutex};
- if (rtLogging.entries.count(pb.getPath()))
- {
- throw InvalidArgumentException {"There already is a logger for the path '" + pb.getPath() + "'"};
- }
- auto ptr = std::make_shared<RtLogging::Entry>();
- rtLogging.entries[pb.getPath()] = ptr;
- RtLogging::Entry& e = *ptr;
- e.filename = pb.getPath();
- pb.createParentDirectories();
- e.stream.open(e.filename);
- if (!e.stream)
- {
- rtLogging.entries.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)
- {
- for (const auto& pair : aliasNames)
- {
- std::string name = pair.first;
- if (boost::starts_with(dev + "$", name))
- {
- 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;
- }
- }
- return false;
- };
-
- //get logged sensor device values
- {
- e.loggedSensorDeviceValues.reserve(rtLogging.sensorDeviceValueLoggingNames.size());
- for (const auto& svfields : rtLogging.sensorDeviceValueLoggingNames)
- {
- e.loggedSensorDeviceValues.emplace_back();
- auto& svfieldsFlags = e.loggedSensorDeviceValues.back(); //vv
- svfieldsFlags.reserve(svfields.size());
- for (const auto& field : svfields)
- {
- svfieldsFlags.emplace_back(logDev(field));
- }
- }
- }
- //get logged control device values
- {
- e.loggedControlDeviceValues.reserve(rtLogging.controlDeviceValueLoggingNames.size());
- for (const auto& sjctrls : rtLogging.controlDeviceValueLoggingNames)
- {
- 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());
-
- 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;
- }
-
- RemoteReferenceCounterBasePtr armarx::RobotUnit::startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current&)
- {
- StringStringDictionary alias;
- for (const auto& name : loggingNames)
- {
- alias.emplace(name, "");
- }
- return startRtLoggingWithAliasNames(formatString, alias);
- }
-
- void armarx::RobotUnit::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
- {
- if (!isRtLoggingEnabled())
- {
- throw LogicError {"RtLogging is disabled in the config! (set RTLoggingPeriodMs to a non zwero value"};
- }
- std::lock_guard<std::mutex> guard {rtLogging.mutex};
- if (!rtLogging.entries.count(token->getId()))
- {
- throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
- }
- rtLogging.entries.at(token->getId())->marker = marker;
- }
-
- void armarx::RobotUnit::stopRtLogging(const armarx::RemoteReferenceCounterBasePtr& token, const Ice::Current&)
- {
- if (!isRtLoggingEnabled())
- {
- throw LogicError {"RtLogging is disabled in the config! (set RTLoggingPeriodMs to a non zwero value"};
- }
- std::lock_guard<std::mutex> guard {rtLogging.mutex};
- if (!rtLogging.entries.count(token->getId()))
- {
- throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
- }
- ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << rtLogging.entries.at(token->getId())->filename;
- rtLogging.entries.at(token->getId())->stopLogging = true;
- }
-
- Ice::StringSeq armarx::RobotUnit::getLoggingNames(const Ice::Current&) const
- {
- Ice::StringSeq result;
- for (const auto& strs : rtLogging.sensorDeviceValueLoggingNames)
- {
- result.insert(result.end(), strs.begin(), strs.end());
- }
- for (const auto& strvecss : rtLogging.controlDeviceValueLoggingNames)
- {
- for (const auto& strs : strvecss)
- {
- result.insert(result.end(), strs.begin(), strs.end());
- }
- }
- return result;
- }
-
- void armarx::RobotUnit::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
- {
- if (!isRtLoggingEnabled())
- {
- throw LogicError {"RtLogging is disabled in the config! (set RTLoggingPeriodMs to a non zero value"};
- }
- std::lock_guard<std::mutex> guard {rtLogging.mutex};
- 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 " << rtLogging.backlog.size() << " iterations to " << pb.getPath() << ".{csv, messages}";
- //write csv header
- {
- outCSV << "iteration;timestamp";
- for (const auto& vs : rtLogging.sensorDeviceValueLoggingNames)
- {
- for (const auto& s : vs)
- {
- outCSV << ";" << s;
- }
- }
- for (const auto& vvs : rtLogging.controlDeviceValueLoggingNames)
- {
- for (const auto& vs : vvs)
- {
- for (const auto& s : vs)
- {
- outCSV << ";" << s;
- }
- }
- }
- outCSV << std::endl;
- }
-
- for (const detail::ControlThreadOutputBufferEntry& iteration : rtLogging.backlog)
- {
- //write csv data
- {
- outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
- //sens
- {
- for (const SensorValueBase* val : iteration.sensors)
- {
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
- {
- outCSV << ";" << val->getDataFieldAsString(idxField);
- }
- }
- }
- //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
- {
- for (const detail::RtMessageLogEntryBase* msg : iteration.messages.getEntries())
- {
- if (!msg)
- {
- break;
- }
- outMsg << "[" << msg->getTime().toDateTime() << "] iteration "
- << iteration.iteration << ":\n"
- << msg->format() << std::endl;
- }
- outMsg << "\nmessages lost: " << iteration.messages.messagesLost
- << " (required additional "
- << iteration.messages.requiredAdditionalBufferSpace << " bytes, "
- << iteration.messages.requiredAdditionalEntries << " message entries)\n" << std::endl;
- }
- }
- }
-
- const RobotUnit::SelfCollisionAvoidanceData& RobotUnit::getSelfCollisionAvoidanceData() const
- {
- return selfCollisionAvoidanceData;
- }
-
- const VirtualRobot::RobotPtr& RobotUnit::getRtRobot() const
- {
- return rtRobot;
- }
-
- void armarx::RobotUnit::startSelfCollisionAvoidance()
- {
- // fill self collision avoidance data
- // clone robot
- // VirtualRobot::CollisionCheckerPtr cc(new VirtualRobot::CollisionChecker());
- selfCollisionAvoidanceData.robot = this->robot->clone(robot->getName() + "_Clone_SelfCollisionAvoidance");
-
- selfCollisionAvoidanceData.minDistance = getProperty<float>("MinSelfDistance").getValue();
-
- // set collision models
- std::map<std::string, VirtualRobot::SceneObjectSetPtr> colModels;
- const std::string colModelsString = getProperty<std::string>("SelfCollisionModelPairs").getValue();
- std::vector<std::string> entries;
- if (!colModelsString.empty())
- {
- entries = armarx::Split(colModelsString, ";", true, true);
- }
- for (std::string entry : entries)
- {
- // Removing parentheses
- boost::trim_if(entry, boost::is_any_of("{}"));
- std::string first = armarx::Split(entry, ",", true, true)[0];
- std::string second = armarx::Split(entry, ",", true, true)[1];
-
- if (!selfCollisionAvoidanceData.robot->hasRobotNodeSet(first) && first != "FLOOR")
- {
- ARMARX_WARNING << "No RobotNodeSet with name '" << first << "' defined in " << robotFileName << ". Skipping.";
- continue;
- }
- if (!selfCollisionAvoidanceData.robot->hasRobotNodeSet(second) && second != "FLOOR")
- {
- ARMARX_WARNING << "No RobotNodeSet with name '" << second << "' defined in " << robotFileName << ". Skipping.";
- continue;
- }
-
- selfCollisionAvoidanceData.modelPairs.push_back(std::make_pair(first, second));
-
- auto inflateNodeSet = [&](VirtualRobot::RobotNodeSetPtr nodeset)
- {
- for (int i = 0; i < static_cast<int>(nodeset->getSize()); ++i)
- {
- if (nodeset->getNode(i)->getCollisionModel())
- {
- nodeset->getNode(i)->getCollisionModel()->inflateModel(selfCollisionAvoidanceData.minDistance / 2.f);
- }
- else
- {
- ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '" << nodeset->getNode(i)->getName() << "'";
- }
- }
- };
-
- if (colModels.find(first) == colModels.end() && first != "FLOOR")
- {
- inflateNodeSet(selfCollisionAvoidanceData.robot->getRobotNodeSet(first));
- colModels[first] = selfCollisionAvoidanceData.robot->getRobotNodeSet(first);
- }
- if (colModels.find(second) == colModels.end() && second != "FLOOR")
- {
- inflateNodeSet(selfCollisionAvoidanceData.robot->getRobotNodeSet(second));
- colModels[second] = selfCollisionAvoidanceData.robot->getRobotNodeSet(second);
- }
- }
-
- // Adding scene-object representing the floor, to avoid collisions with the floor
- VirtualRobot::SceneObjectSetPtr boxSet(new VirtualRobot::SceneObjectSet("FLOOR"));
- float floorHeight = 10.0f;
- VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(20000, 20000, floorHeight);
- boxOb->getCollisionModel()->inflateModel(selfCollisionAvoidanceData.minDistance / 2);
- Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
-
- boxOb->setGlobalPose(globalPose);
- boxSet->addSceneObject(boxOb);
- colModels["FLOOR"] = boxSet;
-
- selfCollisionAvoidanceData.collisionModels = colModels;
-
- // Setup robot synchronization
- this->selfCollisionAvoidanceRobotSynchronization.reset(new RobotSynchronization(this, selfCollisionAvoidanceData.robot, selfCollisionAvoidanceData.robot->getRobotNodeNames()));
-
- // start the task
- int colChecksPerSecond = getProperty<int>("SelfCollisionChecksPerSecond").getValue();
- int intervalMs = (1.0 / (float) colChecksPerSecond) * 1000;
- selfCollisionAvoidanceTask = new PeriodicTask<RobotUnit>(this, &RobotUnit::updateSelfCollisionAvoidance, intervalMs, false, "SelfCollisionAvoidance");
- selfCollisionAvoidanceTask->setDelayWarningTolerance(200);
- selfCollisionAvoidanceTask->start();
-
- // logging
- std::string validCollisionModelsString;
- for (auto it = selfCollisionAvoidanceData.collisionModels.begin(); it != selfCollisionAvoidanceData.collisionModels.end(); ++it)
- {
- validCollisionModelsString += it->first;
- validCollisionModelsString += " ";
- }
- ARMARX_IMPORTANT << "Started Self Collision Avoidance:" <<
- "\nRobot: " << selfCollisionAvoidanceData.robot->getName() <<
- "\nMin. allowed distance: " << selfCollisionAvoidanceData.minDistance << " mm" <<
- "\nCollision models: " << validCollisionModelsString <<
- "\nCollision checks per second: " << colChecksPerSecond << " (every " << intervalMs << " ms)";
- }
-
- void armarx::RobotUnit::stopSelfCollisionAvoidance()
- {
- ARMARX_IMPORTANT << "Stopping Self Collision Avoidance.";
- if (selfCollisionAvoidanceTask)
- {
- selfCollisionAvoidanceTask->stop();
- }
- }
-
- void armarx::RobotUnit::updateSelfCollisionAvoidance()
- {
- if (emergencyStop || !getProperty<bool>("EnableSelfCollisionAvoidance").getValue())
- {
- return;
- }
- if (!devicesReadyStates.count(state) || state != RobotUnitState::Running)
- {
- ARMARX_VERBOSE << deactivateSpam(1) << "Self Distance Check: Waiting for device initialization.";
- return;
- }
-
- auto startTime = std::chrono::high_resolution_clock::now();
-
- // update joint values
- selfCollisionAvoidanceRobotSynchronization->sync();
-
- // check distances between all collision models
- std::vector<VirtualRobot::SceneObjectSetPtr> conflictingColModels;
- std::string distancesString("All collision states:\n");
- bool anyCollision = false;
- for (auto it = selfCollisionAvoidanceData.modelPairs.begin(); it != selfCollisionAvoidanceData.modelPairs.end(); ++it)
- {
- bool collision = selfCollisionAvoidanceData.robot->getCollisionChecker()->checkCollision(selfCollisionAvoidanceData.collisionModels[it->first], selfCollisionAvoidanceData.collisionModels[it->second]);
- anyCollision |= collision;
- distancesString += "---- " + it->first + " <-> " + it->second + " in Collision: " + (collision ? "True" : "False") + "\n";
- if (collision)
- {
- conflictingColModels.push_back(selfCollisionAvoidanceData.collisionModels[it->first]);
- conflictingColModels.push_back(selfCollisionAvoidanceData.collisionModels[it->second]);
- }
- }
-
- auto endTime = std::chrono::high_resolution_clock::now();
- std::chrono::duration<double> duration = std::chrono::duration_cast<std::chrono::duration<double>>(endTime - startTime);
-
- if (anyCollision)
- {
- // collision
- std::string conflictingColModelsString = "";
- for (std::size_t i = 0; i < conflictingColModels.size(); i++)
- {
- conflictingColModelsString += conflictingColModels.at(i)->getName() + ((i % 2) ? ";" : "<->");
- }
-
- ARMARX_WARNING << "Self Distance Check: Collision. Entering EmergencyStop." <<
- "\nmin. allowed distance: " << selfCollisionAvoidanceData.minDistance << " mm" <<
- "\nConflicting collision models: " << conflictingColModelsString <<
- "\n" << distancesString <<
- "\nComputation time: " << (duration.count() * 1000) << " ms (max. allowed duration (as specified): " << (1000.0 / getProperty<int>("SelfCollisionChecksPerSecond").getValue()) << " ms)";;
-
- setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- // since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
- setActivateControllersRequest({});
- }
- else
- {
- // no collision
- ARMARX_DEBUG << deactivateSpam(5) << "Self Distance Check: OK." <<
- "\nmin. allowed distance: " << selfCollisionAvoidanceData.minDistance << " mm" <<
- "\n" << distancesString <<
- "\nComputation time: " << (duration.count() * 1000) << " ms (max. allowed duration (as specified): " << (1000.0 / getProperty<int>("SelfCollisionChecksPerSecond").getValue()) << " ms)";
- }
-
- }
-
- void armarx::RobotUnit::icePropertiesUpdated(const std::set<std::string>& changedProperties)
- {
-
- 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();
- }
- }
-
- void armarx::RobotUnit::onInitComponent()
- {
- ARMARX_DEBUG << "armarx::RobotUnit::onInitComponent()";
- // EmergencyStopMaster::onInitComponent();
- {
- auto guard = getGuard();
- getArmarXManager()->increaseSchedulers(static_cast<int>(getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue()));
-
- throwIfStateIsNot(RobotUnitState::PreComponentInitialization, __FUNCTION__);
- robotUnitListenerTopicName = getProperty<std::string>("RobotUnitListenerTopicName").getValue();
- offeringTopic(getListenerTopicName());
- offeringTopic(getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue());
-
- ObserverPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
- ObserverPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
-
- //load robot
- {
- robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
- robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
- robotFileName = getProperty<std::string>("RobotFileName").getValue();
- robotPlatformName = getProperty<std::string>("PlatformName").getValue();
- debugObserverSkipIterations = getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
- if (debugObserverSkipIterations < 1)
- {
- debugObserverSkipIterations = 1;
- }
- Ice::StringSeq includePaths;
- if (!robotProjectName.empty())
- {
- CMakePackageFinder finder(robotProjectName);
- Ice::StringSeq projectIncludePaths;
- auto pathsString = finder.getDataDir();
- boost::split(projectIncludePaths,
- pathsString,
- boost::is_any_of(";,"),
- boost::token_compress_on);
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
- }
- if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
- {
- throw UserException("Could not find robot file " + robotFileName);
- }
- // read the robot
- try
- {
- robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
- rtRobot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eStructure);
- rtRobot->setUpdateCollisionModel(false);
- rtRobot->setUpdateVisualization(false);
- rtRobot->setThreadsafe(false);
- }
- catch (VirtualRobot::VirtualRobotException& e)
- {
- throw UserException(e.what());
- }
- ARMARX_INFO << "Loaded robot:"
- << "\n\tProject : " << robotProjectName
- << "\n\tRobot file : " << robotFileName
- << "\n\tRobotNodeSet : " << robotNodeSetName
- << "\n\tNodeNames : " << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
- }
- //ControlDeviceHardwareControlModeGroups
- const std::string controlDeviceHardwareControlModeGroupsStr = getProperty<std::string>("ControlDeviceHardwareControlModeGroups").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)
- {
- 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())
- {
- ARMARX_DEBUG << "adding device group:\n"
- << ARMARX_STREAM_PRINTER
- {
- for (const auto& elem : group)
- {
- out << " " << elem << "\n";
- }
- };
- ctrlModeGroups.groups.emplace_back(std::move(group));
- }
- }
- }
- ARMARX_CHECK_EXPRESSION(robot);
- state = RobotUnitState::InitializingDevices;
- }
- ARMARX_DEBUG << "armarx::RobotUnit::onInitComponent(): onInitRobotUnit()";
- onInitRobotUnit();
- getArmarXManager()->addObject(robotUnitObs);
- ARMARX_DEBUG << "armarx::RobotUnit::onInitComponent(): add emergency stop master";
- {
- emergencyStopMaster = new RobotUnitEmergencyStopMaster {this, 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 << "armarx::RobotUnit::onInitComponent()...done!";
- }
-
- void armarx::RobotUnit::onConnectComponent()
- {
- ARMARX_DEBUG << "RobotUnit::onConnectComponent()";
- listenerPrx = getTopic<RobotUnitListenerPrx>(getListenerTopicName());
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue());
- debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverTopicName").getValue());
- onConnectRobotUnit();
- ARMARX_DEBUG << "RobotUnit::onConnectComponent()...done!";
- }
-
- void armarx::RobotUnit::onDisconnectComponent()
- {
- ARMARX_DEBUG << "RobotUnit::onDisconnectComponent()";
- stopSelfCollisionAvoidance();
-
- // Disconnecting TCPController and TrajectoryController
-
- onDisconnectRobotUnit();
- ARMARX_DEBUG << "RobotUnit::onDisconnectComponent()...done!";
- }
-
- void armarx::RobotUnit::onExitComponent()
- {
- ARMARX_DEBUG << "RobotUnit::onExitComponent()";
- ARMARX_DEBUG << "remove EmergencyStopMaster";
- try
- {
- getArmarXManager()->removeObjectBlocking(ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
- getArmarXManager()->getIceManager()->removeObject(getProperty<std::string>("EmergencyStopMasterName").getValue());
- }
- catch (...) {}
- ARMARX_DEBUG << "onExitRobotUnit()";
- onExitRobotUnit();
- ARMARX_DEBUG << "finishRunning()";
- finishRunning();
- ARMARX_DEBUG << "RobotUnit::onExitComponent()...done!";
- }
-
- void armarx::RobotUnit::rtUpdateSensorAndControlBuffer(
- const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
- {
- rtThreadTimingsSensorDevice->rtMarkRtUpdateSensorAndControlBufferStart();
- SensorAndControl& sc = controlThreadOutputBuffer.getWriteBuffer();
- sc.writeTimestamp = IceUtil::Time::now(); // this has to be in real time
- sc.sensorValuesTimestamp = sensorValuesTimestamp;
- sc.timeSinceLastIteration = timeSinceLastIteration;
- for (std::size_t sensIdx = 0; sensIdx < rtGetNumberOfSensorDevices(); ++sensIdx)
- {
- sensorDevices.at(sensIdx)->getSensorValue()->_copyTo(sc.sensors.at(sensIdx));
- }
- for (std::size_t ctrlIdx = 0; ctrlIdx < controlDevices.size(); ++ctrlIdx)
- {
- ControlDevice& controlDevice = *controlDevices.at(ctrlIdx);
- auto& tv = sc.control.at(ctrlIdx);
- for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetJointControllers().size(); ++targIdx)
- {
- JointController& jointCtrl = *controlDevice.rtGetJointControllers().at(targIdx);
- jointCtrl.getControlTarget()->_copyTo(tv.at(targIdx));
- }
- }
- controlThreadOutputBuffer.commitWrite();
-
- for (auto& node : nodePositionSensorMap)
- {
- node.first->setJointValueNoUpdate(node.second->position);
- }
- rtRobot->applyJointValues();
- rtThreadTimingsSensorDevice->rtMarkRtUpdateSensorAndControlBufferEnd();
- }
-
- void armarx::RobotUnit::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
- {
- auto guard = getGuard();
- 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;
- });
-
- ARMARX_CHECK_EXPRESSION(nNJointCtrls == nJointSet.size());
- ARMARX_CHECK_EXPRESSION(nJointCtrls.size() == 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(), NJointController::IdComp {}));
- ARMARX_CHECK_EXPRESSION(NJointController::AreNotInConflict(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls));
-
- //check JointCtrl
- const auto& jointCtrls = setOfControllers.jointControllers;
- ARMARX_CHECK_EXPRESSION(jointCtrls.size() == getNumberOfControlDevices());
- ARMARX_CHECK_EXPRESSION(std::all_of(jointCtrls.begin(), jointCtrls.end(), [](JointController * p)
- {
- return p;
- }));
-
- //check groups
- {
- ARMARX_DEBUG << "check groups";
- std::size_t grpIdx = 0;
- for (const auto& group : ctrlModeGroups.groups)
- {
- ARMARX_CHECK_EXPRESSION(!group.empty());
- const auto hwMode = setOfControllers.jointControllers.at(controlDevices.index(*group.begin()))->getHardwareControlMode();
- ARMARX_DEBUG << "---- group " << grpIdx << " mode '" << hwMode << "'";
- for (const auto& other : group)
- {
- const auto& dev = controlDevices.index(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
- {
- ARMARX_DEBUG << "setup assignement index";
- setOfControllers.resetAssignement();
- for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < setOfControllers.nJointControllers.size(); ++nJointCtrlIndex)
- {
- const NJointController* nJoint = setOfControllers.nJointControllers.at(nJointCtrlIndex);
- if (!nJoint)
- {
- break;
- }
- ARMARX_DEBUG << "---- " << nJoint->getInstanceName();
- for (std::size_t jointIndex : nJoint->getControlDeviceUsedIndices())
- {
- ARMARX_CHECK_EXPRESSION(setOfControllers.jointToNJointControllerAssignement.at(jointIndex) == IndexSentinel());
- setOfControllers.jointToNJointControllerAssignement.at(jointIndex) = nJointCtrlIndex;
- }
- }
- }
-
- {
- ARMARX_DEBUG << "call checkSetOfControllersToActivate";
- checkSetOfControllersToActivate(setOfControllers);
- }
- //set requested state
- {
- ARMARX_DEBUG << "set requested state for NJoint controllers";
- for (const auto& name2NJoint : nJointControllers)
- {
- const NJointControllerPtr& nJoint = name2NJoint.second;
- SetNJointControllerRequested(nJoint, nJointSet.count(nJoint));
- }
- }
- controllersRequested.getWriteBuffer() = std::move(setOfControllers);
- controllersRequested.commitWrite();
- }
-
- const JointAndNJointControllers& armarx::RobotUnit::getRequestedControllers() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return controllersRequested.getWriteBuffer();
- }
-
- const std::vector<armarx::JointController*>& armarx::RobotUnit::getRequestedJointControllers() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return getRequestedControllers().jointControllers;
- }
-
- const std::vector<armarx::NJointController*>& armarx::RobotUnit::getRequestedNJointControllers() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return getRequestedControllers().nJointControllers;
- }
-
- bool armarx::RobotUnit::activatedControllersChanged() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return controllersActivated.updateReadBuffer();
- }
-
- const JointAndNJointControllers& armarx::RobotUnit::getActivatedControllers() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return controllersActivated.getReadBuffer();
- }
-
- const std::vector<armarx::JointController*>& armarx::RobotUnit::getActivatedJointControllers() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return getActivatedControllers().jointControllers;
- }
-
- const std::vector<armarx::NJointController*>& armarx::RobotUnit::getActivatedNJointControllers() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return getActivatedControllers().nJointControllers;
- }
-
- bool armarx::RobotUnit::sensorAndControlBufferChanged() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return controlThreadOutputBuffer.updateReadBuffer();
- }
-
- const SensorAndControl& armarx::RobotUnit::getSensorAndControlBuffer() const
- {
- auto guard = getGuard();
- throwIfDevicesNotReady(__FUNCTION__);
- return controlThreadOutputBuffer.getReadBuffer();
- }
-
- std::ostream& operator<<(std::ostream& o, RobotUnitState s)
- {
- o << std::to_string(s);
- return o;
- }
-}
-
-void armarx::RobotUnit::RtLogging::doLogging()
-{
- ARMARX_CHECK_EXPRESSION(enabled);
- std::lock_guard<std::mutex> guard {mutex};
- const auto now = IceUtil::Time::now();
- // entries are removed last
-
- //remove backlog entries
- {
- while (!backlog.empty() && backlog.front().writeTimestamp + keepBacklogFor < now)
- {
- backlog.pop_front();
- }
- }
- //log all
- {
- ru->controlThreadOutputBuffer.foreachNewLoggingEntry([&](const ControlThreadOutputBuffer::Entry & data)
- {
- //base (marker;iteration;timestamp)
- {
- for (auto& pair : entries)
- {
- Entry& e = *pair.second;
- e.stream << "\n"
- << e.marker << ";"
- << data.iteration << ";"
- << data.sensorValuesTimestamp.toMicroSeconds();
- e.marker.clear();
- }
- }
- //sens
- {
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
- {
- const SensorValueBase* val = data.sensors.at(idxDev);
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
- {
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& pair : entries)
- {
- Entry& e = *pair.second;
- if (e.loggedSensorDeviceValues.at(idxDev).at(idxField))
- {
- pair.second->stream << ";" << str;
- }
- }
- }
- }
- }
- //ctrl
- {
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
- {
- const auto& vals = data.control.at(idxDev);
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
- {
- const ControlTargetBase* val = vals.at(idxVal);
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
- {
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& pair : entries)
- {
- Entry& e = *pair.second;
- if (e.loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
- {
- pair.second->stream << ";" << str;
- }
- }
- }
- }
- }
- }
- //store data to backlog
- {
- if (data.writeTimestamp + keepBacklogFor >= now)
- {
- backlog.emplace_back(data, true); //true for minimal copy
- }
- }
- //print + reset messages
- {
- for (const detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
- {
- if (!ptr)
- {
- break;
- }
- ptr->print(rtThreadId);
- }
- }
- });
- }
- ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
- //flush all files
- {
- for (auto& pair : entries)
- {
- pair.second->stream << std::flush;
- }
- }
-
- //remove entries
- {
- std::vector<std::string> toRemove;
- toRemove.reserve(entries.size());
- for (auto& pair : entries)
- {
- 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)
- {
- entries.erase(rem);
- }
- }
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 9d16cc9a716a44e0d8bb87895907be8963cac2c8..30541f3206fa56f344b77c524dac669af4b2cedb 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -22,299 +22,58 @@
#pragma once
-#include <thread>
-#include <atomic>
-#include <chrono>
-#include <mutex>
-#include <unordered_map>
-#include <memory>
-#include <fstream>
-
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/util/algorithm.h>
-#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-#include <ArmarXCore/observers/DebugObserver.h>
-#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
-
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/SceneObjectSet.h>
-
-#include "Units/RobotUnitSubUnit.h"
-#include "JointControllers/JointController.h"
+#include "RobotUnitModules/RobotUnitModules.h"
#include "NJointControllers/NJointController.h"
-#include "Devices/ControlDevice.h"
-#include "Devices/SensorDevice.h"
-#include "Devices/RTThreadTimingsSensorDevice.h"
-#include "util.h"
-#include "util/JointAndNJointControllers.h"
-#include "RobotUnitObserver.h"
-#include "RobotSynchronization.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
- class RobotUnitObserver;
- typedef IceInternal::Handle<RobotUnitObserver> RobotUnitObserverPtr;
- class SensorValue1DoFActuatorPosition;
/**
* @class RobotUnitPropertyDefinitions
* @brief
*/
class RobotUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ public RobotUnitModule::UnitsPropertyDefinitions,
+ public RobotUnitModule::DevicesPropertyDefinitions,
+ public RobotUnitModule::LoggingPropertyDefinitions,
+ public RobotUnitModule::RobotDataPropertyDefinitions,
+ public RobotUnitModule::PublisherPropertyDefinitions,
+ public RobotUnitModule::ManagementPropertyDefinitions,
+ public RobotUnitModule::SelfCollisionCheckerPropertyDefinitions
{
public:
RobotUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
- {
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////////////////////////////////// General Config /////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- defineOptionalProperty<std::uint64_t>(
- "AdditionalObjectSchedulerCount", 10,
- "Number of ObjectSchedulers to be added");
-
- 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>(
- "RobotNodeSetName", "Robot",
- "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
- defineOptionalProperty<std::string>(
- "PlatformName", "Platform",
- "Name of the platform (will publish values on PlatformName + 'State')");
-
- defineOptionalProperty<std::string>(
- "ControlDeviceHardwareControlModeGroups", "",
- "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!"
- );
-
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////////////////////////////////// Emergency Stop /////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- 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.");
-
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////////////////////////////// Publishing ///////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- 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);
-
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // /////////////////////////////////////////////////////////////// Units /////////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- defineOptionalProperty<std::string>(
- "KinematicUnitName", "KinematicUnit",
- "The name of the created kinematic 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>(
- "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>(
- "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");
-
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // //////////////////////////////////////////////////////////// Rt Logging ///////////////////////////////////////////////////////////// //
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- defineOptionalProperty<std::size_t>(
- "RTLoggingPeriodMs", 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. "
- "If set to 0, all rt-logging is deactivated");
-
- defineOptionalProperty<std::string>(
- "RTLoggingDefaultLog", "",
- "If rt logging is active and a file path is given, all data is logged to this file.");
-
- defineOptionalProperty<std::size_t>(
- "RTLoggingMessageNumber", 1000,
- "Number of messages that can be logged in the control thread");
- defineOptionalProperty<std::size_t>(
- "RTLoggingMessageBufferSize", 1024 * 1024,
- "Number of bytes that can be logged in the control thread");
- defineOptionalProperty<std::size_t>(
- "RTLoggingMaxMessageNumber", 16000,
- "Max number of messages that can be logged in the control thread");
- defineOptionalProperty<std::size_t>(
- "RTLoggingMaxMessageBufferSize", 16 * 1024 * 1024,
- "Max number of bytes that can be logged in the control thread");
-
- defineOptionalProperty<std::size_t>(
- "RTLoggingKeepIterationsForMs", 60 * 1000,
- "All logging data (SensorValues, ControlTargets, Messages) is keept for this duration "
- "and can be dunped in case of an error.");
-
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////////////////////////////// Self Collision Avoidance ///////////////////////////////////////////////////// //
- // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- defineOptionalProperty<bool>(
- "EnableSelfCollisionAvoidance", true,
- "Whether self collision avoidance should be active or not.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<int>(
- "SelfCollisionChecksPerSecond", 10,
- "Frequency [Hz] of self collision checking (default = 10).", PropertyDefinitionBase::eConstant);
- defineOptionalProperty<float>(
- "MinSelfDistance", 20,
- "Minimum allowed distance (mm) between each collision model before entering emergency stop (default = 20).",
- PropertyDefinitionBase::eConstant);
- defineOptionalProperty<std::string>(
- "SelfCollisionModelPairs", "",
- "Comma seperated list of pairs of collision models 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.", PropertyDefinitionBase::eConstant);
-
- defineOptionalProperty<std::string>(
- "TCPControlUnitName", "TCPControlUnit",
- "Name of the TCPControlUnit.");
- defineOptionalProperty<std::string>(
- "TrajectoryControllerUnitName", "TrajectoryPlayer",
- "Name of the TrajectoryControllerUnit. The respective component outside of the RobotUnit is TrajectoryPlayer");
- }
+ UnitsPropertyDefinitions(prefix),
+ DevicesPropertyDefinitions(prefix),
+ LoggingPropertyDefinitions(prefix),
+ RobotDataPropertyDefinitions(prefix),
+ PublisherPropertyDefinitions(prefix),
+ ManagementPropertyDefinitions(prefix),
+ SelfCollisionCheckerPropertyDefinitions(prefix)
+ {}
};
-
- /// @brief The current state of the multi step initialization of a RobotUnit
- enum class RobotUnitState : std::size_t
- {
- PreComponentInitialization,
- InitializingDevices,
- //ok to use devices/create controllers
- InitializingUnits,
- WaitingForRTThreadInitialization,
- Running,
- Exiting
- };
-}
-namespace std
-{
- std::string to_string(armarx::RobotUnitState s);
}
namespace armarx
{
- std::ostream& operator<<(std::ostream& o, RobotUnitState s);
-
- /**
- * @brief This class is a friend of NJointController.
- *
- * It encapsulates all access to privates of a NJointController
- * to reduce the amount of code which could break invariants
- * in NJointController and to collect all such code in one place
- * to make this problem more manageable.
- *
- * Also the private functions of NJointController should not be made public to prevent
- * other classes (e.g. KinematicSubUnit) or derived NJointControllers
- * from accessing them
- *
- */
- class RobotUnitNJointControllerAccess
- {
- protected:
- static void RtSetNJointControllerActivatedStatus(const NJointControllerPtr& nJointCtrl);
- static void RtSetNJointControllerDeactivatedStatus(const NJointControllerPtr& nJointCtrl);
- static void RtSetNJointControllerDeactivatedBecauseOfErrorStatus(const NJointControllerPtr& nJointCtrl);
- static void InitNJointControler(
- const NJointControllerPtr& nJointCtrl,
- RobotUnit* ru,
- std::size_t ctrlId,
- std::map<std::string, const JointController*> ctrlDeviceUsedJointController,
- std::vector<char> ctrlDeviceUsedBitmap,
- std::vector<std::size_t> ctrlDeviceUsedIndices,
- bool deletable,
- bool internal
- ) noexcept;
- static void PublishNJointController(
- const NJointControllerPtr& nJointCtrl,
- const SensorAndControl& sac,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- );
- bool GetNJointControllerStatusReportedActive(const NJointControllerPtr& nJointCtrl);
- bool GetNJointControllerStatusReportedRequested(const NJointControllerPtr& nJointCtrl);
- void UpdateNJointControllerStatusReported(const NJointControllerPtr& nJointCtrl);
- void SetNJointControllerRequested(const NJointControllerPtr& nJointCtrl, bool requested);
-
- void DeactivateNJointControllerPublishing(
- const NJointControllerPtr& nJointCtrl,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer);
- };
-
/**
* @brief The RobotUnit class manages a robot and its controllers.
*
* The \ref RobotUnit manages \ref NJointController and \ref JointController for a robot.
* Controllers are executed in a control thread.
+ *
+ * \section Controllers
+ *
+ * \subsection RobotUnitCtrlsJoint Joint Controller
+ *
+ * \subsection RobotUnitCtrlsNJoint Multi Joint Controller
+ *
+ * \subsection RobotUnitControlModes Control Modes
+ * \subsubsection RobotUnitCtrlModesJointCtrl Control Modes of JointControlers
+ * \subsubsection RobotUnitCtrlModesHWCtrl Hardware Control Modes of JointControlers
+ * \subsubsection RobotUnitCtrlModeGroups Device Groups of Control Modes
+ *
* \section Units
* Units are created on an as needed basis.
* (e.g. If there is a platform, a \ref PlatformSubUnit is created)
@@ -325,6 +84,7 @@ namespace armarx
* - \ref PlatformSubUnit
* - \ref ForceTorqueSubUnit
*
+ * \section Publishing
* \section RtLogging
* Since the control thread (Section \ref softrt) should adhere soft rt properties,
* no function executed in it is allowed to block.
@@ -340,7 +100,7 @@ namespace armarx
* - \ref ARMARX_RT_LOGF_ERROR
* - \ref ARMARX_RT_LOGF_FATAL
*
- * \warning These functions must only be called in the control thread after calling armarx::RobotUnit::initRTThread
+ * \warning These functions must only be called in the control thread after calling armarx::RobotUnit::finishControlThreadInitialization
*
* These macros work similar to printf and write their content to the corresponding
* armarx logging stream.
@@ -406,520 +166,26 @@ namespace armarx
*/
class RobotUnit :
virtual public RobotUnitInterface,
- virtual public Component,
- virtual public NJointControllerDescriptionProviderInterface,
- virtual private RobotUnitNJointControllerAccess
+ virtual public RobotUnitModule::Units,
+ virtual public RobotUnitModule::Logging,
+ virtual public RobotUnitModule::Devices,
+ virtual public RobotUnitModule::Publisher,
+ virtual public RobotUnitModule::RobotData,
+ virtual public RobotUnitModule::Management,
+ virtual public RobotUnitModule::ControlThread,
+ virtual public RobotUnitModule::SelfCollisionChecker,
+ virtual public RobotUnitModule::ControllerManagement,
+ virtual public RobotUnitModule::ControlThreadDataBuffer
{
- protected:
- using PublisherTaskT = SimplePeriodicTask<std::function<void(void)>>;
- using RTLoggingTaskT = SimplePeriodicTask<std::function<void(void)>>;
public:
- using MutexType = std::recursive_mutex;
- using GuardType = std::unique_lock<MutexType>;
- using ClockT = std::chrono::high_resolution_clock;
- using TimePointT = std::chrono::high_resolution_clock::time_point;
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // data
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- //devices
- private:
- /**
- * @brief ControlDevices added to this unit
- * data may only be added during and only be used after State::InitializingDevices
- */
- KeyValueVector<std::string, ControlDevicePtr> controlDevices;
- /**
- * @brief SensorDevices added to this unit
- * data may only be added during and only be used after State::InitializingDevices
- */
- KeyValueVector<std::string, SensorDevicePtr > sensorDevices;
-
- /// @brief const pointer to all ControlDevices (passed to GenerateConfigDescription of a NJointController)
- std::map<std::string, ConstControlDevicePtr> controlDevicesConstPtr;
- /// @brief const pointer to all SensorDevices (passed to GenerateConfigDescription of a NJointController)
- std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
-
- /// @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;
- //[triple] buffers
- 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 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 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;
- //(instantiating) NJoint controllers
- private:
- /// data may only be used after State::InitializingDevices
- std::thread::id initNJointThreadId;
- /// data may only be used after State::InitializingDevices
- std::map<std::string, const JointController*> initNJointControllerUsedJointController;
- /// @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 the id of the next NJointController instance that is created
- std::size_t nJointControllerNextId {0};
-
- //listeners
- private:
- std::string robotUnitListenerTopicName;
- RobotUnitListenerPrx listenerPrx;
- DebugDrawerInterfacePrx debugDrawerPrx;
-
- //publishing
- private:
- std::uint64_t publishIterationCount {0};
-
- IceUtil::Time publishNewSensorDataTime;
- PublisherTaskT::pointer_type publisherTask;
- std::set<std::string> lastReportedClasses;
-
- std::atomic_bool ObserverPublishSensorValues;
- std::atomic_bool ObserverPublishControlTargets;
- std::atomic<std::uint64_t> debugObserverSkipIterations;
-
- std::chrono::microseconds lastRobotUnitObserverFlush;
- std::chrono::microseconds lastPublishLoop;
- DebugObserverInterfacePrx debugObserverPrx;
- RobotUnitObserverPtr robotUnitObs;
-
- ManagedIceObjectPtr tcpControllerSubUnit;
- ManagedIceObjectPtr trajectoryControllerSubUnit;
-
- //rt logging
- private:
- struct RtLogging
+ static RobotUnit& Instance()
{
- RtLogging(RobotUnit* ru): ru {ru} {}
- struct Entry
- {
- std::atomic_bool stopLogging {false};
- std::vector<std::vector<std::vector<char>>> loggedControlDeviceValues;
- std::vector<std::vector<char>> loggedSensorDeviceValues;
- std::ofstream stream;
- std::string filename;
- std::string marker;
- };
- mutable std::mutex mutex;
- std::map<std::string, std::shared_ptr<Entry>> entries;
- bool enabled {false};
- std::vector<std::vector<std::vector<std::string>>> controlDeviceValueLoggingNames;
- std::vector<std::vector< std::string >> sensorDeviceValueLoggingNames;
-
- RobotUnit* const ru;
- RTLoggingTaskT::pointer_type task;
-
- RemoteReferenceCounterBasePtr defaultLogHandle;
-
- std::deque<detail::ControlThreadOutputBufferEntry> backlog;
- IceUtil::Time keepBacklogFor;
-
- std::size_t loggingTaskTimestep {0};
-
- Ice::Int rtThreadId {0};
- void doLogging();
- };
- RtLogging rtLogging;
-
- //other
- private:
- /// @brief The RobotUnit's state
- std::atomic<RobotUnitState> state {RobotUnitState::PreComponentInitialization};
- /// @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;
- std::thread::id rtThreadId;
- mutable MutexType dataMutex;
- static const std::set<RobotUnitState> devicesReadyStates;
-
- std::atomic_bool emergencyStop {false};
- std::atomic_bool rtIsInEmergencyStop {false};
- EmergencyStopMasterInterfacePtr emergencyStopMaster;
-
- 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;
- };
- ControlDeviceHardwareControlModeGroups ctrlModeGroups;
-
- struct SelfCollisionAvoidanceData
- {
- VirtualRobot::RobotPtr robot;
- std::map<std::string, VirtualRobot::SceneObjectSetPtr> collisionModels;
- float minDistance {0}; // min allowed distance (mm) between each collision model
- std::vector<std::pair<std::string, std::string>> modelPairs; // list of pairs of collision models that should be checked agaisnt each other
- };
-
- //robot
- private:
- boost::shared_ptr<VirtualRobot::Robot> robot;
- std::string robotNodeSetName;
- std::string robotProjectName;
- std::string robotFileName;
- std::string robotPlatformName;
-
- VirtualRobot::RobotPtr rtRobot;
- VirtualRobot::RobotNodeSetPtr rtRobotJointSet;
- std::map<VirtualRobot::RobotNodePtr, const SensorValue1DoFActuatorPosition*> nodePositionSensorMap;
-
- SelfCollisionAvoidanceData selfCollisionAvoidanceData;
- PeriodicTask<RobotUnit>::pointer_type selfCollisionAvoidanceTask;
- void startSelfCollisionAvoidance();
- void stopSelfCollisionAvoidance();
- void updateSelfCollisionAvoidance();
-
- RobotSynchronizationPtr selfCollisionAvoidanceRobotSynchronization;
-
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // util
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- template<class ExceptT = LogicError>
- inline void throwIfStateIsNot(const std::set<RobotUnitState>& sinit, const std::string& fnc, bool onlyWarn = false) const;
- template<class ExceptT = LogicError>
- inline void throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
- template<class ExceptT = LogicError>
- inline void throwIfDevicesNotReady(const std::string& fnc) const;
-
- template<class Cont>
- static Ice::StringSeq GetNonNullNames(const Cont& c);
-
- GuardType getGuard() const;
- inline const KeyValueVector<std::string, SensorDevicePtr>& getSensorDevices() const;
- inline const KeyValueVector<std::string, ControlDevicePtr>& getControlDevices() const;
-
- void checkNJointControllerClassName(const std::string& className) const;
-
- //time
- static TimePointT Now();
- static std::chrono::microseconds MicroToNow(TimePointT last);
- static std::chrono::microseconds MicroNow();
-
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // other
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- public:
- RobotUnit(): rtLogging {this} {}
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- std::string getDefaultName() const override;
-
- static constexpr std::size_t IndexSentinel();
- public:
- void icePropertiesUpdated(const std::set<std::string>& changedProperties) override;
- protected:
- virtual void onInitRobotUnit();
- virtual void onConnectRobotUnit();
- virtual void onDisconnectRobotUnit();
- /// @brief called in RobotUnit::onExitComponent() before calling RobotUnit::finishRunning
- virtual void onExitRobotUnit();
- void onInitComponent() final override;
- void onConnectComponent() final override;
- void onDisconnectComponent() final override;
- void onExitComponent() final override;
- private:
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // TripleBuffer
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- //rt requested
- bool rtRequestedControllersChanged() const;
- const JointAndNJointControllers& rtGetRequestedControllers() const;
- const std::vector<JointController*>& rtGetRequestedJointControllers() const;
- const std::vector<NJointController*>& rtGetRequestedNJointControllers() const;
- // rt activated
- void rtWriteActivatedControllers();
- JointAndNJointControllers& rtGetActivatedControllers();
- std::vector<JointController*>& rtGetActivatedJointControllers();
- std::vector<NJointController*>& rtGetActivatedNJointControllers();
- std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement();
- // rt sensor and control
- //declaration further down
- //void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- // non rt requested
- private:
- void writeRequestedControllers(JointAndNJointControllers&& setOfControllers);
- const JointAndNJointControllers& getRequestedControllers() const;
- const std::vector<JointController*>& getRequestedJointControllers() const;
- const std::vector<NJointController*>& getRequestedNJointControllers() const;
- // non rt activated
- private:
- bool activatedControllersChanged() const;
- const JointAndNJointControllers& getActivatedControllers() const;
- const std::vector<JointController*>& getActivatedJointControllers() const;
- const std::vector<NJointController*>& getActivatedNJointControllers() const;
- // non rt sensor and control
- private:
- bool sensorAndControlBufferChanged() const;
- const SensorAndControl& getSensorAndControlBuffer() const;
-
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // State: InitializingDevices
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- void addControlDevice(const ControlDevicePtr& cd);
- void addSensorDevice(const SensorDevicePtr& sd);
-
- virtual RTThreadTimingsSensorDevicePtr createRTThreadTimingSensorDevice() const;
-
- void finishDeviceInitialization();
-
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // State: InitializingUnits
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- protected:
- virtual void initializeDefaultUnits();
- virtual void initializeKinematicUnit();
- virtual void initializePlatformUnit();
- virtual void initializeForceTorqueUnit();
- virtual void initializeInertialMeasurementUnit();
- virtual void initializeTrajectoryControllerUnit();
- virtual void initializeTcpControllerUnit();
- void addUnit(ManagedIceObjectPtr unit);
- void finishUnitInitialization();
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // State: Running (RT)
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- virtual void initRTThread();
-
- //switching helper (rt)
- void rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex, bool writeActivatedControlers = true);
- /**
- * @brief Deactivates a NJointController and sets all used ControlDevices to EmergencyStop
- * @param nJointCtrlIndex The NJointController to deactivate (index in controllersActivated)
- * @param writeActivatedControlers Whether controllersActivated should be committed
- */
- void rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex, bool writeActivatedControlers = true);
- /**
- * @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*/);
-
- //run (these functions should be called in the control loop)
- bool rtSwitchControllerSetup();
- void rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- void rtHandleInvalidTargets();
- /**
- * @brief Runs Joint controllers and writes target values to ControlDevices
- * @param sensorValuesTimestamp
- * @param timeSinceLastIteration
- */
- void rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- //rtBusSendReceive call this robot specific function here
- void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- void rtResetAllTargets();
-
- RTThreadTimingsSensorDevice& rtGetRTThreadTimingsSensorDevice();
-
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // State: InitializingUnits + Running (NON RT)
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- public:
- //create NJoint controllers
- ConstSensorDevicePtr getSensorDevice(const std::string& sensorDeviceName) const override;
- ConstControlDevicePtr getControlDevice(const std::string& deviceName) const override;
- ControlTargetBase* getControlTarget(const std::string& deviceName, const std::string& controlMode) override;
- NJointControllerInterfacePrx createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current&) override;
- NJointControllerInterfacePrx createNJointControllerFromVariantConfig(
- const std::string& className, const std::string& instanceName,
- const StringVariantBaseMap& variants, const Ice::Current&) override;
- const NJointControllerPtr& createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, bool deletable, bool internal);
- //deleting NJoint controllers
- void deleteNJointController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
- void deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // State: Running (NON RT)
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- public:
- //switching
- void activateNJointController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
- void activateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
- void deactivateNJointController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
- void deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
- void switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current& = GlobalIceCurrent) override;
- private:
- void setActivateControllersRequest(std::set<NJointControllerPtr, NJointController::IdComp>&& ctrls);
- /**
- * @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;
- // publishing
- protected:
- virtual void publish(StringVariantBaseMap additionalMap = {}, StringVariantBaseMap timingMap = {});
-
- //shutting down
- protected:
- ///@brief joint your thread in this hook. (used by RobotUnit::finishRunning())
- virtual void joinRTThread() = 0;
- private:
- ///@brief delete all units (used by RobotUnit::finishRunning())
- virtual void removeAllUnits();
- void finishRunning();
-
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // Lib loading
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- public:
- bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) override
- {
- return getArmarXManager()->loadLibFromPath(path);
- }
- bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) override
- {
- return getArmarXManager()->loadLibFromPackage(package, lib);
- }
-
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // querry data
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- public:
- //NJoint
- Ice::StringSeq getNJointControllerNames(const Ice::Current& = GlobalIceCurrent) const override;
- Ice::StringSeq getRequestedNJointControllerNames(const Ice::Current& = GlobalIceCurrent) const override;
- Ice::StringSeq getActivatedNJointControllerNames(const Ice::Current& = GlobalIceCurrent) const override;
-
- NJointControllerInterfacePrx getNJointController(const std::string& name, const Ice::Current&) const override;
- StringNJointControllerPrxDictionary getAllNJointControllers(const Ice::Current&) const override;
- std::vector<NJointControllerPtr> getNJointControllersNotNull(const std::vector<std::string>& names) const;
- const NJointControllerPtr& getNJointControllerNotNull(const std::string& name) const;
-
- NJointControllerStatus getNJointControllerStatus(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
- NJointControllerStatusSeq getNJointControllerStatuses(const Ice::Current&) const override;
-
- NJointControllerDescription getNJointControllerDescription(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
- NJointControllerDescriptionSeq getNJointControllerDescriptions(const Ice::Current&) const override;
-
- NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(
- const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
- NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const override;
- //control devices
- Ice::StringSeq getControlDeviceNames(const Ice::Current& = GlobalIceCurrent) const override;
- ControlDeviceDescription getControlDeviceDescription(std::size_t cdidx) const;
- ControlDeviceDescription getControlDeviceDescription(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
- ControlDeviceDescriptionSeq getControlDeviceDescriptions(const Ice::Current&) const override;
- ControlDeviceStatus getControlDeviceStatus(std::size_t cdidx) const;
- ControlDeviceStatus getControlDeviceStatus(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
- ControlDeviceStatusSeq getControlDeviceStatuses(const Ice::Current&) const override;
-
- //sensor devices
- Ice::StringSeq getSensorDeviceNames(const Ice::Current&) const override;
- SensorDeviceDescription getSensorDeviceDescription(std::size_t cdidx) const;
- SensorDeviceDescription getSensorDeviceDescription(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
- SensorDeviceDescriptionSeq getSensorDeviceDescriptions(const Ice::Current&) const override;
- SensorDeviceStatus getSensorDeviceStatus(std::size_t idx) const;
- SensorDeviceStatus getSensorDeviceStatus(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
- SensorDeviceStatusSeq getSensorDeviceStatuses(const Ice::Current&) const override;
-
- //nJointController classes
- Ice::StringSeq getNJointControllerClassNames(const Ice::Current& = GlobalIceCurrent) const override;
- NJointControllerClassDescription getNJointControllerClassDescription(
- const std::string& className, const Ice::Current& = GlobalIceCurrent) const override;
- NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(const Ice::Current&) const override;
-
- //units
- Ice::ObjectProxySeq getUnits(const Ice::Current& = GlobalIceCurrent) const override;
- Ice::ObjectPrx getUnit(const std::string& staticIceId, const Ice::Current&) const override;
- const ManagedIceObjectPtr& getUnit(const std::string& staticIceId) const;
- KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const override;
- ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const override;
- InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const override;
- PlatformUnitInterfacePrx getPlatformUnit(const Ice::Current& = GlobalIceCurrent) const override;
- //other ice functions
- std::string getListenerTopicName(const Ice::Current& = GlobalIceCurrent) const override;
- bool isRunning(const Ice::Current& = GlobalIceCurrent) const override
- {
- return getRobotUnitState() == RobotUnitState::Running;
+ return ModuleBase::Instance<RobotUnit>();
}
- //other local functions
- const boost::shared_ptr<VirtualRobot::Robot>& getRobot() const;
- std::string getRobotName() const;
- const std::string& getRobotNodetSeName() const;
- const std::string& getRobotProjectName() const;
- const std::string& getRobotFileName() const;
- const std::string& getRobotPlatformName() const;
-
- std::size_t getNumberOfControlDevices() const;
- std::size_t rtGetNumberOfControlDevices() const;
-
- std::size_t getNumberOfSensorDevices() const;
- std::size_t rtGetNumberOfSensorDevices() const;
-
- const DebugDrawerInterfacePrx& getDebugDrawerProxy() const;
- bool areDevicesReady() const;
- RobotUnitState getRobotUnitState() const;
- std::string getName() const override;
-
- void setEmergencyStopState(EmergencyStopState state);
-
- EmergencyStopState getEmergencyStopState() const;
- const EmergencyStopMasterInterfacePtr& getEmergencyStopMaster() const
- {
- return emergencyStopMaster;
- }
-
- const SelfCollisionAvoidanceData& getSelfCollisionAvoidanceData() const;
-
- const VirtualRobot::RobotPtr& getRtRobot() const;
-
-
- virtual IceUtil::Time getControlThreadTargetPeriod() const = 0;
-
- protected:
- const RobotUnitListenerPrx& getListenerProxy() const;
- void icePropertiesInitialized() override;
-
- // RT-Logging
- public:
- bool isRtLoggingEnabled(const Ice::Current& = GlobalIceCurrent) const override
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
{
- return rtLogging.enabled;
+ return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
}
- RemoteReferenceCounterBasePtr startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current& = GlobalIceCurrent) override;
- RemoteReferenceCounterBasePtr startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current& = GlobalIceCurrent) override;
- void addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current& = GlobalIceCurrent) override;
- void stopRtLogging(const RemoteReferenceCounterBasePtr& token, const Ice::Current& = GlobalIceCurrent) override;
-
- Ice::StringSeq getLoggingNames(const Ice::Current& = GlobalIceCurrent) const override;
-
- void writeRecentIterationsToFile(const std::string& formatString, const Ice::Current& = GlobalIceCurrent) const override;
-
};
}
-
-#include "RobotUnit.ipp"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.ipp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.ipp
deleted file mode 100644
index df0a0e814d6c1de57f826e7d5624bc5e7ecadca2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.ipp
+++ /dev/null
@@ -1,398 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::RobotUnit
- * @author Raphael ( ufdrv at student dot kit dot edu )
- * @date 2017
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include "RobotUnit.h"
-
-//inline functions
-namespace armarx
-{
- //RobotUnit
- inline const KeyValueVector<std::string, SensorDevicePtr>& RobotUnit::getSensorDevices() const
- {
- return sensorDevices;
- }
-
- inline const KeyValueVector<std::string, ControlDevicePtr>& RobotUnit::getControlDevices() const
- {
- return controlDevices;
- }
-
- inline RobotUnit::TimePointT RobotUnit::Now()
- {
- return ClockT::now();
- }
-
- inline std::chrono::microseconds RobotUnit::MicroToNow(RobotUnit::TimePointT last)
- {
- return std::chrono::duration_cast<std::chrono::microseconds>(Now() - last);
- }
-
- inline std::chrono::microseconds RobotUnit::MicroNow()
- {
- return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
- }
-
- inline PropertyDefinitionsPtr RobotUnit::createPropertyDefinitions()
- {
- return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
- }
-
- inline std::string RobotUnit::getDefaultName() const
- {
- return "RobotUnit";
- }
-
- inline constexpr std::size_t RobotUnit::IndexSentinel()
- {
- return JointAndNJointControllers::Sentinel();
- }
-
- inline void RobotUnit::onInitRobotUnit() {}
-
- inline void RobotUnit::onConnectRobotUnit() {}
-
- inline void RobotUnit::onDisconnectRobotUnit() {}
-
- inline void RobotUnit::onExitRobotUnit() {}
-
- inline bool RobotUnit::rtRequestedControllersChanged() const
- {
- return controllersRequested.updateReadBuffer();
- }
-
- inline const JointAndNJointControllers& RobotUnit::rtGetRequestedControllers() const
- {
- return controllersRequested.getReadBuffer();
- }
-
- inline const std::vector<JointController*>& RobotUnit::rtGetRequestedJointControllers() const
- {
- return rtGetRequestedControllers().jointControllers;
- }
-
- inline const std::vector<NJointController*>& RobotUnit::rtGetRequestedNJointControllers() const
- {
- return rtGetRequestedControllers().nJointControllers;
- }
-
- inline void RobotUnit::rtWriteActivatedControllers()
- {
- controllersActivated.commitWrite();
- }
-
- inline JointAndNJointControllers& RobotUnit::rtGetActivatedControllers()
- {
- return controllersActivated.getWriteBuffer();
- }
-
- inline std::vector<JointController*>& RobotUnit::rtGetActivatedJointControllers()
- {
- return rtGetActivatedControllers().jointControllers;
- }
-
- inline std::vector<NJointController*>& RobotUnit::rtGetActivatedNJointControllers()
- {
- return rtGetActivatedControllers().nJointControllers;
- }
-
- inline std::vector<std::size_t>& RobotUnit::rtGetActivatedJointToNJointControllerAssignement()
- {
- return rtGetActivatedControllers().jointToNJointControllerAssignement;
- }
-
- inline RTThreadTimingsSensorDevicePtr RobotUnit::createRTThreadTimingSensorDevice() const
- {
- return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
- }
-
- inline void RobotUnit::rtDeactivatedNJointControllerBecauseOfError(const NJointControllerPtr&) {}
-
- inline RTThreadTimingsSensorDevice& RobotUnit::rtGetRTThreadTimingsSensorDevice()
- {
- return *rtThreadTimingsSensorDevice;
- }
-
- inline NJointControllerInterfacePrx RobotUnit::createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current&
- )
- {
- //no lock required
- return NJointControllerInterfacePrx::uncheckedCast(
- createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
- }
-
- inline void RobotUnit::deleteNJointController(const std::string& name, const Ice::Current&)
- {
- deleteNJointControllers({name});
- }
-
- inline void RobotUnit::activateNJointController(const std::string& name, const Ice::Current&)
- {
- activateNJointControllers({name});
- }
-
- inline void RobotUnit::deactivateNJointController(const std::string& name, const Ice::Current&)
- {
- deactivateNJointControllers(Ice::StringSeq {name});
- }
-
- inline void RobotUnit::checkSetOfControllersToActivate(const JointAndNJointControllers&) const {}
-
- inline Ice::StringSeq RobotUnit::getNJointControllerNames(const Ice::Current&) const
- {
- auto guard = getGuard();
- return getMapKeys(nJointControllers);
- }
-
- inline Ice::StringSeq RobotUnit::getRequestedNJointControllerNames(const Ice::Current&) const
- {
- auto guard = getGuard();
- return GetNonNullNames(getRequestedNJointControllers());
- }
-
- inline Ice::StringSeq RobotUnit::getActivatedNJointControllerNames(const Ice::Current&) const
- {
- auto guard = getGuard();
- return GetNonNullNames(getActivatedNJointControllers());
- }
-
- inline Ice::StringSeq RobotUnit::getNJointControllerClassNames(const Ice::Current&) const
- {
- return NJointControllerRegistry::getKeys();
- }
-
- inline KinematicUnitInterfacePrx RobotUnit::getKinematicUnit(const Ice::Current&) const
- {
- //no lock required
- return KinematicUnitInterfacePrx::uncheckedCast(getUnit(KinematicUnitInterface::ice_staticId(), GlobalIceCurrent));
- }
-
- inline ForceTorqueUnitInterfacePrx RobotUnit::getForceTorqueUnit(const Ice::Current&) const
- {
- //no lock required
- return ForceTorqueUnitInterfacePrx::uncheckedCast(getUnit(ForceTorqueUnitInterface::ice_staticId(), GlobalIceCurrent));
- }
-
- inline InertialMeasurementUnitInterfacePrx RobotUnit::getInertialMeasurementUnit(const Ice::Current&) const
- {
- //no lock required
- return InertialMeasurementUnitInterfacePrx::uncheckedCast(
- getUnit(InertialMeasurementUnitInterface::ice_staticId(), GlobalIceCurrent));
- }
-
- inline PlatformUnitInterfacePrx RobotUnit::getPlatformUnit(const Ice::Current&) const
- {
- //no lock required
- return PlatformUnitInterfacePrx::uncheckedCast(getUnit(PlatformUnitInterface::ice_staticId(), GlobalIceCurrent));
- }
-
- inline std::string RobotUnit::getListenerTopicName(const Ice::Current&) const
- {
- auto guard = getGuard();
- return robotUnitListenerTopicName;
- }
-
- inline const std::string& RobotUnit::getRobotNodetSeName() const
- {
- return robotNodeSetName;
- }
-
- inline const std::string& RobotUnit::getRobotProjectName() const
- {
- return robotProjectName;
- }
-
- inline const std::string& RobotUnit::getRobotFileName() const
- {
- return robotFileName;
- }
-
- inline const std::string& RobotUnit::getRobotPlatformName() const
- {
- return robotPlatformName;
- }
-
- inline std::size_t RobotUnit::rtGetNumberOfControlDevices() const
- {
- return controlDevices.size();
- }
-
- inline std::size_t RobotUnit::rtGetNumberOfSensorDevices() const
- {
- return sensorDevices.size();
- }
-
- inline bool RobotUnit::areDevicesReady() const
- {
- return devicesReadyStates.count(state);
- }
-
- inline RobotUnitState RobotUnit::getRobotUnitState() const
- {
- return state;
- }
-
- inline std::string RobotUnit::getName() const
- {
- return Component::getName();
- }
-
- inline void RobotUnit::setEmergencyStopState(EmergencyStopState state)
- {
- emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
- }
-
- inline EmergencyStopState RobotUnit::getEmergencyStopState() const
- {
- return emergencyStop ? EmergencyStopState::eEmergencyStopActive :
- EmergencyStopState::eEmergencyStopInactive;
- }
-
- template<class ExceptT>
- inline void RobotUnit::throwIfStateIsNot(const std::set<RobotUnitState>& sinit, const std::string& fnc, bool onlyWarn) const
- {
- std::set<RobotUnitState> sset {sinit};
- if (! sset.count(state))
- {
- std::stringstream ss;
- ss << fnc << ": Can't be called if state is not in {";
- bool fst = true;
- for (RobotUnitState st : sset)
- {
- if (!fst)
- {
- ss << ", ";
- }
- ss << st;
- fst = false;
- }
- ss << "} (current state " << getRobotUnitState() << ")";
- ARMARX_ERROR << ss.str();
- if (!onlyWarn)
- {
- throw ExceptT {ss.str()};
- }
- }
- }
-
- template<class ExceptT>
- inline void RobotUnit::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
- {
- throwIfStateIsNot<ExceptT>(std::set<RobotUnitState> {s}, fnc, onlyWarn);
- }
-
- template<class ExceptT>
- inline void RobotUnit::throwIfDevicesNotReady(const std::string& fnc) const
- {
- throwIfStateIsNot<ExceptT>(devicesReadyStates, fnc);
- }
-
- template<class Cont>
- inline Ice::StringSeq RobotUnit::GetNonNullNames(const Cont& c)
- {
- Ice::StringSeq result;
- result.reserve(c.size());
- for (const auto& e : c)
- {
- if (e)
- {
- result.emplace_back(e->getName());
- }
- }
- return result;
- }
-
- //RobotUnitNJointControllerAccess
- inline void RobotUnitNJointControllerAccess::RtSetNJointControllerActivatedStatus(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtActivateController();
- }
-
- inline void RobotUnitNJointControllerAccess::RtSetNJointControllerDeactivatedStatus(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtDeactivateController();
- }
-
- inline void RobotUnitNJointControllerAccess::RtSetNJointControllerDeactivatedBecauseOfErrorStatus(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->rtDeactivateControllerBecauseOfError();
- }
-
- inline void RobotUnitNJointControllerAccess::InitNJointControler(
- const NJointControllerPtr& nJointCtrl,
- RobotUnit* ru,
- std::size_t ctrlId,
- std::map<std::string, const JointController*> ctrlDeviceUsedJointController,
- std::vector<char> ctrlDeviceUsedBitmap,
- std::vector<std::size_t> ctrlDeviceUsedIndices,
- bool deletable, bool internal
- ) noexcept
- {
- nJointCtrl->robotUnitInit(
- ru, ctrlId, std::move(ctrlDeviceUsedJointController), std::move(ctrlDeviceUsedBitmap),
- std::move(ctrlDeviceUsedIndices), deletable, internal
- );
- }
-
- inline void RobotUnitNJointControllerAccess::PublishNJointController(
- const NJointControllerPtr& nJointCtrl,
- const SensorAndControl& sac,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
- {
- nJointCtrl ->publish(sac, draw, observer);
- }
-
- inline bool RobotUnitNJointControllerAccess::GetNJointControllerStatusReportedActive(const NJointControllerPtr& nJointCtrl)
- {
- return nJointCtrl->statusReportedActive;
- }
-
- inline bool RobotUnitNJointControllerAccess::GetNJointControllerStatusReportedRequested(const NJointControllerPtr& nJointCtrl)
- {
- return nJointCtrl->statusReportedRequested;
- }
-
- inline void RobotUnitNJointControllerAccess::UpdateNJointControllerStatusReported(const NJointControllerPtr& nJointCtrl)
- {
- nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
- nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
- }
-
- inline void RobotUnitNJointControllerAccess::SetNJointControllerRequested(const NJointControllerPtr& nJointCtrl, bool requested)
- {
- nJointCtrl->isRequested = requested;
- }
-
- inline void RobotUnitNJointControllerAccess::DeactivateNJointControllerPublishing(
- const NJointControllerPtr& nJointCtrl,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
- {
- nJointCtrl->deactivatePublish(draw, observer);
- }
-}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..48fb28cd78bff5150b73cd683847500305b8ba18
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -0,0 +1,480 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/Preprocessor.h>
+
+#include "../NJointControllers/NJointController.h"
+#include "RobotUnitModuleBase.h"
+#include "RobotUnitModules.h"
+
+namespace armarx
+{
+
+std::string to_string(RobotUnitState s)
+{
+ switch (s)
+ {
+ case armarx::RobotUnitState::InitializingComponent:
+ return "RobotUnitState::InitializingComponent";
+ case armarx::RobotUnitState::InitializingDevices:
+ return "RobotUnitState::InitializingDevices";
+ case armarx::RobotUnitState::InitializingUnits:
+ return "RobotUnitState::InitializingUnits";
+ case armarx::RobotUnitState::InitializingControlThread:
+ return "RobotUnitState::InitializingControlThread";
+ case armarx::RobotUnitState::Running:
+ return "RobotUnitState::Running";
+ case armarx::RobotUnitState::Exiting:
+ return "RobotUnitState::Exiting";
+ }
+ throw std::invalid_argument
+ {
+ "Unknown state " + to_string(static_cast<std::size_t>(s)) +
+ "\nStacktrace\n" + LogSender::CreateBackTrace()
+ };
+}
+namespace RobotUnitModule
+{
+
+ // //////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////// call for each module /////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////// //
+ #define cast_to_and_call(Type, fn, rethrow) \
+ try \
+ { \
+ dynamic_cast<Type*>(this)->Type::fn; \
+ } \
+ catch (Ice::Exception& e) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" \
+ << e.what() \
+ << "\n\tname: " << e.ice_name() \
+ << "\n\tfile: " << e.ice_file() \
+ << "\n\tline: " << e.ice_line() \
+ << "\n\tstack: " << e.ice_stackTrace(); \
+ if(rethrow) { throw;} \
+ } \
+ catch (std::exception& e) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" << e.what(); \
+ if(rethrow) { throw;} \
+ } \
+ catch (...) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!"; \
+ if(rethrow) { throw;} \
+ }
+
+ #define for_each_module_apply(r, data, elem) data(elem)
+ #define for_each_module(macro) \
+ BOOST_PP_SEQ_FOR_EACH(for_each_module_apply, macro, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
+
+ #define check_base(Type) \
+ static_assert( \
+ std::is_base_of<ModuleBase, Type>::value, \
+ "The RobotUnitModule '" #Type "' has to derived ModuleBase" \
+ );
+ for_each_module(check_base)
+ #undef check_base
+
+ 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)
+ #undef check_deriving
+ }
+ // //////////////////////////////////////////////////////////////////////////// //
+ // ManagedIceObject life cycle
+ void ModuleBase::onInitComponent()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnInitRobotUnit(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+
+ onInitRobotUnit();
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+
+ finishComponentInitialization();
+
+ }
+
+ void ModuleBase::onConnectComponent()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+
+ onConnectRobotUnit();
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnConnectRobotUnit(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+ void ModuleBase::onDisconnectComponent()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+
+ onDisconnectRobotUnit();
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+
+ void ModuleBase::onExitComponent()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
+
+ finishRunning();
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+
+ onExitRobotUnit();
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnExitRobotUnit(), false)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+ // //////////////////////////////////////////////////////////////////////////// //
+ // other ManagedIceObject / Component methods
+ void ModuleBase::icePropertiesUpdated(const std::set<std::string>& changedProperties)
+ {
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesUpdated(changedProperties), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+ void ModuleBase::icePropertiesInitialized()
+ {
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesInitialized(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+ // //////////////////////////////////////////////////////////////////////////// //
+ // 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)
+ #undef call_module_hook
+
+ setRobotUnitState(RobotUnitState::InitializingDevices);
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishComponentInitialization(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+ 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)
+ #undef call_module_hook
+
+ setRobotUnitState(RobotUnitState::InitializingUnits);
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishDeviceInitialization(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+
+ 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)
+ #undef call_module_hook
+
+ setRobotUnitState(RobotUnitState::InitializingControlThread);
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishUnitInitialization(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+
+ 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)
+ #undef call_module_hook
+
+ setRobotUnitState(RobotUnitState::Running);
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishControlThreadInitialization(), true)
+ for_each_module(call_module_hook)
+ #undef call_module_hook
+ }
+
+ void ModuleBase::finishRunning()
+ {
+ checkDerivedClasses();
+ shutDown();
+ GuardType guard {dataMutex}; //DO NOT USE getGuard here!
+ if(getRobotUnitState() != RobotUnitState::Running)
+ {
+ 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)
+ #undef call_module_hook
+
+ setRobotUnitState(RobotUnitState::Exiting);
+ joinControlThread();
+
+ #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishRunning(), false)
+ 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 /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////// //
+
+ const std::set<RobotUnitState> ModuleBase::DevicesReadyStates
+ {
+ RobotUnitState::InitializingUnits,
+ RobotUnitState::InitializingControlThread,
+ RobotUnitState::Running
+ };
+
+ void ModuleBase::setRobotUnitState(RobotUnitState to)
+ {
+ 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)
+ {
+ 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)
+ {
+ ARMARX_ERROR << transitionErrorMessage(RobotUnitState::Running);
+ }
+ 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();
+ }
+
+ void ModuleBase::throwIfInControlThread(const std::string &fnc) const
+ {
+ if (inControlThread())
+ {
+ throw LogicError
+ {
+ "Called function '" + fnc + "' in the Control Thread\nStack trace:\n" +
+ LogSender::CreateBackTrace()
+ };
+ }
+ }
+
+ 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 guard;
+ }
+ 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(const std::set<RobotUnitState> &stateSet, const std::string &fnc, bool onlyWarn) const
+ {
+ if (! stateSet.count(state))
+ {
+ 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)
+ {
+ throw LogicError {ss.str()};
+ }
+ }
+ }
+ 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))
+ {
+ 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()};
+ }
+ }
+ }
+ 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);
+ }
+
+
+ std::atomic<ModuleBase*> ModuleBase::Instance_{nullptr};
+
+ ModuleBase::ModuleBase()
+ {
+ if(Instance_ && this != Instance_)
+ {
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..17ed09e53c7068cc8364bc6a38ce3dd0741b0193
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
@@ -0,0 +1,502 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#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 "../JointControllers/JointController.h"
+#include "../util/JointAndNJointControllers.h"
+#include "../util.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(RobotUnit);
+ TYPEDEF_PTRS_HANDLE(NJointController);
+
+ template<class T>
+ T& CheckedDeref(T* ptr)
+ {
+ if(!ptr)
+ {
+ throw std::invalid_argument{"Ptr passed to CheckedDeref is NULL"};
+ }
+ return *ptr;
+ }
+ template<class Targ, class Src>
+ Targ& CheckedCastAndDeref(Src* src)
+ {
+ Targ* ptr = dynamic_cast<Targ*>(src);
+ if(!ptr)
+ {
+ if(!src)
+ {
+ throw std::invalid_argument{"Ptr passed to CheckedCastAndDeref is NULL"};
+ }
+ throw std::invalid_argument
+ {
+ "Ptr of type '" +
+ GetTypeString<Src>() +
+ "' passed to CheckedCastAndDeref is is not related to target type '" +
+ GetTypeString<Targ>()
+ };
+ }
+ return *ptr;
+ }
+
+ /// @brief The current state of the multi step initialization of a RobotUnit
+ enum class RobotUnitState : std::size_t
+ {
+ InitializingComponent,
+ InitializingDevices,
+ //ok to use devices/create controllers
+ InitializingUnits,
+ InitializingControlThread,
+ Running,
+ Exiting
+ };
+
+ std::string to_string(armarx::RobotUnitState s);
+ inline std::ostream& operator<<(std::ostream& o, RobotUnitState s)
+ {
+ return o << to_string(s);
+ }
+}
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ namespace detail
+ {
+ class ModuleBaseIntermediatePropertyDefinitions: public ComponentPropertyDefinitions
+ {
+ public:
+ ModuleBaseIntermediatePropertyDefinitions():ComponentPropertyDefinitions(""){}
+ };
+ }
+
+ class ModuleBasePropertyDefinitions: virtual public detail::ModuleBaseIntermediatePropertyDefinitions
+ {
+ public:
+ ModuleBasePropertyDefinitions(std::string 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))
+ #undef forward_declare
+
+ /**
+ * @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:
+ /**
+ * @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:
+ 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))
+#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
+ /// @see \ref ManagedIceObject::onInitComponent
+ virtual void onInitComponent() final;
+ /// @see \ref ManagedIceObject::onConnectComponent
+ virtual void onConnectComponent() final;
+ /// @see \ref ManagedIceObject::onDisconnectComponent
+ virtual void onDisconnectComponent() final;
+ /// @see \ref ManagedIceObject::onExitComponent
+ virtual void onExitComponent() final;
+
+ //ManagedIceObject life cycle module hooks
+ /// @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called before \ref onInitRobotUnit was called)
+ void _preOnInitRobotUnit(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called after \ref onInitRobotUnit was called)
+ void _postOnInitRobotUnit(){}
+
+ /// @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called before \ref onConnectRobotUnit was called)
+ void _preOnConnectRobotUnit(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called after \ref onConnectRobotUnit was called)
+ void _postOnConnectRobotUnit(){}
+
+ /// @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called before \ref onDisconnectRobotUnit was called)
+ void _preOnDisconnectRobotUnit(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called after \ref onDisconnectRobotUnit was called)
+ void _postOnDisconnectRobotUnit(){}
+
+ /// @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called before \ref onExitRobotUnit was called)
+ void _preOnExitRobotUnit(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called after \ref onExitRobotUnit was called)
+ void _postOnExitRobotUnit(){}
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////// other ManagedIceObject / Component methods ////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ /// @see \ref ManagedIceObject::icePropertiesUpdated
+ void icePropertiesUpdated(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 icePropertiesUpdated
+ void _icePropertiesUpdated(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
+ virtual void finishComponentInitialization();
+ /// @brief Transition \ref RobotUnitState::InitializingDevices -> \ref RobotUnitState::InitializingUnits
+ virtual void finishDeviceInitialization();
+ /// @brief Transition \ref RobotUnitState::InitializingUnits -> \ref RobotUnitState::WaitingForRTThreadInitialization
+ virtual void finishUnitInitialization();
+ /// @brief Transition \ref RobotUnitState::InitializingControlThread -> \ref RobotUnitState::Running
+ virtual void finishControlThreadInitialization();
+ /// @brief Transition \ref RobotUnitState::Running -> \ref RobotUnitState::Exiting
+ virtual void finishRunning();
+
+ //state transition module hooks
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishComponentInitialization (called before the state has changed)
+ void _preFinishComponentInitialization(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishComponentInitialization (called after the state has changed)
+ void _postFinishComponentInitialization(){}
+
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called before the state has changed)
+ void _preFinishDeviceInitialization(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called after the state has changed)
+ void _postFinishDeviceInitialization(){}
+
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called before the state has changed)
+ void _preFinishUnitInitialization(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called after the state has changed)
+ void _postFinishUnitInitialization(){}
+
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called before the state has changed)
+ void _preFinishControlThreadInitialization(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called after the state has changed)
+ void _postFinishControlThreadInitialization(){}
+
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called before the state has changed)
+ void _preFinishRunning(){}
+ /// @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called after the state has changed)
+ 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 RobotUnit::onInitComponent()
+ virtual void onInitRobotUnit(){}
+ /// @brief called in RobotUnit::onConnectComponent()
+ virtual void onConnectRobotUnit(){}
+ /// @brief called in RobotUnit::onDisconnecComponent()
+ virtual void onDisconnectRobotUnit(){}
+ /// @brief called in RobotUnit::onExitComponent() before calling RobotUnit::finishRunning
+ 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;
+ /**
+ * @brief Returns the current Time
+ * @return The current Time
+ */
+ static TimePointType Now();
+ /**
+ * @brief Returns the microseconds since the given start timepoint.
+ * @param last Start timepoint
+ * @return The microseconds since the given start timepoint.
+ */
+ static std::chrono::microseconds MicroToNow(TimePointType last);
+ /**
+ * @brief Returns the microseconds since epoch.
+ * @return The microseconds since epoch.
+ */
+ static std::chrono::microseconds MicroNow();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// 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
new file mode 100644
index 0000000000000000000000000000000000000000..7f8a977b0c7bd27a11332ce4fa655550a5930678
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.ipp
@@ -0,0 +1,49 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotUnitModuleBase.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ inline ModuleBase::TimePointType ModuleBase::Now()
+ {
+ return ClockType::now();
+ }
+ inline std::chrono::microseconds ModuleBase::MicroToNow(ModuleBase::TimePointType last)
+ {
+ return std::chrono::duration_cast<std::chrono::microseconds>(Now() - last);
+ }
+ inline std::chrono::microseconds ModuleBase::MicroNow()
+ {
+ return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
+ }
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..fb6bf1ce6d8e83378592dfa83291ba5eb2d2b15f
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -0,0 +1,573 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/util/OnScopeExit.h>
+
+#include "RobotUnitModuleControlThread.h"
+#include "../NJointControllers/NJointController.h"
+
+#include "../Devices/RTThreadTimingsSensorDevice.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ /**
+ * \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 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 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.getWriteBuffer().jointControllers;
+ }
+ static const std::vector<NJointController*>& GetRequestedNJointControllers(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersRequested.getWriteBuffer().nJointControllers;
+ }
+ 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 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);
+ }
+ };
+ }
+}
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ bool ControlThread::rtSwitchControllerSetup()
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupStart();
+ ARMARX_ON_SCOPE_EXIT { rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd(); };
+ // !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)
+ {
+ return false;
+ }
+ rtIsInEmergencyStop = true;
+ bool updatedActive = false;
+ //deactivate all nJointCtrl
+ for (auto& nJointCtrl : rtGetActivatedNJointControllers())
+ {
+ if (nJointCtrl)
+ {
+ NJointControllerAttorneyForControlThread::RtDeactivateController(nJointCtrl);
+ nJointCtrl = nullptr;
+ updatedActive = 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;
+ updatedActive = true;
+ }
+ }
+ if (updatedActive)
+ {
+ ControlThreadDataBufferAttorneyForControlThread::ResetActivatedControllerAssignement(this);
+ rtCommitActivatedControllers();
+ }
+ return updatedActive;
+ }
+
+ if (
+ !ControlThreadDataBufferAttorneyForControlThread::RequestedControllersChanged(this) &&
+ !rtIsInEmergencyStop
+ )
+ {
+ return false;
+ }
+ rtIsInEmergencyStop = false;
+
+ //handle nJointCtrl
+ {
+ //"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)
+ {
+ //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
+ NJointControllerAttorneyForControlThread::RtActivateController(req);
+ ++idxReq;
+ }
+ else if (reqId < actId)
+ {
+ NJointControllerAttorneyForControlThread::RtDeactivateController(act);
+ ++idxAct;
+ }
+ else //if(reqId == actId)
+ {
+ //same ctrl or both null ctrl
+ ++idxReq;
+ ++idxAct;
+ }
+ if (idxAct >= n && !req)
+ {
+ break;
+ }
+ }
+ rtGetActivatedNJointControllers() = rtGetRequestedNJointControllers();
+ }
+
+ //handle Joint Ctrl
+ {
+ ARMARX_CHECK_EQUAL(rtGetRequestedJointControllers().size(), rtGetControlDevices().size());
+ for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
+ {
+ auto& controlDevice = rtGetControlDevices().at(i);
+ const auto requestedJointCtrl = rtGetRequestedJointControllers().at(i);
+ controlDevice->rtSetActiveJointController(requestedJointCtrl);
+ rtGetActivatedJointControllers().at(i) = requestedJointCtrl;
+ }
+ }
+ ControlThreadDataBufferAttorneyForControlThread::AcceptRequestedJointToNJointControllerAssignement(this);
+ rtCommitActivatedControllers();
+ return true;
+ }
+
+
+ void ControlThread::rtResetAllTargets()
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsStart();
+ for (const ControlDevicePtr& controlDev : rtGetControlDevices())
+ {
+ controlDev->rtGetActiveJointController()->rtResetTarget();
+ }
+ rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsEnd();
+ }
+
+
+ void ControlThread::rtHandleInvalidTargets()
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart();
+ bool oneIsInvalid = 0;
+ for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
+ {
+ if (!rtGetActivatedJointControllers().at(i)->rtIsTargetVaild())
+ {
+ rtDeactivateAssignedNJointControllerBecauseOfError(i, false);
+ oneIsInvalid = true;
+ }
+ }
+ if (oneIsInvalid)
+ {
+ rtCommitActivatedControllers();
+ }
+ rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsEnd();
+ }
+
+
+ void ControlThread::rtReadSensorDeviceValues(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart();
+ for (const SensorDevicePtr& device : rtGetSensorDevices())
+ {
+ device->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ rtUpdateVirtualRobot();
+ rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesEnd();
+ }
+
+
+ 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();
+ for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < rtGetActivatedNJointControllers().size(); ++nJointCtrlIndex)
+ {
+ NJointController* nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
+ try
+ {
+ if (nJointCtrl)
+ {
+ auto start = MicroNow();
+ nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
+ auto duration = MicroNow() - start;
+ if (duration.count() > 500)
+ {
+ ARMARX_ERROR << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
+ }
+ else if (duration.count() > 50)
+ {
+ ARMARX_WARNING << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
+ }
+ }
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "NJoint Controller " << nJointCtrl->getInstanceName()
+ << " threw an exception and is now deactivated: "
+ << GetHandledExceptionString();
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false);
+ }
+ }
+ rtCommitActivatedControllers();
+ rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
+ }
+
+
+ void ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex, bool writeActivatedControlers)
+ {
+ const NJointControllerPtr& nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
+ NJointControllerAttorneyForControlThread ::RtDeactivateControllerBecauseOfError(nJointCtrl);
+ for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
+ {
+ const ControlDevicePtr& controlDevice = rtGetControlDevices().at(ctrlDevIdx);
+ JointController* es = controlDevice->rtGetJointEmergencyStopController();
+ controlDevice->rtSetActiveJointController(es);
+ rtGetActivatedJointControllers().at(ctrlDevIdx) = es;
+ ARMARX_CHECK_EXPRESSION(rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) == nJointCtrlIndex);
+ rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) = IndexSentinel();
+ }
+ rtGetActivatedNJointControllers().at(nJointCtrlIndex) = nullptr;
+ //check ControlDeviceHardwareControlModeGroups
+ {
+ 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, false);
+ }
+ }
+ }
+ rtDeactivatedNJointControllerBecauseOfError(nJointCtrl);
+
+ if (writeActivatedControlers)
+ {
+ rtCommitActivatedControllers();
+ }
+ }
+
+
+ void ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex, bool writeActivatedControlers)
+ {
+ std::size_t nJointCtrlIndex = rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ nJointCtrlIndex < rtGetControlDevices().size(),
+ nJointCtrlIndex << " < " << rtGetControlDevices().size() <<
+ ": no NJoint controller controls this device (name = "
+ << rtGetControlDevices().at(ctrlDevIndex)->getDeviceName()
+ << ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
+ << VAROUT(rtGetActivatedJointControllers()) << "\n"
+ << VAROUT(rtGetActivatedNJointControllers())
+ );
+
+ rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false);
+
+ if (writeActivatedControlers)
+ {
+ rtCommitActivatedControllers();
+ }
+ }
+
+ void ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferStart();
+ 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)
+ {
+ rtGetSensorDevices().at(sensIdx)->getSensorValue()->_copyTo(sc.sensors.at(sensIdx));
+ }
+
+ ARMARX_CHECK_EQUAL(rtGetControlDevices().size(), sc.control.size());
+ for (std::size_t ctrlIdx = 0; ctrlIdx < rtGetControlDevices().size(); ++ctrlIdx)
+ {
+ 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)
+ {
+ JointController& jointCtrl = *controlDevice.rtGetJointControllers().at(targIdx);
+ jointCtrl.getControlTarget()->_copyTo(sc.control.at(ctrlIdx).at(targIdx));
+ }
+ }
+ _module<ControlThreadDataBuffer>().rtSensorAndControlBufferCommitWrite();
+
+ rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferEnd();
+ }
+
+ const std::vector<ControlDevicePtr> &ControlThread::rtGetControlDevices()
+ {
+ return DevicesAttorneyForControlThread::GetControlDevices(this);
+ }
+
+ const std::vector<SensorDevicePtr> &ControlThread::rtGetSensorDevices()
+ {
+ return DevicesAttorneyForControlThread::GetSensorDevices(this);
+ }
+
+ RTThreadTimingsSensorDevice &ControlThread::rtGetThreadTimingsSensorDevice()
+ {
+ return DevicesAttorneyForControlThread::GetThreadTimingsSensorDevice(this);
+ }
+
+ void ControlThread::rtUpdateVirtualRobot()
+ {
+ DevicesAttorneyForControlThread::UpdateRobotWithSensorValues(this, rtRobot, rtRobotNodes);
+ for(NJointController* c: rtGetActivatedNJointControllers())
+ {
+ if(!c)
+ {
+ break;
+ }
+ if(c->rtGetRobot())
+ {
+ auto& from = rtRobotNodes;
+ auto& to = c->rtGetRobotNodes();
+ for(std::size_t i = 0; i < from.size(); ++i)
+ {
+ to.at(i)->copyPoseFrom(from.at(i));
+ }
+ }
+ }
+ }
+
+ void ControlThread::_preFinishControlThreadInitialization()
+ {
+ controlThreadId = std::this_thread::get_id();
+ }
+
+ void ControlThread::_postOnInitRobotUnit()
+ {
+ 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());
+ }
+ }
+
+ void ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current &)
+ {
+ _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
+ }
+
+ EmergencyStopState ControlThread::getEmergencyStopState(const Ice::Current &) const
+ {
+ return emergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ }
+
+ EmergencyStopState ControlThread::getRtEmergencyStopState(const Ice::Current &) const
+ {
+ return rtIsInEmergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ }
+
+ void ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
+ {
+ emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
+ }
+
+ std::vector<JointController *> &ControlThread::rtGetActivatedJointControllers()
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
+ }
+
+ std::vector<NJointController *> &ControlThread::rtGetActivatedNJointControllers()
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
+ }
+
+ void ControlThread::rtCommitActivatedControllers()
+ {
+ ControlThreadDataBufferAttorneyForControlThread::CommitActivatedControllers(this);
+ }
+
+ const std::vector<JointController *> &ControlThread::rtGetRequestedJointControllers()
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointControllers(this);
+ }
+
+ const std::vector<NJointController *> &ControlThread::rtGetRequestedNJointControllers()
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetRequestedNJointControllers(this);
+ }
+
+ std::vector<std::size_t> &ControlThread::rtGetActivatedJointToNJointControllerAssignement()
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ }
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e901c5fdbdf2daab33348c22aa9075f92171200
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -0,0 +1,248 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <thread>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include "../Devices/ControlDevice.h"
+#include "../Devices/SensorDevice.h"
+
+#include "RobotUnitModuleBase.h"
+
+namespace armarx
+{
+ class RTThreadTimingsSensorDevice;
+ namespace RobotUnitModule
+ {
+ /**
+ * @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
+ {
+ friend class ModuleBase;
+ 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::_postOnInitRobotUnit
+ void _postOnInitRobotUnit();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Sets the \ref EmergencyStopState
+ * @param state The \ref EmergencyStopState to set
+ */
+ void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = GlobalIceCurrent) override;
+ /**
+ * @brief Returns the \ref ControlThread's target \ref EmergencyStopState
+ * @return The \ref EmergencyStopState
+ */
+ EmergencyStopState getEmergencyStopState(const Ice::Current& = GlobalIceCurrent) const override;
+ /**
+ * @brief Returns the \ref ControlThread's \ref EmergencyStopState
+ * @return The \ref EmergencyStopState
+ */
+ EmergencyStopState getRtEmergencyStopState(const Ice::Current& = GlobalIceCurrent) const override;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// Module interface /////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Sets the \ref EmergencyStopState without updating the topic
+ * @param state The \ref EmergencyStopState to set
+ */
+ void setEmergencyStopStateNoReportToTopic(EmergencyStopState state);
+ /**
+ * @brief Teturns he \ref ControlThread's thread id
+ * @return The \ref ControlThread's thread id
+ * @see controlThreadId
+ */
+ std::thread::id getControlThreadId() const
+ {
+ return controlThreadId;
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// 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 rtDeactivateAssignedNJointControllerBecauseOfError
+ * @param ctrlDevIndex
+ * @param writeActivatedControlers
+ */
+ void rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex, bool writeActivatedControlers = true);
+ /**
+ * @brief Deactivates a NJointController and sets all used ControlDevices to EmergencyStop
+ * @param nJointCtrlIndex The NJointController to deactivate (index in controllersActivated)
+ * @param writeActivatedControlers Whether controllersActivated should be committed
+ */
+ void rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex, bool writeActivatedControlers = true);
+ /**
+ * @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 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();
+ /**
+ * @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 Updates the \ref VirtualRobot::Robot used in the \ref ControlThread with the current \ref SensorValueBase SensorValues
+ void rtUpdateVirtualRobot();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// implementation //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /**
+ * @brief Returns the activated \ref JointController "JointControllers"
+ * @return The activated \ref JointController "JointControllers"
+ */
+ std::vector<JointController*>& rtGetActivatedJointControllers();
+ /**
+ * @brief Returns the activated \ref NJointController "NJointControllers"
+ * @return The activated \ref NJointController "NJointControllers"
+ */
+ std::vector<NJointController*>& rtGetActivatedNJointControllers();
+ /**
+ * @brief Writes the activated \ref NJointController "NJointControllers" and
+ * \ref JointController "JointControllers" to the output buffer.
+ */
+ void rtCommitActivatedControllers();
+
+ /**
+ * @brief Returns the requested \ref JointController "JointControllers"
+ * @return The requested \ref JointController "JointControllers"
+ */
+ const std::vector<JointController*>& rtGetRequestedJointControllers();
+ /**
+ * @brief Returns the requested \ref NJointController "NJointControllers"
+ * @return The requested \ref NJointController "NJointControllers"
+ */
+ const std::vector<NJointController*>& rtGetRequestedNJointControllers();
+
+ /**
+ * @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();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// 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 ControlThread's thread id
+ std::atomic<std::thread::id> controlThreadId;
+ };
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..24ca950bf4eace1d3c5d6cbbfa63d41ea7198943
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
@@ -0,0 +1,319 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+
+#include "RobotUnitModuleControllerManagement.h"
+#include "RobotUnitModuleDevices.h"
+
+#include "../NJointControllers/NJointController.h"
+
+namespace armarx
+{
+ 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
+ {
+ 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;
+
+ static void UpdateNJointControllerRequestedState(ControlThreadDataBuffer* p, const std::set<NJointControllerPtr>& request)
+ {
+ p->_module<ControllerManagement>().updateNJointControllerRequestedState(request);
+ }
+ };
+ }
+}
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ JointAndNJointControllers ControlThreadDataBuffer::getActivatedControllers() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
+ return controllersActivated.getReadBuffer();
+ }
+
+ std::vector<JointController *> ControlThreadDataBuffer::getActivatedJointControllers() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
+ return controllersActivated.getReadBuffer().jointControllers;
+ }
+
+ std::vector<NJointController *> ControlThreadDataBuffer::getActivatedNJointControllers() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
+ return controllersActivated.getReadBuffer().nJointControllers;
+ }
+ bool ControlThreadDataBuffer::activatedControllersChanged() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
+ return controllersActivated.updateReadBuffer();
+ }
+
+ JointAndNJointControllers ControlThreadDataBuffer::getRequestedControllers() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
+ return controllersRequested.getWriteBuffer();
+ }
+
+ std::vector<JointController *> ControlThreadDataBuffer::getRequestedJointControllers() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
+ return controllersRequested.getWriteBuffer().jointControllers;
+ }
+
+ std::vector<NJointController *> ControlThreadDataBuffer::getRequestedNJointControllers() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
+ return controllersRequested.getWriteBuffer().nJointControllers;
+ }
+
+ bool ControlThreadDataBuffer::sensorAndControlBufferChanged() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlThreadOutputBuffer.updateReadBuffer();
+ }
+
+ const SensorAndControl &ControlThreadDataBuffer::getSensorAndControlBuffer() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlThreadOutputBuffer.getReadBuffer();
+ }
+
+ void ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
+ {
+ auto guard = getGuard();
+ 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;
+ });
+
+ 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));
+
+ //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
+ {
+ 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)
+ {
+ 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
+ {
+ ARMARX_DEBUG << "setup assignement index";
+ setOfControllers.resetAssignement();
+ for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < setOfControllers.nJointControllers.size(); ++nJointCtrlIndex)
+ {
+ 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;
+ }
+ }
+ }
+
+ {
+ 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!";
+ }
+
+ void ControlThreadDataBuffer::setActivateControllersRequest(std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> ctrls)
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ //erase nullptr
+ ctrls.erase(nullptr);
+ ARMARX_CHECK_EXPRESSION(ctrls.size() <= _module<Devices>().getNumberOfControlDevices());
+ const std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> setOfRequestedCtrls
+ {
+ getRequestedNJointControllers().begin(),
+ getRequestedNJointControllers().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)
+ {
+ 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::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()};
+ }
+ request.jointControllers.at(jointCtrlIndex) = _module<Devices>().getControlDevices().at(jointCtrlIndex)->getJointController(cd2cm.second);
+ }
+ }
+ for (auto i : getIndices(request.jointControllers))
+ {
+ JointController*& jointCtrl = request.jointControllers.at(i);
+ if (!jointCtrl)
+ {
+ JointController* jointCtrlOld = getRequestedJointControllers().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();
+ }
+ }
+ }
+ writeRequestedControllers(std::move(request));
+ ARMARX_DEBUG << "wrote requested controllers";
+ ARMARX_INFO << "requested controllers:\n" << _module<ControllerManagement>().getRequestedNJointControllerNames();
+ }
+
+ void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr &robot, const std::vector<VirtualRobot::RobotNodePtr> &nodes) const
+ {
+ _module<Devices>().updateVirtualRobotFromSensorValues(robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
+ }
+
+ void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr &robot) const
+ {
+ _module<Devices>().updateVirtualRobotFromSensorValues(robot, controlThreadOutputBuffer.getReadBuffer().sensors);
+ }
+ void ControlThreadDataBuffer::_postFinishDeviceInitialization()
+ {
+ ARMARX_DEBUG << "initializing buffers:";
+ {
+ 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);
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..962593d12c33bca0d5c7f3893948d35b97bb3c2d
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
@@ -0,0 +1,281 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/util/TripleBuffer.h>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include "RobotUnitModuleBase.h"
+#include "../util/JointAndNJointControllers.h"
+#include "../util/ControlThreadOutputBuffer.h"
+
+#include "../SensorValues/SensorValue1DoFActuator.h"
+
+namespace armarx
+{
+ class SensorValue1DoFActuatorPosition;
+
+ namespace RobotUnitModule
+ {
+ /**
+ * @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
+ */
+ class ControlThreadDataBuffer : virtual public ModuleBase
+ {
+ 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;
+
+ /**
+ * @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 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 getRequestedControllers() 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*> getRequestedJointControllers() 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 *> getRequestedNJointControllers() 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 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);
+
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// 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;
+
+ /// @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 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
new file mode 100644
index 0000000000000000000000000000000000000000..33716271d1331d1608e64bc00aa702061025a872
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -0,0 +1,628 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <ArmarXCore/core/util/OnScopeExit.h>
+
+#include "RobotUnitModuleControllerManagement.h"
+#include "../NJointControllers/NJointController.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ template<class Cont>
+ static Ice::StringSeq GetNonNullNames(const Cont& c)
+ {
+ Ice::StringSeq result;
+ result.reserve(c.size());
+ for (const auto& e : c)
+ {
+ if (e)
+ {
+ result.emplace_back(e->getName());
+ }
+ }
+ return result;
+ }
+
+ /**
+ * \brief This class allows minimal access to private members of \ref 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
+ {
+ friend class ControllerManagement;
+ static void SetRequested(const NJointControllerPtr& nJointCtrl, bool requested)
+ {
+ nJointCtrl->isRequested = requested;
+ }
+ };
+// /**
+// * \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer 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 ControlThreadDataBufferAttorneyForControllerManagement
+// {
+// friend class ControllerManagement;
+// static void SetActivateControllersRequest(ControllerManagement* p, std::set<NJointControllerPtr, std::greater<NJointControllerPtr>>&& ctrls)
+// {
+// p->_module<ControlThreadDataBuffer>().setActivateControllersRequest(std::move(ctrls));
+// }
+// };
+ }
+}
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ Ice::StringSeq ControllerManagement::getRequestedNJointControllerNames(const Ice::Current &) const
+ {
+ auto guard = getGuard();
+ return GetNonNullNames(_module<ControlThreadDataBuffer>().getRequestedNJointControllers());
+ }
+
+ Ice::StringSeq ControllerManagement::getActivatedNJointControllerNames(const Ice::Current &) const
+ {
+ auto guard = getGuard();
+ return GetNonNullNames(_module<ControlThreadDataBuffer>().getActivatedNJointControllers());
+ }
+
+ void ControllerManagement::deleteNJointController(const std::string &name, const Ice::Current &)
+ {
+ deleteNJointControllers({name}, GlobalIceCurrent);
+ }
+
+ void ControllerManagement::activateNJointController(const std::string &name, const Ice::Current &)
+ {
+ activateNJointControllers({name}, GlobalIceCurrent);
+ }
+
+ void ControllerManagement::deactivateNJointController(const std::string &name, const Ice::Current &)
+ {
+ deactivateNJointControllers(Ice::StringSeq {name}, GlobalIceCurrent);
+ }
+
+ void ControllerManagement::checkNJointControllerClassName(const std::string &className) const
+ {
+ 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::vector<NJointControllerPtr> ControllerManagement::getNJointControllersNotNull(const std::vector<std::string> &names) const
+ {
+ 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
+ {
+ 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;
+ }
+
+ StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current &) const
+ {
+ 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;
+ }
+
+ NJointControllerInterfacePrx ControllerManagement::createNJointController(
+ const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current&)
+ {
+ //no lock required
+ return NJointControllerInterfacePrx::uncheckedCast(
+ createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
+ }
+
+ NJointControllerInterfacePrx ControllerManagement::createNJointControllerFromVariantConfig(
+ const std::string& className,
+ const std::string& instanceName,
+ const StringVariantBaseMap& variants,
+ const Ice::Current&)
+ {
+ //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),
+ GlobalIceCurrent/*to select ice overload*/);
+ }
+
+ const NJointControllerPtr& ControllerManagement::createNJointController(
+ const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool deletable,
+ bool internal)
+ {
+ 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);
+ NJointControllerPtr nJointCtrl = factory->create(
+ this,
+ config,
+ controllerCreateRobot,
+ deletable, internal
+ );
+ 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()->addObjectAsync(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 &)
+ {
+ return getArmarXManager()->loadLibFromPath(path);
+ }
+
+ bool ControllerManagement::loadLibFromPackage(const std::string &package, const std::string &lib, const Ice::Current &)
+ {
+ return getArmarXManager()->loadLibFromPackage(package, lib);
+ }
+
+ Ice::StringSeq ControllerManagement::getNJointControllerClassNames(const Ice::Current &) const
+ {
+ return NJointControllerRegistry::getKeys();
+ }
+
+ Ice::StringSeq ControllerManagement::getNJointControllerNames(const Ice::Current &) const
+ {
+ auto guard = getGuard();
+ return getMapKeys(nJointControllers);
+ }
+
+
+ void ControllerManagement::activateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+ {
+ ARMARX_VERBOSE << "requesting controller activation for:\n" << names;
+ if (names.empty())
+ {
+ return;
+ }
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ auto ctrlsToActVec = getNJointControllersNotNull(names); //also checks if these controllers exist
+ ARMARX_DEBUG << "all requested controllers exist" << std::flush;
+ std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> ctrlsToAct {ctrlsToActVec.begin(), ctrlsToActVec.end()};
+ ARMARX_CHECK_EXPRESSION(!ctrlsToAct.count(nullptr));
+ std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> ctrls
+ {
+ _module<ControlThreadDataBuffer>().getRequestedNJointControllers().begin(),
+ _module<ControlThreadDataBuffer>().getRequestedNJointControllers().end()
+ };
+ ctrls.erase(nullptr);
+ //check for conflict
+ std::vector<char> inuse;
+ //check requested controllers
+ {
+ auto r = NJointController::AreNotInConflict(ctrlsToAct.begin(), ctrlsToAct.end());
+ if (!r)
+ {
+ std::stringstream ss;
+ ss << "activateNJointControllers: requested controllers are in conflict!\ncontrollers:\n" << names;
+ 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)
+ {
+ out << (c ? 1 : 0);
+ }
+ };
+ ARMARX_DEBUG << "inuse field (request)\n" << printInUse;
+ //add already active controllers if they are conflict free
+ {
+ if (ctrls.empty())
+ {
+ ARMARX_DEBUG << "no already requested NJointControllers";
+ }
+ for (const NJointControllerPtr& nJointCtrl : ctrls)
+ {
+ 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_DEBUG << "inuse field (all)\n" << printInUse;
+ }
+ _module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrlsToAct);
+ }
+ void ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+ {
+ ARMARX_VERBOSE << "requesting controller deactivation for:\n" << names;
+ if (names.empty())
+ {
+ return;
+ }
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ auto ctrlsDeacVec = getNJointControllersNotNull(names); //also checks if these controllers exist
+ std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> ctrls
+ {
+ _module<ControlThreadDataBuffer>().getRequestedNJointControllers().begin(),
+ _module<ControlThreadDataBuffer>().getRequestedNJointControllers().end()
+ };
+ for (const auto& nJointCtrlToDeactivate : ctrlsDeacVec)
+ {
+ ctrls.erase(nJointCtrlToDeactivate);
+ }
+ _module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrls);
+ }
+ void ControllerManagement::switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current&)
+ {
+ 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 Ice::StringSeq& names, const Ice::Current&)
+ {
+ ARMARX_VERBOSE << "requesting controller deletion for:\n" << names;
+ if (names.empty())
+ {
+ return;
+ }
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ auto ctrlsToActVec = getNJointControllersNotNull(names); //also checks if these controllers exist
+ ARMARX_DEBUG << "all controllers requested to delete exist" << std::flush;
+ //check if all can be deleted
+ for (const auto& nJointCtrl : ctrlsToActVec)
+ {
+ if (!nJointCtrl->isDeletable())
+ {
+ throw LogicError
+ {
+ "The NJointController '" + nJointCtrl->getInstanceName() +
+ "' can't be deleted since this operation is not allowed for this controller! (no NJointController was deleted)"
+ };
+ }
+ }
+ for (const auto& nJointCtrl : ctrlsToActVec)
+ {
+ 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)"
+ };
+ }
+ }
+ for (const auto& nJointCtrl : ctrlsToActVec)
+ {
+ 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";
+ }
+ }
+
+
+ NJointControllerClassDescription ControllerManagement::getNJointControllerClassDescription(
+ const std::string& className, const Ice::Current&) const
+ {
+ 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
+ {
+ std::size_t tries = 200;
+ while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices)
+ {
+ //this phase should only last short so busy waiting is ok
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ if (! --tries)
+ {
+ throw RuntimeError {"RobotUnit::getNJointControllerClassDescriptions: it took too long to for the unit to get in a valid state"};
+ }
+ }
+ auto guard = getGuard();
+ NJointControllerClassDescriptionSeq r;
+ r.reserve(NJointControllerRegistry::getKeys().size());
+ for (const auto& key : NJointControllerRegistry::getKeys())
+ {
+ r.emplace_back(getNJointControllerClassDescription(key));
+ }
+ return r;
+ }
+
+ NJointControllerInterfacePrx ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
+ {
+ 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));
+ }
+
+ NJointControllerStatus ControllerManagement::getNJointControllerStatus(const std::string& name, const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return getNJointControllerNotNull(name)->getControllerStatus();
+ }
+
+ NJointControllerStatusSeq ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ NJointControllerStatusSeq r;
+ r.reserve(nJointControllers.size());
+ for (const auto& nJointCtrl : nJointControllers)
+ {
+ r.emplace_back(nJointCtrl.second->getControllerStatus());
+ }
+ return r;
+ }
+
+ NJointControllerDescription ControllerManagement::getNJointControllerDescription(const std::string& name, const Ice::Current&) const
+ {
+ NJointControllerPtr nJointCtrl;
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ nJointCtrl = getNJointControllerNotNull(name);
+ }
+ return nJointCtrl->getControllerDescription();
+ }
+
+ NJointControllerDescriptionSeq ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
+ {
+ 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;
+ }
+
+ NJointControllerDescriptionWithStatus ControllerManagement::getNJointControllerDescriptionWithStatus(
+ const std::string& name, const Ice::Current&) const
+ {
+ NJointControllerPtr nJointCtrl;
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ nJointCtrl = getNJointControllerNotNull(name);
+ }
+ return nJointCtrl->getControllerDescriptionWithStatus();
+ }
+
+ NJointControllerDescriptionWithStatusSeq ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
+ {
+ std::map<std::string, NJointControllerPtr> nJointControllersCopy;
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ nJointControllersCopy = nJointControllers;
+ }
+ NJointControllerDescriptionWithStatusSeq r;
+ r.reserve(nJointControllersCopy.size());
+ for (const auto& nJointCtrl : nJointControllersCopy)
+ {
+ r.emplace_back(nJointCtrl.second->getControllerDescriptionWithStatus());
+ }
+ return r;
+ }
+
+ void ControllerManagement::removeNJointControllers(std::map<std::string, NJointControllerPtr>& ctrls, bool blocking, RobotUnitListenerPrx l)
+ {
+ for (auto& n2NJointCtrl : ctrls)
+ {
+ 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);
+ }
+ }
+ ctrls.clear();
+ }
+
+ void ControllerManagement::removeNJointControllersToBeDeleted(bool blocking, RobotUnitListenerPrx l)
+ {
+ removeNJointControllers(nJointControllersToBeDeleted, blocking, l);
+ }
+
+ void ControllerManagement::_preFinishRunning()
+ {
+ //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()
+ {
+ nJointControllers.clear();
+ }
+
+ void ControllerManagement::_postOnInitRobotUnit()
+ {
+ controllerCreateRobot = _module<RobotData>().cloneRobot();
+ }
+
+ void ControllerManagement::updateNJointControllerRequestedState(const std::set<NJointControllerPtr> &request)
+ {
+ ARMARX_DEBUG << "set requested state for NJoint controllers";
+ for (const auto& name2NJoint : nJointControllers)
+ {
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..f48cf5976e5941afbf5eb2fd8d860b37fdb169b3
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
@@ -0,0 +1,356 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <thread>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include "RobotUnitModuleBase.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ /**
+ * @brief This \ref ModuleBase "Module" manages \ref NJointController "NJointControllers".
+ *
+ * @see ModuleBase
+ */
+ class ControllerManagement : virtual public ModuleBase, virtual public RobotUnitControllerManagementInterface
+ {
+ 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::_postOnInitRobotUnit
+ void _postOnInitRobotUnit();
+ /// @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&) const override;
+ /**
+ * @brief Returns proxies to all \ref NJointController "NJointControllers"
+ * @return Proxies to all \ref NJointController "NJointControllers"
+ * @see getNJointController
+ */
+ StringNJointControllerPrxDictionary getAllNJointControllers(const Ice::Current&) 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& = GlobalIceCurrent) 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&) 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& = GlobalIceCurrent) 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
+ */
+
+ NJointControllerDescriptionSeq getNJointControllerDescriptions(const Ice::Current&) 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& = GlobalIceCurrent) 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&) const override;
+
+ /**
+ * @brief getNJointControllerClassDescription
+ * @param className
+ * @return
+ */
+ NJointControllerClassDescription getNJointControllerClassDescription(
+ const std::string& className, const Ice::Current& = GlobalIceCurrent) const override;
+ /**
+ * @brief getNJointControllerClassDescriptions
+ * @return
+ */
+ NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(const Ice::Current&) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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&) 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&) 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& = GlobalIceCurrent) 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 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;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////// 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);
+
+ /**
+ * @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;
+
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// 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;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////////// 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
new file mode 100644
index 0000000000000000000000000000000000000000..456705d417e637098b2e52fd8f31cd588277fef9
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -0,0 +1,648 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotUnitModuleDevices.h"
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleRobotData.h"
+
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
+
+ ControlDeviceDescription Devices::getControlDeviceDescription(const std::string &name, const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ 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));
+ }
+
+ ControlDeviceDescriptionSeq Devices::getControlDeviceDescriptions(const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ ControlDeviceDescriptionSeq r;
+ r.reserve(getNumberOfControlDevices());
+ for (auto idx : getIndices(controlDevices.values()))
+ {
+ r.emplace_back(getControlDeviceDescription(idx));
+ }
+ return r;
+ }
+
+ std::size_t Devices::getNumberOfControlDevices() const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlDevices.size();
+ }
+
+ std::size_t Devices::getNumberOfSensorDevices() const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorDevices.size();
+ }
+
+ std::size_t Devices::getControlDeviceIndex(const std::string& deviceName) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
+ return sensorDevices.index(deviceName);
+ }
+ std::size_t Devices::getSensorDeviceIndex(const std::string& deviceName) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
+ return controlDevices.index(deviceName);
+ }
+
+ ConstSensorDevicePtr Devices::getSensorDevice(const std::string &sensorDeviceName) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
+ }
+
+ ConstControlDevicePtr Devices::getControlDevice(const std::string &deviceName) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlDevices.at(deviceName, ControlDevice::NullPtr);
+ }
+
+ Ice::StringSeq Devices::getControlDeviceNames(const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlDevices.keys();
+ }
+
+ ControlDeviceDescription Devices::getControlDeviceDescription(std::size_t idx) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ 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();
+ }
+ return data;
+ }
+
+ ControlDeviceStatus Devices::getControlDeviceStatus(std::size_t idx) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ 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();
+ ARMARX_CHECK_EXPRESSION(_module<ControlThreadDataBuffer>().getRequestedJointControllers().at(idx));
+ status.requestedControlMode = _module<ControlThreadDataBuffer>().getRequestedJointControllers().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 = MicroNow().count();
+ return status;
+ }
+
+ ControlDeviceStatus Devices::getControlDeviceStatus(const std::string &name, const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ 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));
+ }
+
+ ControlDeviceStatusSeq Devices::getControlDeviceStatuses(const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ ControlDeviceStatusSeq r;
+ r.reserve(getNumberOfControlDevices());
+ for (auto idx : getIndices(controlDevices.values()))
+ {
+ r.emplace_back(getControlDeviceStatus(idx));
+ }
+ return r;
+ }
+
+ Ice::StringSeq Devices::getSensorDeviceNames(const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorDevices.keys();
+ }
+
+ SensorDeviceDescription Devices::getSensorDeviceDescription(std::size_t idx) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ 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();
+ return data;
+ }
+
+ SensorDeviceStatus Devices::getSensorDeviceStatus(std::size_t idx) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ 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 = MicroNow().count();
+ return status;
+ }
+
+ SensorDeviceDescription Devices::getSensorDeviceDescription(const std::string &name, const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ if (!sensorDevices.has(name))
+ {
+ 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));
+ }
+
+ SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ SensorDeviceDescriptionSeq r;
+ r.reserve(getNumberOfSensorDevices());
+ for (auto idx : getIndices(sensorDevices.values()))
+ {
+ r.emplace_back(getSensorDeviceDescription(idx));
+ }
+ return r;
+ }
+
+ SensorDeviceStatus Devices::getSensorDeviceStatus(const std::string &name, const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ throwIfDevicesNotReady(__FUNCTION__);
+ if (!sensorDevices.has(name))
+ {
+ 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));
+ }
+
+ SensorDeviceStatusSeq Devices::getSensorDeviceStatuses(const Ice::Current &) const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ SensorDeviceStatusSeq r;
+ r.reserve(getNumberOfSensorDevices());
+ for (auto idx : getIndices(sensorDevices.values()))
+ {
+ r.emplace_back(getSensorDeviceStatus(idx));
+ }
+ return r;
+ }
+
+ void Devices::addControlDevice(const ControlDevicePtr &cd)
+ {
+ throwIfInControlThread(__FUNCTION__);
+ //this guards prevents the RobotUnitState to change
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ controlDevices.add(cd->getDeviceName(), cd);
+ ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
+ }
+
+ void Devices::addSensorDevice(const SensorDevicePtr &sd)
+ {
+ throwIfInControlThread(__FUNCTION__);
+ //this guards prevents the RobotUnitState to change
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ 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()};
+ }
+ if (sd->getDeviceName() == 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)
+ sensorDevices.add(sd->getDeviceName(), sd);
+ rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
+ }
+ else
+ {
+ sensorDevices.add(sd->getDeviceName(), sd);
+ }
+ ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";
+ }
+
+ RTThreadTimingsSensorDevicePtr Devices::createRTThreadTimingSensorDevice() const
+ {
+ throwIfInControlThread(__FUNCTION__);
+ return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
+ }
+
+ void Devices::_postFinishRunning()
+ {
+ throwIfInControlThread(__FUNCTION__);
+ controlDevicesConstPtr.clear();
+ sensorDevicesConstPtr.clear();
+ sensorDevices.clear();
+ controlDevices.clear();
+ }
+
+ std::vector<JointController *> Devices::getStopMovementJointControllers() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ throwIfInControlThread(__FUNCTION__);
+
+ 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());
+ return controllers;
+ }
+
+ std::vector<JointController *> Devices::getEmergencyStopJointControllers() const
+ {
+ throwIfDevicesNotReady(__FUNCTION__);
+ throwIfInControlThread(__FUNCTION__);
+
+ std::vector<JointController*> controllers;
+ controllers.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());
+ }
+ ARMARX_CHECK_EQUAL(controlDevices.size(), controllers.size());
+ return controllers;
+ }
+
+ void Devices::_preFinishDeviceInitialization()
+ {
+ throwIfInControlThread(__FUNCTION__);
+ if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
+ {
+ addSensorDevice(createRTThreadTimingSensorDevice());
+ }
+ }
+
+ void Devices::_postFinishDeviceInitialization()
+ {
+ throwIfInControlThread(__FUNCTION__);
+ ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
+ {
+ for (const ControlDevicePtr& controlDevice : controlDevices.values())
+ {
+ ARMARX_CHECK_EXPRESSION(controlDevice);
+ ARMARX_DEBUG << "----" << controlDevice->getDeviceName();
+ if (!controlDevice->hasJointController(ControlModes::EmergencyStop))
+ {
+ 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 (!controlDevice->hasJointController(ControlModes::StopMovement))
+ {
+ 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()};
+ }
+ }
+ }
+ ARMARX_DEBUG << "checking " << controlDevices.size() << " SensorDevices:";
+ {
+ for (const SensorDevicePtr& sensorDevice : sensorDevices.values())
+ {
+ ARMARX_CHECK_EXPRESSION(sensorDevice);
+ ARMARX_DEBUG << "----" << sensorDevice->getDeviceName();
+ if (!sensorDevice->getSensorValue())
+ {
+ std::stringstream s;
+ s << "SensorDevice " << sensorDevice->getSensorValue() << " has null SensorValue";
+ ARMARX_ERROR << "--------" << s.str();
+ throw LogicError {s.str()};
+ }
+ 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())
+ {
+ ARMARX_CHECK_NOT_NULL(ctrl);
+ controlTargets.back().emplace_back(ctrl->getControlTarget());
+ ARMARX_CHECK_NOT_NULL(controlTargets.back().back().get());
+ }
+ }
+ }
+ ARMARX_DEBUG << "setup ControlDeviceHardwareControlModeGroups";
+ {
+ ctrlModeGroups.groupIndices.assign(getNumberOfControlDevices(), IndexSentinel());
+
+ if (!ctrlModeGroups.groupsMerged.empty())
+ {
+ ARMARX_DEBUG << "Remove control devs from ControlDeviceHardwareControlModeGroups ("
+ << ctrlModeGroups.groups.size() << ")";
+ const auto groupsMerged = ctrlModeGroups.groupsMerged;
+ for (const auto& dev : groupsMerged)
+ {
+ if (controlDevices.has(dev))
+ {
+ continue;
+ }
+ ctrlModeGroups.groupsMerged.erase(dev);
+ for (auto& group : ctrlModeGroups.groups)
+ {
+ group.erase(dev);
+ }
+ 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)
+ {
+ 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_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)
+ {
+ const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
+ if(node->isRotationalJoint() || node->isTranslationalJoint())
+ {
+ const auto& name = node->getName();
+ if(sensorDevices.has(name))
+ {
+ 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";
+ }
+ }
+ else
+ {
+ ARMARX_INFO << "No SensorDevice for RobotNode: " << name;
+ }
+ }
+ }
+ }
+
+ ARMARX_VERBOSE << "ControlDevices:\n" << controlDevices.keys();
+ ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
+ }
+
+ void Devices::_preOnInitRobotUnit()
+ {
+ throwIfInControlThread(__FUNCTION__);
+ //ControlDeviceHardwareControlModeGroups
+ const std::string controlDeviceHardwareControlModeGroupsStr = getProperty<std::string>("ControlDeviceHardwareControlModeGroups").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)
+ {
+ 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())
+ {
+ ARMARX_DEBUG << "adding device group:\n"
+ << ARMARX_STREAM_PRINTER
+ {
+ for (const auto& elem : 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
new file mode 100644
index 0000000000000000000000000000000000000000..0872a2be7a9d1a6a5378baf6d4c237b347d3d945
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
@@ -0,0 +1,449 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include "RobotUnitModuleBase.h"
+
+#include "../Devices/ControlDevice.h"
+#include "../Devices/SensorDevice.h"
+#include "../Devices/RTThreadTimingsSensorDevice.h"
+
+#include "../SensorValues/SensorValue1DoFActuator.h"
+
+namespace armarx
+{
+
+ namespace RobotUnitModule
+ {
+ class DevicesPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ public:
+ DevicesPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>(
+ "ControlDeviceHardwareControlModeGroups", "",
+ "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!"
+ );
+ }
+ };
+
+ /**
+ * @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;
+ /**
+ * @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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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 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
+ {
+ 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();
+ }
+
+ /**
+ * @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
+ * @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 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 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:
+ /// @brief Device groups requiring the same hardware control mode.
+ ControlDeviceHardwareControlModeGroups ctrlModeGroups;
+
+ /// @brief ControlDevices added to this unit (data may only be added during and only be used after State::InitializingDevices)
+ KeyValueVector<std::string, ControlDevicePtr> controlDevices;
+ /// @brief SensorDevices added to this unit (data may only be added during and only be used after State::InitializingDevices)
+ KeyValueVector<std::string, SensorDevicePtr > sensorDevices;
+
+ /// @brief const pointer to all ControlDevices (passed to GenerateConfigDescription of a NJointController)
+ std::map<std::string, ConstControlDevicePtr> controlDevicesConstPtr;
+ /// @brief const pointer to all SensorDevices (passed to GenerateConfigDescription of a NJointController)
+ std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
+
+ /// @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;
+ };
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..77675b15a15e595a02947c5104ab32829be0c1e8
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -0,0 +1,503 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
+#include <ArmarXCore/core/ArmarXManager.h>
+
+#include "RobotUnitModuleLogging.h"
+
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleDevices.h"
+
+#include "../util/ControlThreadOutputBuffer.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr &token, const std::string &marker, const Ice::Current &)
+ {
+ 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;
+ }
+
+ RemoteReferenceCounterBasePtr Logging::startRtLogging(const std::string &formatString, const Ice::StringSeq &loggingNames, const Ice::Current &)
+ {
+ StringStringDictionary alias;
+ for (const auto& name : loggingNames)
+ {
+ alias.emplace(name, "");
+ }
+ return startRtLoggingWithAliasNames(formatString, alias, GlobalIceCurrent);
+ }
+
+ void Logging::stopRtLogging(const RemoteReferenceCounterBasePtr &token, const Ice::Current &)
+ {
+ 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;
+ }
+
+ Ice::StringSeq Logging::getLoggingNames(const Ice::Current &) const
+ {
+ 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());
+ }
+ }
+ return result;
+ }
+
+ RemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(const std::string &formatString, const StringStringDictionary &aliasNames, const Ice::Current &)
+ {
+ 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() + "'"};
+ }
+ 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)
+ {
+ for (const auto& pair : aliasNames)
+ {
+ std::string name = pair.first;
+ if (boost::starts_with(dev + "$", name))
+ {
+ 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;
+ }
+ }
+ return false;
+ };
+
+ //get logged sensor device values
+ {
+ 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)
+ {
+ svfieldsFlags.emplace_back(logDev(field));
+ }
+ }
+ }
+ //get logged control device values
+ {
+ 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)
+ {
+
+ sjctrlsFlags.emplace_back();
+ auto& ctargfieldsFlags = sjctrlsFlags.back(); //v
+ ctargfieldsFlags.reserve(ctargfields.size());
+
+ 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
+ {
+ 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)
+ {
+ for (const auto& s : vs)
+ {
+ outCSV << ";" << s;
+ }
+ }
+ for (const auto& vvs : controlDeviceValueLoggingNames)
+ {
+ for (const auto& vs : vvs)
+ {
+ for (const auto& s : vs)
+ {
+ outCSV << ";" << s;
+ }
+ }
+ }
+ outCSV << std::endl;
+ }
+
+ for (const ::armarx::detail::ControlThreadOutputBufferEntry& iteration : backlog)
+ {
+ //write csv data
+ {
+ outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
+ //sens
+ {
+ for (const SensorValueBase* val : iteration.sensors)
+ {
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ {
+ outCSV << ";" << val->getDataFieldAsString(idxField);
+ }
+ }
+ }
+ //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
+ {
+ for (const ::armarx::detail::RtMessageLogEntryBase* msg : iteration.messages.getEntries())
+ {
+ if (!msg)
+ {
+ break;
+ }
+ outMsg << "[" << msg->getTime().toDateTime() << "] iteration "
+ << iteration.iteration << ":\n"
+ << msg->format() << std::endl;
+ }
+ outMsg << "\nmessages lost: " << iteration.messages.messagesLost
+ << " (required additional "
+ << iteration.messages.requiredAdditionalBufferSpace << " bytes, "
+ << iteration.messages.requiredAdditionalEntries << " message entries)\n" << std::endl;
+ }
+ }
+ }
+
+ void Logging::_preFinishRunning()
+ {
+ defaultLogHandle = nullptr;
+ if (rtLoggingTask)
+ {
+ 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";
+ }
+ }
+
+ void Logging::_preFinishControlThreadInitialization()
+ {
+ controlThreadId = LogSender::getThreadId();
+ ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
+
+ ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestep;
+ rtLoggingTask->start();
+ }
+
+ void Logging::doLogging()
+ {
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ const auto now = IceUtil::Time::now();
+ // entries are removed last
+
+ //remove backlog entries
+ {
+ while (!backlog.empty() && backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
+ {
+ backlog.pop_front();
+ }
+ }
+ //log all
+ {
+ _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().foreachNewLoggingEntry(
+ [&](const ControlThreadOutputBuffer::Entry & data)
+ {
+ //base (marker;iteration;timestamp)
+ {
+ for (auto& pair : rtLoggingEntries)
+ {
+ CSVLoggingEntry& e = *pair.second;
+ e.stream << "\n"
+ << e.marker << ";"
+ << data.iteration << ";"
+ << data.sensorValuesTimestamp.toMicroSeconds();
+ e.marker.clear();
+ }
+ }
+ //sens
+ {
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
+ {
+ const SensorValueBase* val = data.sensors.at(idxDev);
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ {
+ const auto str = val->getDataFieldAsString(idxField);
+ for (auto& pair : rtLoggingEntries)
+ {
+ CSVLoggingEntry& e = *pair.second;
+ if (e.loggedSensorDeviceValues.at(idxDev).at(idxField))
+ {
+ pair.second->stream << ";" << str;
+ }
+ }
+ }
+ }
+ }
+ //ctrl
+ {
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
+ {
+ const auto& vals = data.control.at(idxDev);
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ {
+ const ControlTargetBase* val = vals.at(idxVal);
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ {
+ const auto str = val->getDataFieldAsString(idxField);
+ for (auto& pair : rtLoggingEntries)
+ {
+ CSVLoggingEntry& e = *pair.second;
+ if (e.loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
+ {
+ pair.second->stream << ";" << str;
+ }
+ }
+ }
+ }
+ }
+ }
+ //store data to backlog
+ {
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
+ {
+ backlog.emplace_back(data, true); //true for minimal copy
+ }
+ }
+ //print + reset messages
+ {
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ {
+ if (!ptr)
+ {
+ 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;
+ }
+ }
+
+ //remove entries
+ {
+ 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)
+ {
+ rtLoggingEntries.erase(rem);
+ }
+ }
+ }
+
+ void Logging::_postOnInitRobotUnit()
+ {
+ rtLoggingTimestep = getProperty<std::size_t>("RTLoggingPeriodMs");
+ ARMARX_CHECK_LESS_W_HINT(0, rtLoggingTimestep, "The property RTLoggingPeriodMs must not be 0");
+
+ messageBufferSize = getProperty<std::size_t>("RTLoggingMessageBufferSize");
+ messageBufferMaxSize = getProperty<std::size_t>("RTLoggingMaxMessageBufferSize");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
+
+ messageBufferNumberEntries = getProperty<std::size_t>("RTLoggingMessageNumber");
+ messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLoggingMaxMessageNumber");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
+
+ rtLoggingBacklogRetentionTime = IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLoggingKeepIterationsForMs"));
+
+ ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+ }
+
+ void Logging::_postFinishDeviceInitialization()
+ {
+ //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;
+
+ 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())
+ {
+ 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();
+ fullNames.reserve(names.size());
+ for (const auto& name : names)
+ {
+ fullNames.emplace_back(
+ "sens." +
+ sd->getDeviceName() + "." +
+ name);
+ }
+ }
+ }
+ //start logging thread is done in rtinit
+ //maybe add the default log
+ {
+ const auto loggingpath = getProperty<std::string>("RTLoggingDefaultLog").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
new file mode 100644
index 0000000000000000000000000000000000000000..77bb2c0de4e6ec1c1707036997290b2f44eb128a
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -0,0 +1,215 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <atomic>
+#include <map>
+#include <vector>
+#include <fstream>
+
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include "RobotUnitModuleBase.h"
+#include "../util/ControlThreadOutputBuffer.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ class LoggingPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ public:
+ LoggingPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::size_t>(
+ "RTLoggingPeriodMs", 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>(
+ "RTLoggingDefaultLog", "",
+ "If rt logging is active and a file path is given, all data is logged to this file.");
+
+ defineOptionalProperty<std::size_t>(
+ "RTLoggingMessageNumber", 1000,
+ "Number of messages that can be logged in the control thread");
+ defineOptionalProperty<std::size_t>(
+ "RTLoggingMessageBufferSize", 1024 * 1024,
+ "Number of bytes that can be logged in the control thread");
+ defineOptionalProperty<std::size_t>(
+ "RTLoggingMaxMessageNumber", 16000,
+ "Max number of messages that can be logged in the control thread");
+ defineOptionalProperty<std::size_t>(
+ "RTLoggingMaxMessageBufferSize", 16 * 1024 * 1024,
+ "Max number of bytes that can be logged in the control thread");
+
+ defineOptionalProperty<std::size_t>(
+ "RTLoggingKeepIterationsForMs", 60 * 1000,
+ "All logging data (SensorValues, ControlTargets, Messages) is keept for this duration "
+ "and can be dunped in case of an error.");
+ }
+ };
+
+ /**
+ * @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& = GlobalIceCurrent) 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& = GlobalIceCurrent) override;
+
+ /**
+ * @brief Stops logging to the given log.
+ * @param token The log to close.
+ */
+ void stopRtLogging(const armarx::RemoteReferenceCounterBasePtr& token, const Ice::Current& = GlobalIceCurrent) 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& = GlobalIceCurrent) override;
+
+ /**
+ * @brief Returns the names of all loggable data fields.
+ * @return The names of all loggable data fields.
+ */
+ Ice::StringSeq getLoggingNames(const Ice::Current& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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;
+ };
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b532f3f47922c50eb2ac75ad30ac8a6d81c554e5
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
@@ -0,0 +1,37 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <ArmarXCore/core/ArmarXManager.h>
+
+#include "RobotUnitModuleManagement.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ void Management::_preOnInitRobotUnit()
+ {
+ additionalObjectSchedulerCount = getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue();
+ getArmarXManager()->increaseSchedulers(static_cast<int>(additionalObjectSchedulerCount));
+ }
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
new file mode 100644
index 0000000000000000000000000000000000000000..377e3889b5b86a9d17075674e544918776cb3757
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
@@ -0,0 +1,87 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotUnitModuleBase.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+
+ class ManagementPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ public:
+ ManagementPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::uint64_t>(
+ "AdditionalObjectSchedulerCount", 10,
+ "Number of ObjectSchedulers to be added");
+ }
+ };
+
+ /**
+ * @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:
+ /**
+ * @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();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// ice interface //////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /**
+ * @brief Returns whether the RobotUnit is running.
+ * @return Whether the RobotUnit is running.
+ */
+ bool isRunning(const Ice::Current& = GlobalIceCurrent) const override
+ {
+ return getRobotUnitState() == RobotUnitState::Running;
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ /// @brief The number of additional object schedulers
+ std::uint64_t additionalObjectSchedulerCount{0};
+ };
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..36ad04b621caeb6c9d3254530ea06073014218cd
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -0,0 +1,507 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotUnitModulePublisher.h"
+
+#include "../NJointControllers/NJointController.h"
+#include "../Units/RobotUnitSubUnit.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ namespace detail
+ {
+ std::function<std::string(const std::string&)> makeDebugObserverNameFixer(const std::string& prefix)
+ {
+ return
+ [prefix](const std::string & name)
+ {
+ std::string s = prefix + name;
+ std::replace(s.begin(), s.end(), '.', '_');
+ std::replace(s.begin(), s.end(), ':', '_');
+ return s;
+ };
+ }
+ }
+
+ /**
+ * \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);
+ }
+ };
+ }
+}
+
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ const std::map<std::string, NJointControllerPtr> &Publisher::getNJointControllers()
+ {
+ return ControllerManagementAttorneyForPublisher::GetNJointControllers(this);
+ }
+
+ std::string Publisher::getRobotUnitListenerTopicName(const Ice::Current &) const
+ {
+ return robotUnitListenerTopicName;
+ }
+
+ std::string Publisher::getDebugDrawerTopicName(const Ice::Current &) const
+ {
+ return debugDrawerUpdatesTopicName;
+ }
+
+ std::string Publisher::getDebugObserverTopicName(const Ice::Current &) const
+ {
+ return debugObserverTopicName;
+ }
+
+ RobotUnitListenerPrx Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
+ {
+ ARMARX_CHECK_EXPRESSION(robotUnitListenerPrx);
+ return robotUnitListenerPrx;
+ }
+
+ DebugObserverInterfacePrx Publisher::getDebugObserverProxy(const Ice::Current&) const
+ {
+ ARMARX_CHECK_EXPRESSION(debugObserverPrx);
+ return debugObserverPrx;
+ }
+
+ DebugDrawerInterfacePrx Publisher::getDebugDrawerProxy(const Ice::Current&) const
+ {
+ ARMARX_CHECK_EXPRESSION(debugDrawerPrx);
+ return debugDrawerPrx;
+ }
+
+ void Publisher::_preOnConnectRobotUnit()
+ {
+ robotUnitListenerPrx = getTopic<RobotUnitListenerPrx>(getRobotUnitListenerTopicName());
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getDebugDrawerTopicName());
+ debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getDebugObserverTopicName());
+ }
+
+ void Publisher::_preOnInitRobotUnit()
+ {
+ robotUnitListenerTopicName = getProperty<std::string>("RobotUnitListenerTopicName").getValue();
+ debugDrawerUpdatesTopicName = getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
+ debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
+
+ observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
+ observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ debugObserverSkipIterations = std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
+
+ publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());
+
+ offeringTopic(getRobotUnitListenerTopicName());
+ offeringTopic(getDebugDrawerTopicName());
+ offeringTopic(getDebugObserverTopicName());
+ getArmarXManager()->addObject(robotUnitObserver);
+ }
+
+ void Publisher::_icePropertiesInitialized()
+ {
+ robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(getIceProperties(), "", getConfigDomain());
+ addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObserver));
+ }
+
+ void Publisher::_icePropertiesUpdated(const std::set<std::string> &changedProperties)
+ {
+ 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();
+ }
+ }
+
+ void Publisher::_preFinishRunning()
+ {
+ 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_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);
+ }
+ }
+
+ void Publisher::_postFinishControlThreadInitialization()
+ {
+ //start publisher
+ publishNewSensorDataTime = TimeUtil::GetTime();
+ publisherTask = new PublisherTaskT([&] {publish({});}, publishPeriodMs, false, getName() + "_PublisherTask");
+ ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
+ publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
+ publisherTask->start();
+ }
+
+ void Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
+ {
+ auto begPublish = Now();
+ static const int spamdelay = 30;
+
+ 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__);
+
+ //get batch proxies
+ auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
+ auto listenerBatchPrx = getRobotUnitListenerProxy()->ice_batchOneway();
+ //delete NJoint queued for deletion
+ ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(this, false, listenerBatchPrx);
+ //swap buffers in
+ const bool haveActivatedControllersChanged = _module<ControlThreadDataBuffer>().activatedControllersChanged();
+ const bool haveSensorAndControlValsChanged = _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
+ //setup datastructures
+ const auto& controlThreadOutputBuffer = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer();
+ const auto& activatedControllers = _module<ControlThreadDataBuffer>().getActivatedControllers();
+ const auto& requestedJointControllers = _module<ControlThreadDataBuffer>().getRequestedJointControllers();
+
+ const auto timestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
+
+ const auto numSensorDevices = _module<Devices>().getNumberOfSensorDevices();
+ const auto numControlDevices = _module<Devices>().getNumberOfControlDevices();
+
+ const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
+ const bool debugObserverPublishControlTargetsThisIteration = observerPublishControlTargets;
+ const bool debugObserverPublishSensorValuesThisIteration = observerPublishSensorValues;
+ StringVariantBaseMap ctrlDevMap, sensorDevMap;
+ //publish publishing meta state
+ additionalMap["haveActivatedControllersChanged"] = new TimedVariant {haveActivatedControllersChanged, timestamp};
+ additionalMap["haveSensorAndControlValsChanged"] = new TimedVariant {haveSensorAndControlValsChanged, timestamp};
+ additionalMap["publishToObserver" ] = new TimedVariant {publishToObserver, timestamp};
+ //update
+ if (haveSensorAndControlValsChanged)
+ {
+ //update units
+ timingMap["publishTimings_UnitUpdate"] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant)
+ {
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
+ publishNewSensorDataTime = TimeUtil::GetTime();
+ for (const RobotUnitSubUnitPtr& rsu : UnitsAttorneyForPublisher::GetSubUnits(this))
+ {
+ if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
+ {
+ timingMap["publishTimings_UnitUpdate_" + rsu->getName()] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant){rsu->update(controlThreadOutputBuffer, activatedControllers);},
+ timestamp
+ };
+ }
+ }
+ },
+ timestamp
+ };
+ //publish sensor updates
+
+ timingMap["publishTimings_SensorUpdates"] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant)
+ {
+ for (std::size_t sensidx = 0 ; sensidx < numSensorDevices; ++sensidx)
+ {
+ const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
+ auto variants = sensVal.toVariants(timestamp);
+
+ if (!variants.empty())
+ {
+ SensorDeviceStatus status;
+ status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(this, sensidx);
+ status.sensorValue = std::move(variants);
+ status.timestampUSec = MicroNow().count();
+ if (debugObserverPublishSensorValuesThisIteration && publishToObserver)
+ {
+ transformMapKeys(
+ variants, sensorDevMap,
+ detail::makeDebugObserverNameFixer(status.deviceName + "_" + sensVal.getSensorValueType(true) + "_")
+ );
+ }
+ listenerBatchPrx->sensorDeviceStatusChanged(status);
+ }
+ }
+ },
+ timestamp
+ };
+ }
+ 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)
+ {
+ timingMap["publishTimings_ControlUpdates"] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant)
+ {
+ for (std::size_t ctrlidx = 0 ; ctrlidx < numControlDevices; ++ctrlidx)
+ {
+ const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
+
+ StringToStringVariantBaseMapMap variants;
+ for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx))
+ {
+ const auto& controlMode = ctrlVal->getControlMode();
+ variants[controlMode] = ctrlVal->toVariants(timestamp);
+ if (
+ haveSensorAndControlValsChanged &&
+ !variants[controlMode].empty() &&
+ debugObserverPublishControlTargetsThisIteration &&
+ publishToObserver
+ )
+ {
+ transformMapKeys(
+ variants[controlMode], ctrlDevMap,
+ detail::makeDebugObserverNameFixer(ctrlDev.getDeviceName() + "_" + controlMode + "_")
+ );
+ }
+ }
+ ControlDeviceStatus status;
+ const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
+ status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
+ status.deviceName = ctrlDev.getDeviceName();
+ ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(ctrlidx));
+ status.requestedControlMode = requestedJointControllers.at(ctrlidx)->getControlMode();
+ status.controlTargetValues = std::move(variants);
+ status.timestampUSec = MicroNow().count();
+ listenerBatchPrx->controlDeviceStatusChanged(status);
+ }
+ },
+ timestamp
+ };
+ }
+ //call publish hook + publish NJointController changes
+ timingMap["publishTimings_NJointControllerUpdates"] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant)
+ {
+ for (const auto& pair : getNJointControllers())
+ {
+ const NJointControllerPtr& nJointCtrl = pair.second;
+ timingMap["publishTimings_NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant)
+ {
+ //run some hook for active (used for visu)
+ NJointControllerAttorneyForPublisher::Publish(nJointCtrl, controlThreadOutputBuffer, debugDrawerPrx, debugObserverBatchPrx);
+ if (
+ NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) != nJointCtrl->isControllerActive() ||
+ NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) != nJointCtrl->isControllerRequested()
+ )
+ {
+ NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
+ listenerBatchPrx->nJointControllerStatusChanged(nJointCtrl->getControllerStatus());
+ }
+ },
+ timestamp
+ };
+ }
+ },
+ timestamp
+ };
+
+ //report new class names
+ timingMap["publishTimings_ClassNameUpdates"] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant)
+ {
+ const auto classNames = NJointControllerRegistry::getKeys();
+ if (lastReportedClasses.size() != classNames.size())
+ {
+ for (const auto& name : classNames)
+ {
+ if (!lastReportedClasses.count(name))
+ {
+ listenerBatchPrx->nJointControllerClassAdded(name);
+ lastReportedClasses.emplace(name);
+ }
+ }
+ }
+ },
+ timestamp
+ };
+ timingMap["publishTimings_RobotUnitListenerFlush"] = new TimedVariant
+ {
+ ARMARX_STOPWATCH(TimestampVariant){listenerBatchPrx->ice_flushBatchRequests();}, timestamp
+ };
+
+ if (publishToObserver)
+ {
+ timingMap["publishTimings_LastRobotUnitObserverFlush"] =
+ new TimedVariant {TimestampVariant{lastRobotUnitObserverFlush.count()}, timestamp};
+ timingMap["publishTimings_LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop.count()}, timestamp};
+ lastRobotUnitObserverFlush = ARMARX_STOPWATCH()
+ {
+ 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->controlDevicesChannel, ctrlDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
+ robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
+
+ }
+ debugObserverBatchPrx->ice_flushBatchRequests();
+ };
+ }
+ else
+ {
+ lastRobotUnitObserverFlush = std::chrono::microseconds {0};
+ }
+
+ //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;
+ }))
+ {
+ ARMARX_WARNING << "armarx::RobotUnit::publish: Some activated JointControllers are reported to be nullptr! "
+ << "(did you forget to call rtSwitchControllerSetup()?)";
+ }
+ }
+ ++publishIterationCount;
+ lastPublishLoop = MicroToNow(begPublish);
+ }
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
new file mode 100644
index 0000000000000000000000000000000000000000..21cb1a18a5a794de8139bda0ffb72d4d50de62e6
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -0,0 +1,216 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/observers/DebugObserver.h>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+
+#include "RobotUnitModuleBase.h"
+#include "../RobotUnitObserver.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
+ namespace RobotUnitModule
+ {
+ class PublisherPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ 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<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);
+ }
+ };
+
+ /**
+ * @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
+ */
+ class Publisher : virtual public ModuleBase, virtual public RobotUnitPublishingInterface
+ {
+ 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::_icePropertiesUpdated
+ void _icePropertiesUpdated(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& = GlobalIceCurrent) const override;
+ /**
+ * @brief Returns the name of the used DebugObserverTopic
+ * @return The name of the used DebugObserverTopic
+ */
+ std::string getDebugObserverTopicName(const Ice::Current& = GlobalIceCurrent) const override;
+ /**
+ * @brief Returns the name of the used RobotUnitListenerTopic
+ * @return The name of the used RobotUnitListenerTopic
+ */
+ std::string getRobotUnitListenerTopicName(const Ice::Current& = GlobalIceCurrent) const override;
+
+ /**
+ * @brief Returns the used DebugDrawerProxy
+ * @return The used DebugDrawerProxy
+ */
+ DebugDrawerInterfacePrx getDebugDrawerProxy(const Ice::Current& = GlobalIceCurrent) const override;
+ /**
+ * @brief Returns the used RobotUnitListenerProxy
+ * @return The used RobotUnitListenerProxy
+ */
+ RobotUnitListenerPrx getRobotUnitListenerProxy(const Ice::Current& = GlobalIceCurrent) const override;
+ /**
+ * @brief Returns the used DebugObserverProxy
+ * @return The used DebugObserverProxy
+ */
+ DebugObserverInterfacePrx getDebugObserverProxy(const Ice::Current& = GlobalIceCurrent) 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;
+ }
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////// Data ///////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ private:
+ 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 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 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 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 to flush the debug observer
+ std::chrono::microseconds lastRobotUnitObserverFlush;
+ /// @brief The time required by the last iteration of \ref publish
+ std::chrono::microseconds lastPublishLoop;
+
+ /// @brief The used RobotUnitObserver
+ RobotUnitObserverPtr robotUnitObserver;
+
+ /// @brief The period of the publisher loop
+ std::size_t publishPeriodMs{1};
+
+ /// @brief A proxy to the used RobotUnitListener topic
+ RobotUnitListenerPrx robotUnitListenerPrx;
+ };
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..12a389a88f1492772b1e124f3a896d37576391ec
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -0,0 +1,120 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include "RobotUnitModuleRobotData.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ const std::string &RobotData::getRobotPlatformName() const
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ return robotPlatformName;
+ }
+
+ const std::string &RobotData::getRobotNodetSeName() const
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ return robotNodeSetName;
+ }
+
+ const std::string &RobotData::getRobotProjectName() const
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ return robotProjectName;
+ }
+
+ const std::string &RobotData::getRobotFileName() const
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ return robotFileName;
+ }
+
+ std::string RobotData::getRobotName() const
+ {
+ std::lock_guard<std::mutex> guard{robotMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ return robot->getName();
+ }
+
+ VirtualRobot::RobotPtr RobotData::cloneRobot(bool updateCollisionModel) const
+ {
+ std::lock_guard<std::mutex> guard{robotMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ ARMARX_CHECK_EXPRESSION(robot);
+ const VirtualRobot::RobotPtr clone = robot->clone();
+ clone->setUpdateVisualization(false);
+ clone->setUpdateCollisionModel(updateCollisionModel);
+ return clone;
+ }
+
+ void armarx::RobotUnitModule::RobotData::_preOnInitRobotUnit()
+ {
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+ robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
+ robotFileName = getProperty<std::string>("RobotFileName").getValue();
+ robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+
+ //load robot
+ {
+ Ice::StringSeq includePaths;
+ if (!robotProjectName.empty())
+ {
+ CMakePackageFinder finder(robotProjectName);
+ Ice::StringSeq projectIncludePaths;
+ auto pathsString = finder.getDataDir();
+ boost::split(projectIncludePaths,
+ pathsString,
+ boost::is_any_of(";,"),
+ boost::token_compress_on);
+ includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ }
+ if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
+ {
+ throw UserException("Could not find robot file " + robotFileName);
+ }
+ // read the robot
+ try
+ {
+ robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
+ }
+ catch (VirtualRobot::VirtualRobotException& e)
+ {
+ throw UserException(e.what());
+ }
+ ARMARX_INFO << "Loaded robot:"
+ << "\n\tProject : " << robotProjectName
+ << "\n\tRobot file : " << robotFileName
+ << "\n\tRobotNodeSet : " << robotNodeSetName
+ << "\n\tNodeNames : " << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
+ ARMARX_CHECK_NOT_NULL(robot);
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
new file mode 100644
index 0000000000000000000000000000000000000000..b8c1161301acbe11c730c86048a0fffe79c52ecc
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -0,0 +1,135 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/observers/DebugObserver.h>
+
+#include "RobotUnitModuleBase.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ class RobotDataPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ 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>(
+ "RobotNodeSetName", "Robot",
+ "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+ defineOptionalProperty<std::string>(
+ "PlatformName", "Platform",
+ "Name of the platform (will publish values on PlatformName + 'State')");
+ }
+ };
+
+ /**
+ * @brief This \ref ModuleBase "Module" holds all high-level data about the robot.
+ *
+ * @see ModuleBase
+ */
+ class RobotData : virtual public ModuleBase
+ {
+ 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 _preOnInitRobotUnit();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////////////// 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 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 robot's model.
+ boost::shared_ptr<VirtualRobot::Robot> robot;
+ /// @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
new file mode 100644
index 0000000000000000000000000000000000000000..1f4b83449c63f57d7b7971ecb9b6769803587f2b
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -0,0 +1,268 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Obstacle.h>
+
+#include "RobotUnitModuleSelfCollisionChecker.h"
+#include "RobotUnitModuleRobotData.h"
+
+#include "../NJointControllers/NJointController.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ void SelfCollisionChecker::_preOnInitRobotUnit()
+ {
+ {
+ const std::string colModelsString = getProperty<std::string>("SelfCollisionpairsToCheck").getValue();
+ std::vector<std::string> groups;
+ if (!colModelsString.empty())
+ {
+ groups = armarx::Split(colModelsString, ";", true, true);
+ }
+ for (std::string& group : groups)
+ {
+ // Removing parentheses
+ boost::trim_if(group, boost::is_any_of(" \t{}"));
+ std::set<std::string> nodes;
+ {
+ auto splittedRaw = armarx::Split(group, ",", true, true);
+ if(splittedRaw.size() < 2)
+ {
+ continue;
+ }
+ for(auto& subentry : splittedRaw)
+ {
+ boost::trim_if(subentry, boost::is_any_of(" \t{}"));
+ if(selfCollisionAvoidanceRobot->hasRobotNodeSet(subentry))
+ {
+ //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());
+ }
+ }
+ else if(selfCollisionAvoidanceRobot->hasRobotNode(subentry))
+ {
+ //the entry is a node
+ if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)->getCollisionModel())
+ {
+
+ ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '"
+ << selfCollisionAvoidanceRobot->getRobotNode(subentry)->getName() << "'";
+ continue;
+ }
+ nodes.emplace(subentry);
+ }
+ else if(subentry == "FLOOR")
+ {
+ //the entry is the floor
+ nodes.emplace(subentry);
+ }
+ else
+ {
+ ARMARX_ERROR << "No RobotNodeSet or RobotNode with name '"
+ << subentry
+ << "' defined in " << _module<RobotData>().getRobotFileName() << ". Skipping.";
+ continue;
+ }
+ }
+ }
+
+
+
+ for(auto it1 = nodes.begin(); it1 != nodes.end(); ++it1)
+ {
+ auto it2 = it1;
+ ++it2;
+ for(; it2 != nodes.end(); ++it2)
+ {
+ namePairsToCheck.emplace(*it1, *it2);
+ }
+ }
+ }
+ }
+ setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheckFrequency").getValue());
+ }
+
+ void SelfCollisionChecker::_postOnInitRobotUnit()
+ {
+ setSelfCollisionAvoidanceDistance(getProperty<float>("MinSelfDistance").getValue());
+ }
+
+ void SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current&)
+ {
+ 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())
+ {
+ 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"));
+ VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(200000, 200000, std::min(0.001f, minSelfDistance / 2));
+ boxOb->setGlobalPose(Eigen::Matrix4f::Identity());
+ boxOb->setName("FLOOR");
+ floor->addSceneObject(boxOb);
+ }
+ //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") ?
+ floor->getSceneObject(0) :
+ selfCollisionAvoidanceRobot->getRobotNode(pair.first );
+
+ VirtualRobot::SceneObjectPtr second =
+ (pair.second == "FLOOR") ?
+ floor->getSceneObject(0) :
+ selfCollisionAvoidanceRobot->getRobotNode(pair.second);
+
+ nodePairsToCheck.emplace_back(first, second);
+ }
+ }
+
+ void SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
+ {
+ if(freq < 0)
+ {
+ throw InvalidArgumentException{
+ std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal frequency:" + std::to_string(freq)
+ };
+ }
+ checkFrequency = freq;
+ }
+
+ bool SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
+ {
+ return checkFrequency != 0;
+ }
+
+ float SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
+ {
+ return checkFrequency;
+ }
+
+ float SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
+ {
+ return minSelfDistance;
+ }
+
+ void SelfCollisionChecker::componentPropertiesUpdated(const std::set<std::string> &changed)
+ {
+ if(changed.count("SelfCollisionCheckFrequency"))
+ {
+ setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheckFrequency").getValue());
+ }
+ if(changed.count("MinSelfDistance"))
+ {
+ setSelfCollisionAvoidanceDistance(getProperty<float>("MinSelfDistance").getValue());
+ }
+ }
+
+ void SelfCollisionChecker::_postFinishControlThreadInitialization()
+ {
+ selfCollisionAvoidanceThread = std::thread{[&]{selfCollisionAvoidanceTask();}};
+ }
+
+ void SelfCollisionChecker::selfCollisionAvoidanceTask()
+ {
+ while(true)
+ {
+ const auto startT = std::chrono::high_resolution_clock::now();
+ //done
+ if(isShuttingDown())
+ {
+ return;
+ }
+ const auto freq = checkFrequency.load();
+ if (
+ _module<ControlThread>().getEmergencyStopState() ||
+ freq == 0
+ )
+ {
+ //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);
+ //check
+ for(const auto& pair : nodePairsToCheck)
+ {
+ if(selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
+ {
+ ARMARX_WARNING << "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({});
+ continue;
+ }
+ }
+ }
+ //sleep remaining
+ std::this_thread::sleep_until(startT + std::chrono::microseconds{static_cast<int64_t>(1000000 / freq)});
+ }
+ }
+
+ void SelfCollisionChecker::_preFinishRunning()
+ {
+ ARMARX_INFO << "Stopping Self Collision Avoidance.";
+ if (selfCollisionAvoidanceThread.joinable())
+ {
+ selfCollisionAvoidanceThread.join();
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
new file mode 100644
index 0000000000000000000000000000000000000000..5fb8e2faae8b826afe9c5ea977c718053803cbc1
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
@@ -0,0 +1,165 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <thread>
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include "RobotUnitModuleBase.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ class SelfCollisionCheckerPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ public:
+ SelfCollisionCheckerPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<float>(
+ "SelfCollisionCheckFrequency", 10,
+ "Frequency [Hz] of self collision checking (default = 10). If set to 0, no cllisions will be checked.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "MinSelfDistance", 20,
+ "Minimum allowed distance (mm) between each collision model before entering emergency stop (default = 20).",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<std::string>(
+ "SelfCollisionpairsToCheck", "",
+ "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.");
+ }
+ };
+
+ /**
+ * @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:
+ /**
+ * @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::_postOnInitRobotUnit
+ void _postOnInitRobotUnit();
+ /// @see ModuleBase::_postFinishControlThreadInitialization
+ void _postFinishControlThreadInitialization();
+ /// @see ModuleBase::_preFinishRunning
+ void _preFinishRunning();
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////// 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) 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& = GlobalIceCurrent) const override;
+ /**
+ * @brief Returns the frequency of self collision checks.
+ * @return The frequency of self collision checks.
+ */
+ float getSelfCollisionAvoidanceFrequency(const Ice::Current& = GlobalIceCurrent) 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& = GlobalIceCurrent) const override;
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////////// Component interface ///////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /// @see armarx::Component::componentPropertiesUpdated
+ void componentPropertiesUpdated(const std::set<std::string>& changed) 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;
+ };
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..accc6c49421fa76142ed857de4b842d0ea8eb1c7
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -0,0 +1,541 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <ArmarXCore/core/IceManager.h>
+#include <ArmarXCore/core/IceGridAdmin.h>
+
+#include "RobotUnitModuleUnits.h"
+
+#include "../Units/ForceTorqueSubUnit.h"
+#include "../Units/InertialMeasurementSubUnit.h"
+#include "../Units/KinematicSubUnit.h"
+#include "../Units/PlatformSubUnit.h"
+#include "../Units/TCPControllerSubUnit.h"
+#include "../Units/TrajectoryControllerSubUnit.h"
+
+#include "../SensorValues/SensorValue1DoFActuator.h"
+
+#include "../RobotUnit.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ 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());
+ }
+
+ virtual void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = GlobalIceCurrent) final
+ {
+ if(getEmergencyStopState() == state)
+ {
+ return;
+ }
+ controlThreadModule->setEmergencyStopStateNoReportToTopic(state);
+ emergencyStopTopic->reportEmergencyStopState(state);
+ }
+ virtual EmergencyStopState getEmergencyStopState(const Ice::Current& = GlobalIceCurrent) const final
+ {
+ return controlThreadModule->getEmergencyStopState();
+ }
+
+ protected:
+ virtual void onInitComponent() final
+ {
+ armarx::ManagedIceObject::offeringTopic(emergencyStopTopicName);
+ }
+ virtual void onConnectComponent() final
+ {
+ emergencyStopTopic = armarx::ManagedIceObject::getTopic<EmergencyStopListenerPrx>(emergencyStopTopicName);
+ setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
+ }
+
+ virtual std::string getDefaultName() const final
+ {
+ return "EmergencyStopMaster";
+ }
+
+ protected:
+ ControlThread* const controlThreadModule;
+ const std::string emergencyStopTopicName;
+ EmergencyStopListenerPrx emergencyStopTopic;
+ };
+ }
+}
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ Ice::ObjectProxySeq Units::getUnits(const Ice::Current &) const
+ {
+ 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;
+ }
+
+ Ice::ObjectPrx Units::getUnit(const std::string &staticIceId, const Ice::Current &) const
+ {
+ //no lock required
+ ManagedIceObjectPtr unit = getUnit(staticIceId);
+ if (unit)
+ {
+ return unit->getProxy(-1, true);
+ }
+ return nullptr;
+ }
+
+ const EmergencyStopMasterInterfacePtr &Units::getEmergencyStopMaster() const
+ {
+ return emergencyStopMaster;
+ }
+
+ void Units::initializeDefaultUnits()
+ {
+ auto beg = MicroNow();
+ {
+ 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! " << (MicroNow() - beg).count() << " us";
+ }
+
+ void Units::initializeKinematicUnit()
+ {
+ /// TODO move init code to Kinematic sub unit
+ using UnitT = KinematicSubUnit;
+ using IfaceT = KinematicUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+ //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; \
+ NJointKinematicUnitPassThroughControllerPtr& pt = ctrl; \
+ JointController* jointCtrl = controlDevice->getJointController(controlMode); \
+ if (jointCtrl && jointCtrl->getControlTarget()->isA<CtargT>()) \
+ { \
+ NJointKinematicUnitPassThroughControllerConfigPtr config = new NJointKinematicUnitPassThroughControllerConfig; \
+ config->controlMode=controlMode; \
+ config->deviceName=controlDeviceName; \
+ const NJointControllerPtr& nJointCtrl = _module<ControllerManagement>().createNJointController( \
+ "NJointKinematicUnitPassThroughController", \
+ "NJointKU_PTCtrl_"+controlDeviceName+"_"+controlMode, \
+ config, \
+ false, true); \
+ pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl); \
+ ARMARX_CHECK_EXPRESSION(pt); \
+ } \
+ }
+ initializeKinematicUnit_tmp_helper(ControlModes::Position1DoF, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
+ initializeKinematicUnit_tmp_helper(ControlModes::Velocity1DoF, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
+ initializeKinematicUnit_tmp_helper(ControlModes::Torque1DoF , ControlTarget1DoFActuatorTorque , ad.ctrlTor)
+ #undef initializeKinematicUnit_tmp_helper
+
+
+ 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;
+ }
+ 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 + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //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());
+ 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)
+ );
+ //add
+ addUnit(std::move(unit));
+ }
+
+ void Units::initializePlatformUnit()
+ {
+ 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>().getRobotPlatformName());
+ 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", configName + "_VelPTContoller",
+ config, false, true
+ )
+ );
+ ARMARX_CHECK_EXPRESSION(ctrl);
+ unit->pt = ctrl;
+ unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
+ //add
+ addUnit(std::move(unit));
+ }
+
+ void Units::initializeForceTorqueUnit()
+ {
+ 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));
+ }
+
+ void Units::initializeInertialMeasurementUnit()
+ {
+ using UnitT = InertialMeasurementSubUnit;
+ using IfaceT = InertialMeasurementUnitInterface;
+
+ 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;
+ }
+ //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));
+ }
+
+ void Units::initializeTrajectoryControllerUnit()
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ using UnitT = TrajectoryControllerSubUnit;
+ if (!getProperty<bool>("CreateTrajectoryPlayer").getValue())
+ {
+ return;
+ }
+
+ //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::initializeTcpControllerUnit()
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+
+ if (!getProperty<bool>("CreateTrajectoryPlayer").getValue())
+ {
+ return;
+ }
+ using UnitT = TCPControllerSubUnit;
+
+ //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;
+ }
+
+ void Units::addUnit(ManagedIceObjectPtr 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));
+ }
+
+ void Units::_icePropertiesInitialized()
+ {
+ 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));
+ }
+
+ // create TrajectoryControllerSubUnit
+
+ {
+ 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::_preFinishRunning()
+ {
+ //remove all units
+ subUnits.clear();
+ for (ManagedIceObjectPtr& unit : units)
+ {
+ getArmarXManager()->removeObjectBlocking(unit->getName());
+ }
+ units.clear();
+ }
+
+ void Units::_preOnInitRobotUnit()
+ {
+ 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!";
+ }
+
+ void Units::_postOnInitRobotUnit()
+ {
+ unitCreateRobot = _module<RobotData>().cloneRobot();
+ }
+
+ void Units::_postOnExitRobotUnit()
+ {
+ ARMARX_DEBUG << "remove EmergencyStopMaster";
+ try
+ {
+ 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
+ {
+ auto guard = getGuard();
+ for (const ManagedIceObjectPtr& unit : units)
+ {
+ if (unit->ice_isA(staticIceId))
+ {
+ return unit;
+ }
+ }
+ return ManagedIceObject::NullPtr;
+ }
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
new file mode 100644
index 0000000000000000000000000000000000000000..692f2aa43a240686076e09a2b823f7b13a41edfb
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -0,0 +1,292 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include "RobotUnitModuleBase.h"
+#include "../Units/RobotUnitSubUnit.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ class UnitsPropertyDefinitions: public ModuleBasePropertyDefinitions
+ {
+ public:
+ UnitsPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>(
+ "KinematicUnitName", "KinematicUnit",
+ "The name of the created kinematic 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>(
+ "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>(
+ "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.");
+ }
+ };
+
+ /**
+ * @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:
+ /**
+ * @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::_postOnInitRobotUnit
+ void _postOnInitRobotUnit();
+ /// @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;
+ /**
+ * @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;
+
+ /**
+ * @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;
+
+ //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 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 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;
+ };
+ }
+}
+
+#include "RobotUnitModuleUnits.ipp"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp
new file mode 100644
index 0000000000000000000000000000000000000000..557577805b7e10f27d68779300fe1ee3c9b98b11
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.ipp
@@ -0,0 +1,98 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotUnitModuleUnits.h"
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ inline Units& Units::Instance()
+ {
+ return ModuleBase::Instance<Units>();
+ }
+
+ inline KinematicUnitInterfacePrx Units::getKinematicUnit(const Ice::Current&) const
+ {
+ return getUnitPrx<KinematicUnitInterface>();
+ }
+ inline ForceTorqueUnitInterfacePrx Units::getForceTorqueUnit(const Ice::Current&) const
+ {
+ return getUnitPrx<ForceTorqueUnitInterface>();
+ }
+ inline InertialMeasurementUnitInterfacePrx Units::getInertialMeasurementUnit(const Ice::Current&) const
+ {
+ return getUnitPrx<InertialMeasurementUnitInterface>();
+ }
+ inline PlatformUnitInterfacePrx Units::getPlatformUnit(const Ice::Current&) const
+ {
+ return getUnitPrx<PlatformUnitInterface>();
+ }
+ inline TCPControlUnitInterfacePrx Units::getTCPControlUnit(const Ice::Current&) const
+ {
+ return getUnitPrx<TCPControlUnitInterface>();
+ }
+ inline TrajectoryPlayerInterfacePrx Units::getTrajectoryPlayer(const Ice::Current&) const
+ {
+ return getUnitPrx<TrajectoryPlayerInterface>();
+ }
+
+ template<class T>
+ inline typename T::PointerType Units::getUnit() const
+ {
+ return T::PointerType::dynamicCast(getUnit(T::ice_staticId()));
+ }
+
+ template<class T>
+ inline typename T::ProxyType Units::getUnitPrx() const
+ {
+ return T::ProxyType::uncheckedCast(getUnit(T::ice_staticId(), GlobalIceCurrent));
+ }
+
+ inline KinematicUnitInterfacePtr Units::getKinematicUnit() const
+ {
+ return getUnit<KinematicUnitInterface>();
+ }
+ inline ForceTorqueUnitInterfacePtr Units::getForceTorqueUnit() const
+ {
+ return getUnit<ForceTorqueUnitInterface>();
+ }
+ inline InertialMeasurementUnitInterfacePtr Units::getInertialMeasurementUnit() const
+ {
+ return getUnit<InertialMeasurementUnitInterface>();
+ }
+ inline PlatformUnitInterfacePtr Units::getPlatformUnit() const
+ {
+ return getUnit<PlatformUnitInterface>();
+ }
+ inline TCPControlUnitInterfacePtr Units::getTCPControlUnit() const
+ {
+ return getUnit<TCPControlUnitInterface>();
+ }
+ inline TrajectoryPlayerInterfacePtr Units::getTrajectoryPlayer() const
+ {
+ return getUnit<TrajectoryPlayerInterface>();
+ }
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h
new file mode 100644
index 0000000000000000000000000000000000000000..75c57423496559a2a7cca4437f67e0ab3c0542bd
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h
@@ -0,0 +1,36 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotUnitModuleBase.h"
+
+#include "RobotUnitModuleDevices.h"
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleControlThread.h"
+#include "RobotUnitModuleUnits.h"
+#include "RobotUnitModuleLogging.h"
+#include "RobotUnitModulePublisher.h"
+#include "RobotUnitModuleRobotData.h"
+#include "RobotUnitModuleControllerManagement.h"
+#include "RobotUnitModuleManagement.h"
+#include "RobotUnitModuleSelfCollisionChecker.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
index 499f042b367a77303f9cf69a451438836e1e7da8..173d6f616c41438d52850e24cae16ab7965e7e17 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
@@ -2,8 +2,16 @@
#include <ArmarXCore/observers/Observer.h>
+#include "util.h"
+
namespace armarx
{
+ namespace RobotUnitModule
+ {
+ class Publisher;
+ }
+
+ TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
class RobotUnitObserver : public Observer
{
@@ -20,7 +28,7 @@ namespace armarx
void onInitObserver() override;
void onConnectObserver() override;
- friend class RobotUnit;
+ friend class RobotUnitModule::Publisher;
// ManagedIceObject interface
protected:
@@ -29,6 +37,5 @@ namespace armarx
return "RobotUnitObserver";
}
};
-
-} // namespace armarx
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
index 293880b631ed97978cc2955ec1a14b8bbd254443..661ef61773d0a7de94ae228a03fd51b0dea7292a 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
@@ -28,6 +28,8 @@
#include <ArmarXCore/observers/variant/TimedVariant.h>
+#include "../util/HeterogenousContinuousContainerMacros.h"
+
#include "../util.h"
namespace armarx
@@ -88,77 +90,49 @@ namespace armarx
virtual std::string getDataFieldAsString(std::size_t i) const = 0;
//management functions
- virtual void _copyTo(SensorValueBase* target) const = 0;
template<class T, class = typename std::enable_if<std::is_base_of<SensorValueBase, T>::value>::type>
void _copyTo(std::unique_ptr<T>& target) const
{
_copyTo(target.get());
}
- virtual std::unique_ptr<SensorValueBase> _clone() const = 0;
-
- virtual std::size_t _sizeInBytes() const = 0;
- virtual std::size_t _alignof() const = 0;
- virtual SensorValueBase* _placementConstruct(void* place) const = 0;
-
ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
SensorValueHasGetClassMemberInfo,
GetClassMemberInfo, SensorValueInfo<T>(*)(void));
+
+ ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(SensorValueBase)
};
}
-#define DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
- using SensorValueBase = ::armarx::SensorValueBase; \
- using VariantBasePtr = ::armarx::VariantBasePtr; \
- virtual SensorValueBase* _placementConstruct(void* place) const override \
- { \
- return new(place) std::decay<decltype(*this)>::type; \
- } \
- virtual std::size_t _sizeInBytes() const override \
- { \
- return sizeof(std::decay<decltype(*this)>::type); \
- } \
- virtual std::size_t _alignof() const override \
- { \
- return alignof(std::decay<decltype(*this)>::type); \
- } \
- virtual void _copyTo(SensorValueBase* target) const override \
- { \
- const auto targAsThis = target->asA<std::decay<decltype(*this)>::type>(); \
- ARMARX_CHECK_NOT_NULL(targAsThis); \
- *targAsThis = *this; \
- } \
- virtual std::unique_ptr<SensorValueBase> _clone() const override \
- { \
- std::unique_ptr<SensorValueBase> c {new std::decay<decltype(*this)>::type}; \
- SensorValueBase::_copyTo(c); \
- return c; \
- } \
- virtual std::string getSensorValueType(bool withoutNamespaceSpecifier = false) const override \
- { \
- return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
- } \
- void _check_for_static_GetClassMemberInfo_overload() \
- { \
- static_assert(SensorValueHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
- "This class has to implement GetClassMemberInfo() returning " \
- "an instance of SensorValueInfo<THIS_CLASS_TYPE>"); \
- } \
- virtual std::map<std::string, armarx::VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp,this); \
- } \
- virtual std::size_t getNumberOfDataFields() const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
- } \
- virtual std::string getDataFieldAsString(std::size_t i) const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAsString(this, i); \
- } \
- virtual std::vector<std::string> getDataFieldNames() const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
+#define DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
+ ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ using SensorValueBase = ::armarx::SensorValueBase; \
+ using VariantBasePtr = ::armarx::VariantBasePtr; \
+ std::string getSensorValueType(bool withoutNamespaceSpecifier = false) const override \
+ { \
+ return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
+ } \
+ void _check_for_static_GetClassMemberInfo_overload() \
+ { \
+ static_assert(SensorValueHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
+ "This class has to implement GetClassMemberInfo() returning " \
+ "an instance of SensorValueInfo<THIS_CLASS_TYPE>"); \
+ } \
+ std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp,this); \
+ } \
+ std::size_t getNumberOfDataFields() const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
+ } \
+ std::string getDataFieldAsString(std::size_t i) const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAsString(this, i); \
+ } \
+ std::vector<std::string> getDataFieldNames() const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
}
namespace armarx
@@ -173,4 +147,3 @@ namespace armarx
}
};
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp b/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
index 0abba9c4e92641556a49aa5a43afea6db08c2bf2..9b84a642a771e169886e6232bf9207387b75a5bc 100644
--- a/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
@@ -19,6 +19,81 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* 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"
@@ -51,3 +126,4 @@
#include "util.h"
//this file is only for syntax checks
+*/
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index ea17140882f751e83b18b4210bca26c19e7457ee..70f0922a15a5e3687a7f9c70477047d195aa7432 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -39,7 +39,7 @@
namespace armarx
{
- class RobotUnit;
+ TYPEDEF_PTRS_HANDLE(RobotUnit);
TYPEDEF_PTRS_HANDLE(KinematicSubUnit);
class KinematicSubUnit:
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
index 74d52cb225de13b3b027a800d8f979523a931f69..c84b344b6155615f39da64e53c158c018bcf3427 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
@@ -37,4 +37,3 @@ namespace armarx
virtual void update(const SensorAndControl& sc, const JointAndNJointControllers& c) = 0;
};
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index 6dca346a5c2bd056a5d402f7edb55926fd455650..f1cf80f9216e7f582386333a73c97527c26d6402 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -40,8 +40,6 @@ void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
ARMARX_CHECK_EXPRESSION(!robotUnit);
ARMARX_CHECK_EXPRESSION(rUnit);
robotUnit = rUnit;
-
- robotSync.reset(new RobotSynchronization(robotUnit, coordinateTransformationRobot, coordinateTransformationRobot->getRobotNodeNames()));
}
void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
@@ -92,7 +90,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
tcpController = ctrl;
}
- robotSync->sync();
+ robotUnit->updateVirtualRobot(coordinateTransformationRobot);
float xVel = 0.f;
float yVel = 0.f;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index 7ba8b05af8bdf2e7a4ca3d271a5cf0e68cedecc7..2dd3c3c6f3a5a6922fe47dcd99777b40bcb08797 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -23,7 +23,6 @@
#pragma once
#include <RobotAPI/interface/units/TCPControlUnit.h>
-#include "../RobotSynchronization.h"
#include "RobotUnitSubUnit.h"
#include "../NJointControllers/NJointTCPController.h"
@@ -31,7 +30,7 @@
namespace armarx
{
- class RobotUnit;
+ TYPEDEF_PTRS_HANDLE(RobotUnit);
TYPEDEF_PTRS_HANDLE(TCPControllerSubUnit);
class TCPControllerSubUnit :
@@ -53,7 +52,6 @@ namespace armarx
// mutable std::mutex dataMutex;
RobotUnit* robotUnit = NULL;
VirtualRobot::RobotPtr coordinateTransformationRobot;
- RobotSynchronizationPtr robotSync;
// ManagedIceObject interface
protected:
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index f5c0170911f01d3c6b0c0c42fc197a56ee73d593..ab598eb70242718fed9661aafb82fed61ba15f3f 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -30,7 +30,6 @@
#include "KinematicSubUnit.h"
-
namespace armarx
{
class TrajectoryControllerSubUnitPropertyDefinitions:
@@ -164,4 +163,3 @@ namespace armarx
RecursiveMutex controllerMutex;
};
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util.h b/source/RobotAPI/components/units/RobotUnit/util.h
index c83cc4b68ab1a4a92c370f3cb3c8c442dbc82b99..091333dbb03f8921cd7a6778622b9af361367757 100644
--- a/source/RobotAPI/components/units/RobotUnit/util.h
+++ b/source/RobotAPI/components/units/RobotUnit/util.h
@@ -19,7 +19,6 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
#pragma once
#include <ArmarXCore/core/util/PropagateConst.h>
@@ -32,4 +31,3 @@
#include "util/introspection/ClassMemberInfo.h"
#include <type_traits>
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h b/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
index 0ad2e3cc01e3ee0a41710acfa186bc6e8093fa5a..639e9efb425ff7109ef25f01ff28eefa10fc5a53 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
@@ -19,7 +19,6 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
#pragma once
#include <atomic>
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 8d74623f09e12dfddb8c5df596e4d9b3af4c74b3..4355bed67a563a1b97c62012bae2caa473f517ff 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -30,10 +30,6 @@ namespace armarx
ControlThreadOutputBuffer::Entry& ControlThreadOutputBuffer::getWriteBuffer()
{
ARMARX_CHECK_EXPRESSION(isInitialized);
- if (useTripleBuffer)
- {
- return *tripleBuffer.getWriteBuffer();
- }
return entries.at(toBounds(writePosition));
}
@@ -41,21 +37,12 @@ namespace armarx
{
ARMARX_CHECK_EXPRESSION(isInitialized);
getWriteBuffer().iteration = writePosition;
- if (useTripleBuffer)
- {
- tripleBuffer.commitWrite();
- }
++writePosition;
}
const ControlThreadOutputBuffer::Entry& ControlThreadOutputBuffer::getReadBuffer() const
{
ARMARX_CHECK_EXPRESSION(isInitialized);
- if (useTripleBuffer)
- {
- return *tripleBuffer.getReadBuffer();
- }
-
if (!onePastReadPosition)
{
//data is not initialized
@@ -67,10 +54,6 @@ namespace armarx
bool ControlThreadOutputBuffer::updateReadBuffer() const
{
ARMARX_CHECK_EXPRESSION(isInitialized);
- if (useTripleBuffer)
- {
- return tripleBuffer.updateReadBuffer();
- }
std::size_t writePosition = this->writePosition;
if (onePastReadPosition == writePosition)
{
@@ -84,14 +67,12 @@ namespace armarx
void ControlThreadOutputBuffer::resetLoggingPosition() const
{
ARMARX_CHECK_EXPRESSION(isInitialized);
- ARMARX_CHECK_EXPRESSION(!useTripleBuffer);
onePastLoggingReadPosition = writePosition.load();
}
void ControlThreadOutputBuffer::foreachNewLoggingEntry(std::function<void (const ControlThreadOutputBuffer::Entry&)> consumer)
{
ARMARX_CHECK_EXPRESSION(isInitialized);
- ARMARX_CHECK_EXPRESSION(!useTripleBuffer);
if (writePosition - onePastLoggingReadPosition >= numEntries)
{
ARMARX_ERROR << "There are " << writePosition - onePastLoggingReadPosition
@@ -135,11 +116,7 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(!isInitialized);
this->numEntries = numEntries;
//decide whether to use a triple buffer (in case no rtlogging is used)
- if (!numEntries)
- {
- useTripleBuffer = true;
- numEntries = 3;
- }
+ ARMARX_CHECK_NOT_EQUAL(numEntries, 0);
entries.reserve(numEntries);
entries.emplace_back(
controlDevices, sensorDevices,
@@ -150,16 +127,6 @@ namespace armarx
entries.emplace_back(entries.at(0));
}
- //init triple buffer if we use it
- if (useTripleBuffer)
- {
- ARMARX_DEBUG << "Using a triplebuffer.";
- tripleBuffer.reinitAllBuffers(&entries.at(0), &entries.at(1), &entries.at(2));
- }
- else
- {
- ARMARX_DEBUG << "Using a circular buffer.";
- }
isInitialized = true;
return entries.at(0).getDataBufferSize();
}
@@ -207,7 +174,7 @@ namespace armarx
return *this;
}
- void detail::RtMessageLogEntryBase::print(Ice::Int rtThreadId) const
+ void detail::RtMessageLogEntryBase::print(Ice::Int controlThreadId) const
{
if (printMsg)
{
@@ -216,7 +183,7 @@ namespace armarx
(*loghelper(file().c_str(), line(), func().c_str())
->setBacktrace(false)
->setTag({"RtLogMessages"})
- ->setThreadId(rtThreadId)
+ ->setThreadId(controlThreadId)
)
<< loggingLevel
<< ::deactivateSpam(deactivateSpamSec, to_string(deactivateSpamTag_))
@@ -252,7 +219,7 @@ namespace armarx
const RtMessageLogEntryBase* entry = other.entries.at(idx);
ARMARX_CHECK_NOT_NULL(entry);
maxAlign = std::max(maxAlign, entry->_alignof());
- void* place = std::align(entry->_alignof(), entry->_sizeInBytes(), bufferPlace, bufferSpace);
+ void* place = std::align(entry->_alignof(), entry->_sizeof(), bufferPlace, bufferSpace);
const auto hint = ARMARX_STREAM_PRINTER
{
out << "entry " << idx << " of " << other.entriesWritten
@@ -261,7 +228,7 @@ namespace armarx
<< "\nbuffer size = " << buffer.size()
<< "\nbuffer place = " << bufferPlace
<< "\nbuffer space = " << bufferSpace
- << "\nentry size = " << entry->_sizeInBytes()
+ << "\nentry size = " << entry->_sizeof()
<< "\nentry align = " << entry->_alignof()
<< "\n"
<< "\nother buffer size = " << other.buffer.size()
@@ -274,7 +241,7 @@ namespace armarx
ARMARX_CHECK_LESS_EQUAL_W_HINT(static_cast<void*>(&buffer.front()), static_cast<void*>(place), hint);
ARMARX_CHECK_LESS_EQUAL_W_HINT(static_cast<void*>(place), static_cast<void*>(&buffer.back()), hint);
ARMARX_CHECK_LESS_EQUAL_W_HINT(
- static_cast<void*>(static_cast<std::uint8_t*>(place) + entry->_sizeInBytes() - 1),
+ static_cast<void*>(static_cast<std::uint8_t*>(place) + entry->_sizeof() - 1),
static_cast<void*>(&buffer.back()),
hint
);
@@ -283,8 +250,8 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(!entries.at(entriesWritten));
// ARMARX_IMPORTANT << "PRE";
entries.at(entriesWritten++) = entry->_placementCopyConstruct(place);
- bufferSpace -= entry->_sizeInBytes();
- bufferPlace = static_cast<std::uint8_t*>(place) + entry->_sizeInBytes();
+ bufferSpace -= entry->_sizeof();
+ bufferPlace = static_cast<std::uint8_t*>(place) + entry->_sizeof();
// ARMARX_IMPORTANT << "POST";
}
ARMARX_CHECK_EQUAL(entriesWritten, other.entriesWritten);
@@ -377,7 +344,7 @@ namespace armarx
bytes += alignShift;
maxAlign = std::max(maxAlign, align);
- bytes += sd->getSensorValue()->_sizeInBytes();
+ bytes += sd->getSensorValue()->_sizeof();
}
for (const ControlDevicePtr& cd : controlDevices.values())
{
@@ -388,7 +355,7 @@ namespace armarx
bytes += alignShift;
maxAlign = std::max(maxAlign, align);
- bytes += ctrl->getControlTarget()->_sizeInBytes();
+ bytes += ctrl->getControlTarget()->_sizeof();
}
}
return bytes + maxAlign - 1;
@@ -413,7 +380,7 @@ namespace armarx
for (const SensorDevicePtr& sd : sensorDevices.values())
{
const SensorValueBase* sv = sd->getSensorValue();
- sensors.emplace_back(sv->_placementConstruct(getAlignedPlace(sv->_sizeInBytes(), sv->_alignof())));
+ sensors.emplace_back(sv->_placementConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
}
control.reserve(controlDevices.size());
@@ -426,7 +393,7 @@ namespace armarx
for (const JointController* ctrl : ctrls)
{
const ControlTargetBase* ct = ctrl->getControlTarget();
- ctargs.emplace_back(ct->_placementConstruct(getAlignedPlace(ct->_sizeInBytes(), ct->_alignof())));
+ ctargs.emplace_back(ct->_placementConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
ctargs.back()->reset();
}
}
@@ -457,7 +424,7 @@ namespace armarx
sensors.reserve(other.sensors.size());
for (const SensorValueBase* sv : other.sensors)
{
- sensors.emplace_back(sv->_placementConstruct(getAlignedPlace(sv->_sizeInBytes(), sv->_alignof())));
+ sensors.emplace_back(sv->_placementConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
}
control.reserve(other.control.size());
@@ -468,7 +435,7 @@ namespace armarx
ctargs.reserve(cdctargs.size());
for (const ControlTargetBase* ct : cdctargs)
{
- ctargs.emplace_back(ct->_placementConstruct(getAlignedPlace(ct->_sizeInBytes(), ct->_alignof())));
+ ctargs.emplace_back(ct->_placementConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
ctargs.back()->reset();
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 37a705e273c0f816f27d49aa5f5f125a96eb466a..dcfcb94cb0ca285ae8544fb3883f84bf02d8d490 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -21,30 +21,9 @@
*/
#pragma once
-#include <memory>
-#if __GNUC__< 5
-namespace std
-{
- inline void* align(size_t alignment, size_t bytes, void*& bufferPlace, size_t& bufferSpace) noexcept
- {
- const auto uiptr = reinterpret_cast<uintptr_t>(bufferPlace);
- const auto alignedPlace = (uiptr - 1u + alignment) & -alignment;
- const auto spaceRequired = alignedPlace - uiptr;
- if ((bytes + spaceRequired) > bufferSpace)
- {
- return nullptr;
- }
- else
- {
- bufferSpace -= spaceRequired;
- return bufferPlace = reinterpret_cast<void*>(alignedPlace);
- }
- }
-}
-#endif
+#include "HeterogenousContinuousContainer.h"
-#include <memory>
#include <vector>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -66,26 +45,22 @@ namespace armarx
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 rtThreadId) const;
+ 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;
-
- virtual std::size_t _sizeInBytes() const = 0;
- virtual std::size_t _alignof() const = 0;
- virtual RtMessageLogEntryBase* _placementCopyConstruct(void* place) const = 0;
-
- virtual ~RtMessageLogEntryBase() = default;
-
private:
MessageType loggingLevel {eUNDEFINED};
float deactivateSpamSec {0};
@@ -99,12 +74,11 @@ namespace armarx
static RtMessageLogEntryDummy Instance;
protected:
std::string format() const final override;
- std::size_t _sizeInBytes() const final override;
- std::size_t _alignof() const final override;
- RtMessageLogEntryBase* _placementCopyConstruct(void*) 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
};
}
}
@@ -112,6 +86,12 @@ namespace armarx
{
class RobotUnit;
class ControlThreadOutputBuffer;
+
+ namespace RobotUnitModule
+ {
+ class Logging;
+ }
+
namespace detail
{
struct RtMessageLogBuffer
@@ -139,6 +119,7 @@ namespace armarx
friend struct ControlThreadOutputBufferEntry;
friend class ::armarx::ControlThreadOutputBuffer;
friend class ::armarx::RobotUnit;
+ friend class ::armarx::RobotUnitModule::Logging;
const std::size_t initialBufferSize;
const std::size_t initialBufferEntryNumbers;
@@ -197,6 +178,11 @@ namespace armarx
namespace armarx
{
+ namespace RobotUnitModule
+ {
+ class Logging;
+ }
+
struct ControlThreadOutputBuffer
{
using Entry = detail::ControlThreadOutputBufferEntry;
@@ -233,14 +219,13 @@ namespace armarx
RtMessageLogEntryBase* addMessageToLog(Ts&& ...args);
private:
- friend class RobotUnit;
+ friend class RobotUnitModule::Logging; ///TODO change code to make this unnecessary
/// @brief this pointer is used for rt message logging and is not null in the control thread
static thread_local ControlThreadOutputBuffer* RtLoggingInstance;
std::size_t toBounds(std::size_t idx) const;
//settings and initialization:
- bool useTripleBuffer {false};
bool isInitialized {false};
std::size_t numEntries {0};
@@ -248,7 +233,6 @@ namespace armarx
mutable std::atomic_size_t onePastLoggingReadPosition {0};
mutable std::atomic_size_t onePastReadPosition {0};
- TripleBuffer<Entry*> tripleBuffer;
std::vector<Entry> entries;
std::vector<std::uint8_t> data;
@@ -270,12 +254,7 @@ namespace armarx
{ \
using TupleT = decltype(std::make_tuple(__VA_ARGS__)); \
const TupleT tuple; \
- RtMessageLogEntryBase* _placementCopyConstruct(void* place) const final override \
- { \
- return new(place) RtMessageLogEntry(*this); \
- } \
- std::size_t _sizeInBytes() const final override {return sizeof(RtMessageLogEntry);} \
- std::size_t _alignof() const final override {return alignof(RtMessageLogEntry);} \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
std::size_t line() const final override {return line_;} \
std::string file() const final override {return file_;} \
std::string func() const final override {return func_;} \
@@ -314,21 +293,6 @@ namespace armarx
throw std::logic_error {"called 'format' of RtMessageLogEntryDummy"};
}
- inline std::size_t RtMessageLogEntryDummy::_sizeInBytes() const
- {
- throw std::logic_error {"called '_sizeInBytes' of RtMessageLogEntryDummy"};
- }
-
- inline std::size_t RtMessageLogEntryDummy::_alignof() const
- {
- throw std::logic_error {"called '_alignof' of RtMessageLogEntryDummy"};
- }
-
- inline RtMessageLogEntryBase* RtMessageLogEntryDummy::_placementCopyConstruct(void*) const
- {
- throw std::logic_error {"called '_placementCopyConstruct' of RtMessageLogEntryDummy"};
- }
-
inline std::size_t RtMessageLogEntryDummy::line() const
{
throw std::logic_error {"called 'line' of RtMessageLogEntryDummy"};
@@ -400,10 +364,6 @@ namespace armarx
template<class LoggingEntryT, class...Ts>
inline ControlThreadOutputBuffer::RtMessageLogEntryBase* ControlThreadOutputBuffer::addMessageToLog(Ts&& ...args)
{
- if (useTripleBuffer)
- {
- return &detail::RtMessageLogEntryDummy::Instance;
- }
detail::RtMessageLogBuffer& messages = getWriteBuffer().messages;
if (messages.entries.size() <= messages.entriesWritten)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
new file mode 100644
index 0000000000000000000000000000000000000000..6d2118d46b7e8a0ec40728d471fd9c15248fd128
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
@@ -0,0 +1,398 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
+#include <ArmarXCore/core/util/PropagateConst.h>
+
+#include "HeterogenousContinuousContainerMacros.h"
+
+#if __GNUC__< 5
+namespace std
+{
+ inline void* align(size_t alignment, size_t bytes, void*& bufferPlace, size_t& bufferSpace) noexcept
+ {
+ const auto uiptr = reinterpret_cast<uintptr_t>(bufferPlace);
+ const auto alignedPlace = (uiptr - 1u + alignment) & -alignment;
+ const auto spaceRequired = alignedPlace - uiptr;
+ if ((bytes + spaceRequired) > bufferSpace)
+ {
+ return nullptr;
+ }
+ else
+ {
+ bufferSpace -= spaceRequired;
+ return bufferPlace = reinterpret_cast<void*>(alignedPlace);
+ }
+ }
+}
+#endif
+
+namespace armarx
+{
+ namespace detail
+ {
+ template<class Base>
+ class HeterogenousContinuousContainerBase
+ {
+ public:
+ bool empty() const
+ {
+ return begin_ == current_;
+ }
+ bool owning() const
+ {
+ return storage_;
+ }
+ 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)
+ {
+ 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();
+
+ 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()
+ {
+ current_ = begin_;
+ }
+ private:
+ std::unique_ptr<std::uint8_t[]> storage_{nullptr};
+ void* begin_{nullptr};
+ void* current_{nullptr};
+ void* end_{nullptr};
+ };
+ }
+
+ template<class Base, bool UsePropagateConst = true>
+ class HeterogenousContinuousContainer : public detail::HeterogenousContinuousContainerBase<Base>
+ {
+ using BaseContainer = detail::HeterogenousContinuousContainerBase<Base>;
+ public:
+ using ElementType = typename std::conditional<
+ UsePropagateConst,
+ PropagateConst<Base*>,
+ Base*
+ >::type;
+
+ HeterogenousContinuousContainer() = default;
+ HeterogenousContinuousContainer(HeterogenousContinuousContainer&&) = default;
+ HeterogenousContinuousContainer(const HeterogenousContinuousContainer& other, bool compressElems = false)
+ {
+ this->setStorageCapacity(compressElems ? other.getUsedStorageCapacity() : other.getStorageCapacity());
+ setElementCapacity(compressElems ? other.elements().size() : other.elements().capacity());
+ for(auto& e : other.elements())
+ {
+ pushBack(e);
+ }
+ }
+
+ HeterogenousContinuousContainer& operator=(HeterogenousContinuousContainer&&) = default;
+ HeterogenousContinuousContainer& operator=(const HeterogenousContinuousContainer& other)
+ {
+ clear();
+ setStorageCapacity(other.getStorageCapacity());
+ setElementCapacity(other.elements().capacity());
+ for(auto& e : other.elements())
+ {
+ pushBack(e);
+ }
+ return *this;
+ }
+
+ ~HeterogenousContinuousContainer()
+ {
+ clear();
+ }
+
+ std::size_t getElementCount() const
+ {
+ return elements_.size();
+ }
+ std::size_t getElementCapacity() const
+ {
+ return elements_.capacity();
+ }
+ std::size_t getRemainingElementCapacity() const
+ {
+ return getElementCapacity() - getElementCount();
+ }
+
+ void setElementCapacity(std::size_t cnt)
+ {
+ ARMARX_CHECK_EXPRESSION(this->empty());
+ if(elements_.capacity() > cnt)
+ {
+ //force the capacity to reduce
+ elements_ = std::vector<ElementType>{};
+ }
+ elements_.reserve(cnt);
+ }
+
+ template<class Derived>
+ Base* pushBack(const Derived* d)
+ {
+ static_assert(
+ std::is_base_of<Base, Derived>::value,
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
+ );
+ ARMARX_CHECK_NOT_NULL(d);
+ if(!getRemainingElementCapacity())
+ {
+ return nullptr;
+ }
+ Base* const ptr = BaseContainer::pushBack(d);
+ if(ptr)
+ {
+ elements_.emplace_back(ptr);
+ }
+ return ptr;
+ }
+ template<class Derived>
+ Base* pushBack(const PropagateConst<Derived*>& d)
+ {
+ return pushBack(*d);
+ }
+ template<class Derived>
+ Base* pushBack(const Derived& d)
+ {
+ return pushBack(&d);
+ }
+
+ std::vector<ElementType>& elements()
+ {
+ return elements_;
+ }
+ const std::vector<ElementType>& elements() const
+ {
+ return elements_;
+ }
+
+ void clear()
+ {
+ for(auto& e : elements_)
+ {
+ e->~Base();
+ }
+ elements_.clear();
+ BaseContainer::clear();
+ }
+ private:
+ std::vector<ElementType> elements_;
+ };
+
+ template<class Base, bool UsePropagateConst = true>
+ class HeterogenousContinuous2DContainer : public detail::HeterogenousContinuousContainerBase<Base>
+ {
+ using BaseContainer = detail::HeterogenousContinuousContainerBase<Base>;
+ public:
+ using ElementType = typename std::conditional<
+ UsePropagateConst,
+ PropagateConst<Base*>,
+ Base*
+ >::type;
+
+ HeterogenousContinuous2DContainer() = default;
+ HeterogenousContinuous2DContainer(HeterogenousContinuous2DContainer&&) = default;
+ HeterogenousContinuous2DContainer(const HeterogenousContinuous2DContainer& other, bool compressElems = false)
+ {
+ setStorageCapacity(compressElems ? other.getUsedStorageCapacity() : other.getStorageCapacity());
+ std::vector<std::size_t> elemCaps;
+ elemCaps.reserve(other.elements().size());
+ for(const auto& d1 : other.elements())
+ {
+ elemCaps.emplace_back(compressElems ? d1.size() : d1.capacity());
+ }
+ setElementCapacity(elemCaps);
+ for(std::size_t i = 0; i < other.elements().size(); ++i)
+ {
+ for(auto& e : other.elements().at(i))
+ {
+ pushBack(i,e);
+ }
+ }
+ }
+
+ HeterogenousContinuous2DContainer& operator=(HeterogenousContinuous2DContainer&&) = default;
+ HeterogenousContinuous2DContainer& operator=(const HeterogenousContinuous2DContainer& other)
+ {
+ clear();
+ setStorageCapacity(other.getStorageCapacity());
+ std::vector<std::size_t> elemCaps;
+ elemCaps.reserve(other.elements().size());
+ for(const auto& d1 : other.elements())
+ {
+ elemCaps.emplace_back(d1.capacity());
+ }
+ setElementCapacity(elemCaps);
+ for(std::size_t i = 0; i < other.elements().size(); ++i)
+ {
+ for(auto& e : other.elements().at(i))
+ {
+ pushBack(i,e);
+ }
+ }
+ return *this;
+ }
+
+ ~HeterogenousContinuous2DContainer()
+ {
+ clear();
+ }
+
+ std::size_t getElementCount(std::size_t d0) const
+ {
+ return elements_.at(d0).size();
+ }
+ std::size_t getElementCapacity(std::size_t d0) const
+ {
+ return elements_.at(d0).capacity();
+ }
+ std::size_t getRemainingElementCapacity(std::size_t d0) const
+ {
+ return getElementCapacity(d0) - getElementCount(d0);
+ }
+
+ void setElementCapacity(const std::vector<std::size_t>& cnt)
+ {
+ ARMARX_CHECK_EXPRESSION(this->empty());
+ elements_.resize(cnt.size());
+ for(std::size_t i = 0; i < cnt.size(); ++i)
+ {
+ if(elements_.at(i).capacity() > cnt)
+ {
+ //force the capacity to reduce
+ elements_.at(i) = std::vector<ElementType>{};
+ }
+ elements_.at(i).reserve(cnt.at(i));
+ }
+ }
+
+ template<class Derived>
+ Base* pushBack(std::size_t d0, const Derived* d)
+ {
+ static_assert(
+ std::is_base_of<Base, Derived>::value,
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
+ );
+ ARMARX_CHECK_NOT_NULL(d);
+ if(!getRemainingElementCapacity(d0))
+ {
+ return nullptr;
+ }
+ Base* const ptr = BaseContainer::pushBack(d);
+ if(ptr)
+ {
+ elements_.at(d0).emplace_back(ptr);
+ }
+ return ptr;
+ }
+ template<class Derived>
+ Base* pushBack(std::size_t d0, const PropagateConst<Derived*>& d)
+ {
+ return pushBack(d0, *d);
+ }
+ template<class Derived>
+ Base* pushBack(std::size_t d0, const Derived& d)
+ {
+ return pushBack(d0, &d);
+ }
+
+ std::vector<std::vector<ElementType>>& elements()
+ {
+ return elements_;
+ }
+ const std::vector<std::vector<ElementType>>& elements() const
+ {
+ return elements_;
+ }
+
+ void clear()
+ {
+ for(auto& d1 : elements_)
+ {
+ for(auto& e : d1)
+ {
+ e->~Base();
+ }
+ d1.clear();
+ }
+ BaseContainer::clear();
+ }
+ private:
+ std::vector<std::vector<ElementType>> elements_;
+ };
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h
new file mode 100644
index 0000000000000000000000000000000000000000..8178062c2939d175e37de8285ee9a9c365bfd568
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h
@@ -0,0 +1,88 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::RobotUnit
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+#include <type_traits>
+
+#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ using _typeBase = CommonBaseType; \
+ virtual std::size_t _alignof() const = 0; \
+ virtual std::size_t _sizeof() const = 0; \
+ virtual _typeBase* _placementCopyConstruct(void* place) const = 0; \
+ std::size_t _accumulateSize(std::size_t offset = 0) const \
+ { \
+ return offset + \
+ (_alignof() - (offset % _alignof())) % _alignof() + \
+ _sizeof(); \
+ }
+
+#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ virtual void _copyTo(_typeBase* target) const = 0; \
+ virtual std::unique_ptr<_typeBase> _clone() const = 0; \
+ virtual _typeBase* _placementConstruct(void* place) const = 0;
+
+#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
+ std::size_t _alignof() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return alignof(_type); \
+ } \
+ std::size_t _sizeof() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return sizeof(_type); \
+ } \
+ _typeBase* _placementCopyConstruct(void* place) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return new(place) _type(*this); \
+ } \
+ void _checkBaseType() \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ static_assert( \
+ std::is_base_of<_typeBase, _type>::value, \
+ "This class has to derive the common base class"); \
+ }
+
+#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
+ void _copyTo(_typeBase* target) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ _type* const castedTarget = dynamic_cast<_type*>(target); \
+ ARMARX_CHECK_NOT_NULL(castedTarget); \
+ *castedTarget = *this; \
+ } \
+ std::unique_ptr<_typeBase> _clone() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return std::unique_ptr<_typeBase>{new _type(*this)}; \
+ } \
+ _typeBase* _placementConstruct(void* place) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return new(place) _type; \
+ }
diff --git a/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h b/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
index 37193e2728bb695eea96d2678c01f402fc7681d3..faee01fc2c8f714adef111b3ef3d483d54079e81 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
@@ -19,7 +19,6 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
#pragma once
#include <vector>
diff --git a/source/RobotAPI/components/units/RobotUnit/util/Time.h b/source/RobotAPI/components/units/RobotUnit/util/Time.h
index 832ca38dd001f3ad890897adf95bf67d0b17f462..919ea91a7bfb378487614b8997dd044efa72e395 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/Time.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/Time.h
@@ -21,6 +21,7 @@
*/
#pragma once
+
#include <chrono>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index fc17efbbb5d3fb443410c50cf4b4a0473b7a5362..860e627af4d100e1a2fda8bb0f61afb25d725245 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -19,8 +19,8 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
#pragma once
+
#include <ArmarXCore/core/util/algorithm.h>
#include "ClassMemberInfoEntry.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
index c682df829d3ecc30bad2e4793f989914ab488953..9c1de849e80ba5853b28d45c99a08b0ed33fc00d 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
@@ -19,7 +19,6 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
#pragma once
#include "DataFieldsInfo.h"
@@ -236,4 +235,3 @@ namespace armarx
};
}
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index 8d902d9737de36042bf0eb4ccda80096f21d9120..15582f2ecc192c543211adfa25f0d19c859dfbd2 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -19,8 +19,8 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
#pragma once
+
#include "../EigenForwardDeclarations.h"
#include <string>
#include <chrono>
@@ -248,4 +248,3 @@ namespace armarx
};
}
}
-
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
index 0d7911871493b5d79b86678631914ffd9f17da2b..0c4d6e2d110b42a09c4a4e934ed5d3828e33416c 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
@@ -108,7 +108,7 @@ void RobotUnitPluginWidgetController::onInitComponent()
void RobotUnitPluginWidgetController::onConnectComponent()
{
robotUnitPrx = getProxy<RobotUnitInterfacePrx>(robotUnitProxyName);
- listenerTopicName = robotUnitPrx->getListenerTopicName();
+ listenerTopicName = robotUnitPrx->getRobotUnitListenerTopicName();
usingTopic(listenerTopicName);
updateToolBarActionCheckedState();
QMetaObject::invokeMethod(this, "startOnConnectTimer", Qt::QueuedConnection);
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index 49a8e8a19a8a1c83b72c96001a0e0dbdc8d3f520..f5906a1fc6a4ec9cd55d8a2819aab032782373a4 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -25,7 +25,8 @@
#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/core/RemoteReferenceCount.ice>
-
+#include <ArmarXCore/interface/components/EmergencyStopInterface.ice>
+#include <ArmarXCore/interface/observers/ObserverInterface.ice>
#include <ArmarXGui/interface/WidgetDescription.ice>
@@ -35,6 +36,10 @@
#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
#include <RobotAPI/interface/units/InertialMeasurementUnit.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+#include <RobotAPI/interface/units/TCPControlUnit.ice>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.ice>
+
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.ice>
module armarx
{
@@ -90,84 +95,6 @@ module armarx
};
sequence<NJointControllerClassDescription> NJointControllerClassDescriptionSeq;
- interface RobotUnitInterface
- {
- //controllers
- //names
- ["cpp:const"] idempotent Ice::StringSeq getNJointControllerNames();
- ["cpp:const"] idempotent Ice::StringSeq getRequestedNJointControllerNames();
- ["cpp:const"] idempotent Ice::StringSeq getActivatedNJointControllerNames();
- //proxy/information
- ["cpp:const"] idempotent NJointControllerInterface* getNJointController(string name);
- ["cpp:const"] idempotent StringNJointControllerPrxDictionary getAllNJointControllers();
-
- ["cpp:const"] idempotent NJointControllerStatus getNJointControllerStatus(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerStatusSeq getNJointControllerStatuses();
-
- ["cpp:const"] idempotent NJointControllerDescription getNJointControllerDescription(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerDescriptionSeq getNJointControllerDescriptions();
-
- ["cpp:const"] idempotent NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses();
-
- //devices
- ["cpp:const"] idempotent Ice::StringSeq getControlDeviceNames() throws LogicError;
- ["cpp:const"] idempotent ControlDeviceDescription getControlDeviceDescription(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent ControlDeviceDescriptionSeq getControlDeviceDescriptions() throws LogicError;
- ["cpp:const"] idempotent ControlDeviceStatus getControlDeviceStatus(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent ControlDeviceStatusSeq getControlDeviceStatuses() throws LogicError;
-
- ["cpp:const"] idempotent Ice::StringSeq getSensorDeviceNames() throws LogicError;
- ["cpp:const"] idempotent SensorDeviceDescription getSensorDeviceDescription(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent SensorDeviceDescriptionSeq getSensorDeviceDescriptions() throws LogicError;
- ["cpp:const"] idempotent SensorDeviceStatus getSensorDeviceStatus(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent SensorDeviceStatusSeq getSensorDeviceStatuses() throws LogicError;
- //classes
- ["cpp:const"] idempotent Ice::StringSeq getNJointControllerClassNames();
- ["cpp:const"] idempotent NJointControllerClassDescription getNJointControllerClassDescription(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions();
-
- //switching
- void switchNJointControllerSetup(Ice::StringSeq newSetup) throws InvalidArgumentException, LogicError;
-
- void activateNJointController(string controllerInstanceName) throws InvalidArgumentException, LogicError;
- void activateNJointControllers(Ice::StringSeq controllerInstanceNames) throws InvalidArgumentException, LogicError;
- void deactivateNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
- void deactivateNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
-
- //creting controllers
- NJointControllerInterface* createNJointController(string className, string instanceName, NJointControllerConfig config) throws InvalidArgumentException, LogicError;
- NJointControllerInterface* createNJointControllerFromVariantConfig(string className, string instanceName, StringVariantBaseMap config) throws InvalidArgumentException, LogicError;
- //deleting controllers
- void deleteNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
- void deleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
- //loading
- bool loadLibFromPath(string path);
- bool loadLibFromPackage(string package, string libname);
-
- //units
- ["cpp:const"] idempotent Object* getUnit(string staticIceId);
- ["cpp:const"] idempotent Ice::ObjectProxySeq getUnits();
- ["cpp:const"] idempotent KinematicUnitInterface* getKinematicUnit();
- ["cpp:const"] idempotent ForceTorqueUnitInterface* getForceTorqueUnit();
- ["cpp:const"] idempotent InertialMeasurementUnitInterface* getInertialMeasurementUnit();
- ["cpp:const"] idempotent PlatformUnitInterface* getPlatformUnit();
-
- //other
- ["cpp:const"] idempotent string getListenerTopicName();
- ["cpp:const"] idempotent bool isRunning();
-
- //rt-logging
- ["cpp:const"] idempotent bool isRtLoggingEnabled();
- ["cpp:const"] idempotent Ice::StringSeq getLoggingNames();
- RemoteReferenceCounterBase startRtLogging(string filePathFormatString, Ice::StringSeq loggingNames) throws LogicError, InvalidArgumentException;
- RemoteReferenceCounterBase startRtLoggingWithAliasNames(string filePathFormatString, StringStringDictionary aliasNames) throws LogicError, InvalidArgumentException;
- void addMarkerToRtLog(RemoteReferenceCounterBase token, string marker) throws LogicError;
- void stopRtLogging(RemoteReferenceCounterBase token) throws LogicError;
-
- ["cpp:const"] void writeRecentIterationsToFile(string filePathFormatString) throws LogicError, InvalidArgumentException;
- };
-
interface RobotUnitListener
{
void nJointControllerStatusChanged(NJointControllerStatus status);
@@ -177,5 +104,132 @@ module armarx
void nJointControllerCreated(string instanceName);
void nJointControllerDeleted(string instanceName);
};
+
+ module RobotUnitModule
+ {
+ interface RobotUnitManagementInterface
+ {
+ //state
+ ["cpp:const"] idempotent bool isRunning();
+ };
+ interface RobotUnitLoggingInterface
+ {
+ //rt-logging
+ ["cpp:const"] idempotent Ice::StringSeq getLoggingNames();
+ RemoteReferenceCounterBase startRtLogging(string filePathFormatString, Ice::StringSeq loggingNames) throws LogicError, InvalidArgumentException;
+ RemoteReferenceCounterBase startRtLoggingWithAliasNames(string filePathFormatString, StringStringDictionary aliasNames) throws LogicError, InvalidArgumentException;
+ void addMarkerToRtLog(RemoteReferenceCounterBase token, string marker) throws LogicError;
+ void stopRtLogging(RemoteReferenceCounterBase token) throws LogicError;
+
+ ["cpp:const"] void writeRecentIterationsToFile(string filePathFormatString) throws LogicError, InvalidArgumentException;
+ };
+ interface RobotUnitUnitInterface
+ {
+ ["cpp:const"] idempotent Object* getUnit(string staticIceId);
+ ["cpp:const"] idempotent Ice::ObjectProxySeq getUnits();
+ ["cpp:const"] idempotent KinematicUnitInterface* getKinematicUnit();
+ ["cpp:const"] idempotent ForceTorqueUnitInterface* getForceTorqueUnit();
+ ["cpp:const"] idempotent InertialMeasurementUnitInterface* getInertialMeasurementUnit();
+ ["cpp:const"] idempotent PlatformUnitInterface* getPlatformUnit();
+ ["cpp:const"] idempotent TCPControlUnitInterface* getTCPControlUnit();
+ ["cpp:const"] idempotent TrajectoryPlayerInterface* getTrajectoryPlayer();
+
+
+
+ };
+ interface RobotUnitPublishingInterface
+ {
+ ["cpp:const"] idempotent string getRobotUnitListenerTopicName();
+ ["cpp:const"] idempotent string getDebugDrawerTopicName();
+ ["cpp:const"] idempotent string getDebugObserverTopicName();
+
+ ["cpp:const"] idempotent RobotUnitListener* getRobotUnitListenerProxy();
+ ["cpp:const"] idempotent DebugDrawerInterface* getDebugDrawerProxy();
+ ["cpp:const"] idempotent DebugObserverInterface* getDebugObserverProxy();
+ };
+ interface RobotUnitDevicesInterface
+ {
+ //devices
+ ["cpp:const"] idempotent Ice::StringSeq getControlDeviceNames() throws LogicError;
+ ["cpp:const"] idempotent ControlDeviceDescription getControlDeviceDescription(string name) throws InvalidArgumentException, LogicError;
+ ["cpp:const"] idempotent ControlDeviceDescriptionSeq getControlDeviceDescriptions() throws LogicError;
+ ["cpp:const"] idempotent ControlDeviceStatus getControlDeviceStatus(string name) throws InvalidArgumentException, LogicError;
+ ["cpp:const"] idempotent ControlDeviceStatusSeq getControlDeviceStatuses() throws LogicError;
+
+ ["cpp:const"] idempotent Ice::StringSeq getSensorDeviceNames() throws LogicError;
+ ["cpp:const"] idempotent SensorDeviceDescription getSensorDeviceDescription(string name) throws InvalidArgumentException, LogicError;
+ ["cpp:const"] idempotent SensorDeviceDescriptionSeq getSensorDeviceDescriptions() throws LogicError;
+ ["cpp:const"] idempotent SensorDeviceStatus getSensorDeviceStatus(string name) throws InvalidArgumentException, LogicError;
+ ["cpp:const"] idempotent SensorDeviceStatusSeq getSensorDeviceStatuses() throws LogicError;
+ };
+ interface RobotUnitControllerManagementInterface
+ {
+ //names
+ ["cpp:const"] idempotent Ice::StringSeq getNJointControllerNames();
+ ["cpp:const"] idempotent Ice::StringSeq getRequestedNJointControllerNames();
+ ["cpp:const"] idempotent Ice::StringSeq getActivatedNJointControllerNames();
+ //proxy/information
+ ["cpp:const"] idempotent NJointControllerInterface* getNJointController(string name);
+ ["cpp:const"] idempotent StringNJointControllerPrxDictionary getAllNJointControllers();
+
+ ["cpp:const"] idempotent NJointControllerStatus getNJointControllerStatus(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerStatusSeq getNJointControllerStatuses();
+
+ ["cpp:const"] idempotent NJointControllerDescription getNJointControllerDescription(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerDescriptionSeq getNJointControllerDescriptions();
+
+ ["cpp:const"] idempotent NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses();
+
+ //classes
+ ["cpp:const"] idempotent Ice::StringSeq getNJointControllerClassNames();
+ ["cpp:const"] idempotent NJointControllerClassDescription getNJointControllerClassDescription(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions();
+
+ //switching
+ void switchNJointControllerSetup(Ice::StringSeq newSetup) throws InvalidArgumentException, LogicError;
+
+ void activateNJointController(string controllerInstanceName) throws InvalidArgumentException, LogicError;
+ void activateNJointControllers(Ice::StringSeq controllerInstanceNames) throws InvalidArgumentException, LogicError;
+ void deactivateNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
+ void deactivateNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
+ //creting controllers
+ NJointControllerInterface* createNJointController(string className, string instanceName, NJointControllerConfig config) throws InvalidArgumentException, LogicError;
+ NJointControllerInterface* createNJointControllerFromVariantConfig(string className, string instanceName, StringVariantBaseMap config) throws InvalidArgumentException, LogicError;
+ //deleting controllers
+ void deleteNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
+ void deleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
+
+ //loading libs
+ bool loadLibFromPath(string path);
+ bool loadLibFromPackage(string package, string libname);
+ };
+ interface RobotUnitSelfCollisionCheckerInterface
+ {
+ ["cpp:const"] idempotent bool isSelfCollisionCheckEnabled();
+ ["cpp:const"] idempotent float getSelfCollisionAvoidanceFrequency();
+ ["cpp:const"] idempotent float getSelfCollisionAvoidanceDistance();
+ idempotent void setSelfCollisionAvoidanceFrequency(float freq);
+ idempotent void setSelfCollisionAvoidanceDistance(float dist);
+ };
+ interface RobotUnitControlThreadInterface
+ {
+ idempotent void setEmergencyStopState(EmergencyStopState state);
+ ["cpp:const"] idempotent EmergencyStopState getEmergencyStopState();
+ ["cpp:const"] idempotent EmergencyStopState getRtEmergencyStopState();
+ };
+ };
+
+ interface RobotUnitInterface extends
+ RobotUnitModule::RobotUnitUnitInterface,
+ RobotUnitModule::RobotUnitDevicesInterface,
+ RobotUnitModule::RobotUnitLoggingInterface,
+ RobotUnitModule::RobotUnitPublishingInterface,
+ RobotUnitModule::RobotUnitManagementInterface,
+ RobotUnitModule::RobotUnitControlThreadInterface,
+ RobotUnitModule::RobotUnitControllerManagementInterface,
+ RobotUnitModule::RobotUnitSelfCollisionCheckerInterface
+ {
+ };
};
diff --git a/source/RobotAPI/interface/units/TCPControlUnit.ice b/source/RobotAPI/interface/units/TCPControlUnit.ice
index 4e395feea18ae927ce31752a31f64f6e96b7558f..36d0bdf75b3ddde0c8b5a7535a785ed31810aca2 100644
--- a/source/RobotAPI/interface/units/TCPControlUnit.ice
+++ b/source/RobotAPI/interface/units/TCPControlUnit.ice
@@ -92,4 +92,3 @@ module armarx
};
};
-
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
index 1d2c433b93afb51f8d5b3aba5788440fb6c7f0a9..5b48d4fe03f9d108934a8a5854d23f234d75f4c4 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/CMakeLists.txt
@@ -7,7 +7,8 @@ find_package(Eigen3 QUIET)
find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
find_package(DMP QUIET)
find_package(MMMCore QUIET)
-find_package(MMMToolsQUIET)
+find_package(MMMTools QUIET)
+find_package(IVT QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
index c00253ed7a2524d5e6d8a591754f458ef2649168..46cda82c25f0d6d0b4db74b101c05a4ab5d44562 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp
@@ -4,21 +4,21 @@ namespace armarx
{
NJointControllerRegistration<NJointCCDMPController> registrationControllerNJointCCDMPController("NJointCCDMPController");
- NJointCCDMPController::NJointCCDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointCCDMPController::NJointCCDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
+ useSynchronizedRtRobot();
cfg = NJointCCDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(prov);
- RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- ARMARX_CHECK_EXPRESSION(robotUnit);
+ ARMARX_CHECK_NOT_NULL(cfg);
+ ARMARX_CHECK_GREATER_EQUAL(cfg->dmpNum, 0);
ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(cfg->nodeSetName);
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
@@ -36,7 +36,7 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);
};
- tcp = (cfg->tcpName.empty()) ? rns->getTCP() : robotUnit->getRtRobot()->getRobotNode(cfg->tcpName);
+ tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
// set tcp controller
@@ -168,20 +168,20 @@ namespace armarx
// run DMP one after another
Eigen::Matrix4f targetPose = Eigen::Matrix4f::Identity();
- bool finished = true;
- for (size_t i = 0; i < cfg->dmpNum; ++i)
+ //bool finished = true;
+ for (size_t i = 0; i < static_cast<size_t>(cfg->dmpNum); ++i)
{
double timeDuration = timeDurations[i];
std::string dmpType = dmpTypes[i];
- double amplitude = 1.0;
+ //double amplitude = 1.0;
if (dmpType == "Periodic")
{
if (canVals[i] <= 1e-8)
{
canVals[i] = timeDuration;
}
- amplitude = amplitudes[i];
+ //amplitude = amplitudes[i];
}
if (canVals[i] > 1e-8)
@@ -196,7 +196,7 @@ namespace armarx
(currentStates[i], canVals[i] / timeDurations[i], dmpDeltaT, targetSubStates[i]);
- finished = false;
+ //finished = false;
}
@@ -500,14 +500,14 @@ namespace armarx
void NJointCCDMPController::runDMP(const Ice::Current&)
{
+ const auto dmpNum = static_cast<std::size_t>(cfg->dmpNum);
finished = false;
-
- if (dmpTypes.size() != cfg->dmpNum ||
- dmpPtrList.size() != cfg->dmpNum ||
- learnedDMP.size() != cfg->dmpNum ||
- canVals.size() != cfg->dmpNum ||
- currentStates.size() != cfg->dmpNum ||
- targetSubStates.size() != cfg->dmpNum)
+ if (dmpNum != dmpTypes.size() ||
+ dmpNum != dmpPtrList.size() ||
+ dmpNum != learnedDMP.size() ||
+ dmpNum != canVals.size() ||
+ dmpNum != currentStates.size() ||
+ dmpNum != targetSubStates.size())
{
ARMARX_ERROR << "Error: cannot run CCDMP controller. The reason is that some parameters have different sizes";
return;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
index 7dc42afd1e8c99b9a5d269727cec270753abfc5c..e3917f63cf874b8b1b08f6922dd66f08f4da6821 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -54,7 +54,7 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCCDMPControllerConfigPtr;
- NJointCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCCDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
index bcf0d7893db6d82964c70cd456f85a2803e4f2b7..3bd5872a9f85027aa46b7b042a61f5d4d732fdb5 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp
@@ -12,15 +12,15 @@ namespace armarx
return "NJointJSDMPController";
}
- NJointJSDMPController::NJointJSDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointJSDMPController::NJointJSDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr");
for (std::string jointName : cfg->jointNames)
{
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>()));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index 625058be3fa1a6d22047392e6cd4dce582590afc..3b904713a90d8190b0691b2ce5d0250a5262e6ed 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -38,7 +38,7 @@ namespace armarx
{
public:
using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
- NJointJSDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointJSDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
index 0626c1976c4bfe8f6b5de2eed7ba0298006a65f2..ac8005eba8c70cdfce5aa45d80bd189ef06f15c5 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.cpp
@@ -12,15 +12,15 @@ namespace armarx
return "NJointJointSpaceDMPController";
}
- NJointJointSpaceDMPController::NJointJointSpaceDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointJointSpaceDMPController::NJointJointSpaceDMPController(armarx::RobotUnitPtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
NJointJointSpaceDMPControllerConfigPtr cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr");
for (std::string jointName : cfg->jointNames)
{
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
positionSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
torqueSensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorTorque>()));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
index 173c7cfb146f2117b2c6a8df498baffa2eb21477..a4097a0ab06c4ce7d084af4e267ffe2360dceb8c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
@@ -38,7 +38,7 @@ namespace armarx
{
public:
using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
- NJointJointSpaceDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointJointSpaceDMPController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
index 9dfb86763888c436e53384b6019d849582d9c966..2f95ef4b734ac289e4c84b6f11c4b27f426170b9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp
@@ -4,21 +4,19 @@ namespace armarx
{
NJointControllerRegistration<NJointTSDMPController> registrationControllerNJointTSDMPController("NJointTSDMPController");
- NJointTSDMPController::NJointTSDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointTSDMPController::NJointTSDMPController(const RobotUnitPtr&, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
+ useSynchronizedRtRobot();
cfg = NJointTaskSpaceDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(prov);
- RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- ARMARX_CHECK_EXPRESSION(robotUnit);
ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(cfg->nodeSetName);
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
@@ -36,7 +34,7 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);
};
- tcp = (cfg->tcpName.empty()) ? rns->getTCP() : robotUnit->getRtRobot()->getRobotNode(cfg->tcpName);
+ tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
// set tcp controller
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index 620b5c81145b5fb4be3755ad37bdf48c19ed5924..a072119a1b2fd8bf15ceb305f1e3397026474d10 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -54,7 +54,7 @@ namespace armarx
{
public:
using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
- NJointTSDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTSDMPController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
index f7a2de6fee95c7a188cd816158d88836c974e9c2..a210d04f27abcfafd3dcd73eef9a697ad77e02bb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.cpp
@@ -4,21 +4,19 @@ namespace armarx
{
NJointControllerRegistration<NJointTaskSpaceDMPController> registrationControllerNJointTaskSpaceDMPController("NJointTaskSpaceDMPController");
- NJointTaskSpaceDMPController::NJointTaskSpaceDMPController(armarx::NJointControllerDescriptionProviderInterfacePtr prov, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointTaskSpaceDMPController::NJointTaskSpaceDMPController(const RobotUnitPtr& robotUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
{
+ useSynchronizedRtRobot();
cfg = NJointTaskSpaceDMPControllerConfigPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(prov);
- RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
- ARMARX_CHECK_EXPRESSION(robotUnit);
ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
- VirtualRobot::RobotNodeSetPtr rns = robotUnit->getRtRobot()->getRobotNodeSet(cfg->nodeSetName);
+ VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName);
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
- ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
- const SensorValueBase* sv = prov->getSensorValue(jointName);
+ ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
@@ -36,7 +34,7 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);
};
- tcp = (cfg->tcpName.empty()) ? rns->getTCP() : robotUnit->getRtRobot()->getRobotNode(cfg->tcpName);
+ tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName);
// set tcp controller
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
index b5f3fa66e39c20002747e7e02372e7f45c5aed86..edcb07cffe543ca68bd7505c501bbd705954ca85 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceDMPController.h
@@ -57,7 +57,7 @@ namespace armarx
{
public:
using ConfigPtrT = NJointTaskSpaceDMPControllerConfigPtr;
- NJointTaskSpaceDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTaskSpaceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const;