diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
index 16a61f9af703665aa7d489ee115b43cef35edc27..4a147cf86218c9c5035ca27474472bca1bf23c54 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
@@ -23,11 +23,11 @@
#ifndef _ARMARX_LIB_RobotAPI_NJointController_HPP
#define _ARMARX_LIB_RobotAPI_NJointController_HPP
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
-#include "NJointController.h"
#include "../RobotUnit.h"
+#include "NJointController.h"
namespace armarx::RobotUnitModule
{
@@ -39,46 +39,55 @@ namespace armarx
{
private:
friend class RobotUnitModule::ControllerManagement;
- virtual NJointControllerBasePtr create(
- RobotUnitModule::ControllerManagement* cmngr,
- const NJointControllerConfigPtr&,
- const VirtualRobot::RobotPtr&,
- bool deletable,
- bool internal,
- const std::string& instanceName) const = 0;
- virtual WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr&, const std::map<std::string, ConstControlDevicePtr>&, const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
- virtual NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
+ virtual NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr&,
+ const VirtualRobot::RobotPtr&,
+ bool deletable,
+ bool internal,
+ const std::string& instanceName) const = 0;
+ virtual WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ 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()
+ virtual ~NJointControllerRegistryEntry() = default;
+ static bool
+ ConstructorIsRunning()
{
return ConstructorIsRunning_;
}
};
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
- template<class ControllerType>
+ template <class ControllerType>
struct NJointControllerRegistration;
template <typename ControlDataStruct>
- class NJointControllerWithTripleBuffer: public SynchronousNJointController
+ class NJointControllerWithTripleBuffer : public SynchronousNJointController
{
public:
using MutexType = std::recursive_mutex;
using LockGuardType = std::lock_guard<std::recursive_mutex>;
- NJointControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
- controlDataTripleBuffer {initialCommands}
+ NJointControllerWithTripleBuffer(
+ const ControlDataStruct& initialCommands = ControlDataStruct()) :
+ controlDataTripleBuffer{initialCommands}
{
}
- void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
protected:
const ControlDataStruct& rtGetControlStruct() const;
- bool rtUpdateControlStruct();
+ bool rtUpdateControlStruct();
void writeControlStruct();
ControlDataStruct& getWriterControlStruct();
@@ -88,138 +97,170 @@ namespace armarx
void reinitTripleBuffer(const ControlDataStruct& initial);
mutable MutexType controlDataMutex;
+
private:
WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
};
template <typename ControlDataStruct>
- using NJointControllerWithTripleBufferPtr = IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
-}
+ using NJointControllerWithTripleBufferPtr =
+ IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
+} // namespace armarx
//inline functions
namespace armarx
{
- template<class T>
- inline T* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode)
+ template <class T>
+ inline T*
+ NJointControllerBase::useControlTarget(const std::string& deviceName,
+ const std::string& controlMode)
{
- static_assert(std::is_base_of<ControlTargetBase, T>::value, "The given type does not derive ControlTargetBase");
+ static_assert(std::is_base_of<ControlTargetBase, T>::value,
+ "The given type does not derive ControlTargetBase");
ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode);
return ptr ? ptr->asA<T>() : nullptr;
}
- template<class T>
- inline const T* NJointControllerBase::useSensorValue(const std::string& deviceName) const
+ template <class T>
+ inline const T*
+ NJointControllerBase::useSensorValue(const std::string& deviceName) const
{
- static_assert(std::is_base_of<SensorValueBase, T>::value, "The given type does not derive SensorValueBase");
+ static_assert(std::is_base_of<SensorValueBase, T>::value,
+ "The given type does not derive SensorValueBase");
const SensorValueBase* const ptr = useSensorValue(deviceName);
return ptr ? ptr->asA<T>() : nullptr;
}
- inline void SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ inline void
+ SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
- inline bool NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
+ inline bool
+ NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
{
return controlDeviceUsedBitmap.at(deviceIndex);
}
- inline StringStringDictionary NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
+ inline StringStringDictionary
+ NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
{
return controlDeviceControlModeMap;
}
- inline const std::vector<char>& NJointControllerBase::getControlDeviceUsedBitmap() const
+ inline const std::vector<char>&
+ NJointControllerBase::getControlDeviceUsedBitmap() const
{
return controlDeviceUsedBitmap;
}
- inline const std::vector<std::size_t>& NJointControllerBase::rtGetControlDeviceUsedIndices() const
+ inline const std::vector<std::size_t>&
+ NJointControllerBase::rtGetControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
- inline const std::vector<std::size_t>& NJointControllerBase::getControlDeviceUsedIndices() const
+ inline const std::vector<std::size_t>&
+ NJointControllerBase::getControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
- inline const std::map<std::string, const JointController*>& NJointControllerBase::getControlDevicesUsedJointController()
+ inline const std::map<std::string, const JointController*>&
+ NJointControllerBase::getControlDevicesUsedJointController()
{
return controlDeviceUsedJointController;
}
- inline std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
+ inline std::optional<std::vector<char>>
+ NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
{
return isNotInConflictWith(other->getControlDeviceUsedBitmap());
}
- inline std::string NJointControllerBase::getDefaultName() const
+ inline std::string
+ NJointControllerBase::getDefaultName() const
{
return getClassName();
}
- inline bool NJointControllerBase::isControllerActive(const Ice::Current&) const
+ inline bool
+ NJointControllerBase::isControllerActive(const Ice::Current&) const
{
return isActive;
}
- inline bool NJointControllerBase::isControllerRequested(const Ice::Current&) const
+ inline bool
+ NJointControllerBase::isControllerRequested(const Ice::Current&) const
{
return isRequested;
}
- inline bool NJointControllerBase::isDeletable(const Ice::Current&) const
+ inline bool
+ NJointControllerBase::isDeletable(const Ice::Current&) const
{
return deletable;
}
- inline bool NJointControllerBase::hasControllerError(const Ice::Current&) const
+ inline bool
+ NJointControllerBase::hasControllerError(const Ice::Current&) const
{
return deactivatedBecauseOfError;
}
- inline std::string NJointControllerBase::getInstanceName(const Ice::Current&) const
+ inline std::string
+ NJointControllerBase::getInstanceName(const Ice::Current&) const
{
return instanceName_;
}
- inline WidgetDescription::StringWidgetDictionary NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
+ inline WidgetDescription::StringWidgetDictionary
+ NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
{
return {};
}
- inline void NJointControllerBase::callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current&)
+ inline void
+ NJointControllerBase::callDescribedFunction(const std::string&,
+ const StringVariantBaseMap&,
+ const Ice::Current&)
{
}
- inline void NJointControllerBase::rtSetErrorState()
+ inline void
+ NJointControllerBase::rtSetErrorState()
{
errorState.store(true);
}
- inline bool NJointControllerBase::rtGetErrorState() const
+ inline bool
+ NJointControllerBase::rtGetErrorState() const
{
return errorState;
}
- inline std::size_t NJointControllerBase::rtGetNumberOfUsedControlDevices() const
+ inline std::size_t
+ NJointControllerBase::rtGetNumberOfUsedControlDevices() const
{
return controlDeviceUsedIndices.size();
}
- inline const std::string& NJointControllerBase::rtGetClassName() const
+ inline const std::string&
+ NJointControllerBase::rtGetClassName() const
{
return rtClassName_;
}
- inline const std::string& NJointControllerBase::rtGetInstanceName() const
+ inline const std::string&
+ NJointControllerBase::rtGetInstanceName() const
{
return instanceName_;
}
//NJointControllerWithTripleBuffer<ControlDataStruct>
template <typename ControlDataStruct>
- inline void NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
+ inline void
+ NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
@@ -228,19 +269,22 @@ namespace armarx
}
template <typename ControlDataStruct>
- inline const ControlDataStruct& NJointControllerWithTripleBuffer<ControlDataStruct>::rtGetControlStruct() const
+ inline const ControlDataStruct&
+ NJointControllerWithTripleBuffer<ControlDataStruct>::rtGetControlStruct() const
{
return controlDataTripleBuffer.getReadBuffer();
}
template <typename ControlDataStruct>
- inline bool NJointControllerWithTripleBuffer<ControlDataStruct>::rtUpdateControlStruct()
+ inline bool
+ NJointControllerWithTripleBuffer<ControlDataStruct>::rtUpdateControlStruct()
{
return controlDataTripleBuffer.updateReadBuffer();
}
template <typename ControlDataStruct>
- inline void NJointControllerWithTripleBuffer<ControlDataStruct>::writeControlStruct()
+ inline void
+ NJointControllerWithTripleBuffer<ControlDataStruct>::writeControlStruct()
{
//just lock to be save and reduce the impact of an error
//also this allows code to unlock the mutex before calling this function
@@ -250,34 +294,40 @@ namespace armarx
}
template <typename ControlDataStruct>
- inline ControlDataStruct& NJointControllerWithTripleBuffer<ControlDataStruct>::getWriterControlStruct()
+ inline ControlDataStruct&
+ NJointControllerWithTripleBuffer<ControlDataStruct>::getWriterControlStruct()
{
return controlDataTripleBuffer.getWriteBuffer();
}
template <typename ControlDataStruct>
- inline void NJointControllerWithTripleBuffer<ControlDataStruct>::setControlStruct(const ControlDataStruct& newStruct)
+ inline void
+ NJointControllerWithTripleBuffer<ControlDataStruct>::setControlStruct(
+ const ControlDataStruct& newStruct)
{
- LockGuardType lock {controlDataMutex};
+ LockGuardType lock{controlDataMutex};
getWriterControlStruct() = newStruct;
writeControlStruct();
}
template <typename ControlDataStruct>
- inline void NJointControllerWithTripleBuffer<ControlDataStruct>::reinitTripleBuffer(const ControlDataStruct& initial)
+ inline void
+ NJointControllerWithTripleBuffer<ControlDataStruct>::reinitTripleBuffer(
+ const ControlDataStruct& initial)
{
controlDataTripleBuffer.reinitAllBuffers(initial);
}
-}
+} // namespace armarx
namespace armarx
{
- template<class ItT>
- inline std::optional<std::vector<char>> NJointControllerBase::AreNotInConflict(ItT first, ItT last)
+ template <class ItT>
+ inline std::optional<std::vector<char>>
+ NJointControllerBase::AreNotInConflict(ItT first, ItT last)
{
if (first == last)
{
- return std::vector<char> {};
+ return std::vector<char>{};
}
std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
std::vector<char> inuse(n, false);
@@ -293,43 +343,53 @@ namespace armarx
}
return inuse;
}
-}
+} // namespace armarx
namespace armarx::detail
{
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigDescription, GenerateConfigDescription,
- NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigFromVariants, GenerateConfigFromVariants,
- NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
-
- template<class NJointControllerT>
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+ hasGenerateConfigDescription,
+ GenerateConfigDescription,
+ NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+ hasGenerateConfigFromVariants,
+ GenerateConfigFromVariants,
+ NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
+
+ template <class NJointControllerT>
class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
{
- static_assert(
- hasGenerateConfigDescription<NJointControllerT>::value ==
- hasGenerateConfigFromVariants<NJointControllerT>::value,
- "Either overload both GenerateConfigDescription and GenerateConfigFromVariants, or none!"
- );
- static constexpr bool hasRemoteConfiguration_ = hasGenerateConfigDescription<NJointControllerT>::value;
-
- NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
- const NJointControllerConfigPtr& config,
- const VirtualRobot::RobotPtr& rob,
- bool deletable,
- bool internal,
- const std::string& instanceName) const final override
+ static_assert(hasGenerateConfigDescription<NJointControllerT>::value ==
+ hasGenerateConfigFromVariants<NJointControllerT>::value,
+ "Either overload both GenerateConfigDescription and "
+ "GenerateConfigFromVariants, or none!");
+ static constexpr bool hasRemoteConfiguration_ =
+ hasGenerateConfigDescription<NJointControllerT>::value;
+
+ NJointControllerBasePtr
+ create(RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& rob,
+ bool deletable,
+ bool internal,
+ const std::string& instanceName) const final override
{
ARMARX_CHECK_EXPRESSION(cmngr) << "ControllerManagement module is NULL!";
using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(cfg) << "The configuration is of the wrong type! it has to be an instance of: "
- << GetTypeString<ConfigPtrT>();
+ ARMARX_CHECK_EXPRESSION(cfg)
+ << "The configuration is of the wrong type! it has to be an instance of: "
+ << GetTypeString<ConfigPtrT>();
- ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning()) << "Two NJointControllers are created at the same time";
+ ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning())
+ << "Two NJointControllers are created at the same time";
NJointControllerBasePtr ptr;
{
ConstructorIsRunning_ = true;
- ARMARX_ON_SCOPE_EXIT{ConstructorIsRunning_ = false;};
+ ARMARX_ON_SCOPE_EXIT
+ {
+ ConstructorIsRunning_ = false;
+ };
ptr = new NJointControllerT(cmngr->_modulePtr<RobotUnit>(), cfg, rob);
}
ptr->deletable = deletable;
@@ -339,35 +399,42 @@ namespace armarx::detail
return ptr;
}
- WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr& robot,
- const std::map<std::string, ConstControlDevicePtr>& controlDevices,
- const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
+ WidgetDescription::WidgetPtr
+ GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
{
- if constexpr(hasRemoteConfiguration_)
+ if constexpr (hasRemoteConfiguration_)
{
try
{
- return NJointControllerT::GenerateConfigDescription(robot, controlDevices, sensorDevices);
+ return NJointControllerT::GenerateConfigDescription(
+ robot, controlDevices, sensorDevices);
}
catch (Ice::UserException& e)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'"
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'"
<< "\n---- file = " << e.ice_file()
<< "\n---- line = " << e.ice_line()
- << "\n---- id = " << e.ice_id()
- << "\n---- what:\n" << e.what()
- << "\n---- stacktrace:\n" << e.ice_stackTrace();
+ << "\n---- id = " << e.ice_id() << "\n---- what:\n"
+ << e.what() << "\n---- stacktrace:\n"
+ << e.ice_stackTrace();
throw;
}
catch (std::exception& e)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'"
- << "\n---- what:\n" << e.what();
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'"
+ << "\n---- what:\n"
+ << e.what();
throw;
}
catch (...)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigDescription'";
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'";
throw;
}
}
@@ -377,9 +444,10 @@ namespace armarx::detail
}
}
- NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
+ NJointControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
{
- if constexpr(hasRemoteConfiguration_)
+ if constexpr (hasRemoteConfiguration_)
{
try
{
@@ -387,23 +455,27 @@ namespace armarx::detail
}
catch (Ice::UserException& e)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'"
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'"
<< "\n---- file = " << e.ice_file()
<< "\n---- line = " << e.ice_line()
- << "\n---- id = " << e.ice_id()
- << "\n---- what:\n" << e.what()
- << "\n---- stacktrace:\n" << e.ice_stackTrace();
+ << "\n---- id = " << e.ice_id() << "\n---- what:\n"
+ << e.what() << "\n---- stacktrace:\n"
+ << e.ice_stackTrace();
throw;
}
catch (std::exception& e)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'"
- << "\n---- what:\n" << e.what();
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'"
+ << "\n---- what:\n"
+ << e.what();
throw;
}
catch (...)
{
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>() << "::GenerateConfigFromVariants'";
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'";
throw;
}
}
@@ -413,31 +485,35 @@ namespace armarx::detail
}
}
- bool hasRemoteConfiguration() const final override
+ bool
+ hasRemoteConfiguration() const final override
{
return hasRemoteConfiguration_;
}
};
-}
+} // namespace armarx::detail
namespace armarx
{
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
- template<class ControllerType>
+ template <class ControllerType>
struct NJointControllerRegistration
{
NJointControllerRegistration(const std::string& name)
{
NJointControllerRegistry::registerElement(
name,
- std::unique_ptr<NJointControllerRegistryEntry>(new detail::NJointControllerRegistryEntryHelper<ControllerType>));
+ std::unique_ptr<NJointControllerRegistryEntry>(
+ new detail::NJointControllerRegistryEntryHelper<ControllerType>));
}
};
-}
-#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T) \
- static_assert(::armarx::detail::hasGenerateConfigDescription<T>::value , \
- #T " does not offer a construction gui (missing: static GenerateConfigDescription)" ); \
- static_assert(::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
- #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
+} // namespace armarx
+#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T) \
+ static_assert( \
+ ::armarx::detail::hasGenerateConfigDescription<T>::value, \
+ #T " does not offer a construction gui (missing: static GenerateConfigDescription)"); \
+ static_assert( \
+ ::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
+ #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
#endif