Skip to content
Snippets Groups Projects
Commit be8c889b authored by Christian Dreher's avatar Christian Dreher
Browse files

fix: Add virtual destructor.

parent 2004477a
No related branches found
No related tags found
No related merge requests found
......@@ -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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment