From 8f071fd9c891fa3c3019d188c28dda299a011ee6 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Mon, 23 Apr 2018 11:18:36 +0200
Subject: [PATCH] Apply astyle formatting
---
.../RobotUnitModules/RobotUnitModuleBase.cpp | 707 +++++++++---------
.../RobotUnitModuleControlThread.cpp | 60 +-
.../RobotUnitModuleControlThread.h | 2 +-
.../RobotUnitModuleControllerManagement.cpp | 124 +--
.../RobotUnitModuleDevices.cpp | 80 +-
.../RobotUnitModuleRobotData.cpp | 24 +-
.../RobotUnitModuleSelfCollisionChecker.cpp | 74 +-
.../RobotUnitModuleSelfCollisionChecker.h | 4 +-
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 94 +--
9 files changed, 589 insertions(+), 580 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
index 48fb28cd7..354d1638d 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -31,450 +31,451 @@
namespace armarx
{
-std::string to_string(RobotUnitState s)
-{
- switch (s)
+ std::string to_string(RobotUnitState 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";
+ 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()
+ };
}
- throw std::invalid_argument
+ namespace RobotUnitModule
{
- "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;} \
- }
+ // //////////////////////////////////////////////////////////////////////////// //
+ // /////////////////////////// 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 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( \
+#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
+ );
+ 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__);
+ 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, _preOnInitRobotUnit(), true)
+ for_each_module(call_module_hook)
+#undef call_module_hook
- #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
- for_each_module(call_module_hook)
- #undef call_module_hook
+ onInitRobotUnit();
- finishComponentInitialization();
+#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
+ void ModuleBase::onConnectComponent()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
- onConnectRobotUnit();
+#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
+ for_each_module(call_module_hook)
+#undef call_module_hook
- #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();
+ onConnectRobotUnit();
- #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
- for_each_module(call_module_hook)
- #undef call_module_hook
+#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();
- onDisconnectRobotUnit();
+#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
+ for_each_module(call_module_hook)
+#undef call_module_hook
- #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
- for_each_module(call_module_hook)
- #undef call_module_hook
- }
+ onDisconnectRobotUnit();
- void ModuleBase::onExitComponent()
- {
- checkDerivedClasses();
- auto guard = getGuard();
+#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
+ for_each_module(call_module_hook)
+#undef call_module_hook
+ }
- finishRunning();
+ void ModuleBase::onExitComponent()
+ {
+ checkDerivedClasses();
+ auto guard = getGuard();
- #define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
- for_each_module(call_module_hook)
- #undef call_module_hook
+ finishRunning();
- onExitRobotUnit();
+#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
+ for_each_module(call_module_hook)
+#undef call_module_hook
- #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__);
+ 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
+#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);
+ 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, _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
+#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);
+ 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
- }
+#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__);
+ 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
+#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);
+ 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
- }
+#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__);
+ 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
+#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);
+ 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
- }
+#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)
+ void ModuleBase::finishRunning()
{
- ARMARX_ERROR << "called finishRunning when state was " << to_string(getRobotUnitState());
- }
+ 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
+#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();
+ 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
- }
+#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 /////////////////////////////////// //
- // //////////////////////////////////////////////////////////////////////////// //
+ // //////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////////// 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)
+ const std::set<RobotUnitState> ModuleBase::DevicesReadyStates
{
- 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};
- }
+ RobotUnitState::InitializingUnits,
+ RobotUnitState::InitializingControlThread,
+ RobotUnitState::Running
};
- switch (to)
+ void ModuleBase::setRobotUnitState(RobotUnitState 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)
+ auto guard = getGuard();
+
+ const auto transitionErrorMessage = [ = ](RobotUnitState expectedFrom)
{
- ARMARX_ERROR << transitionErrorMessage(RobotUnitState::Running);
- }
- default:
- throw std::invalid_argument
+ 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)
{
- "setRobotUnitState: Unknown target state " + to_string(static_cast<std::size_t>(to))+
- "\nStacktrace\n" + LogSender::CreateBackTrace()
+ if (state != expectedFrom)
+ {
+ const auto msg = transitionErrorMessage(expectedFrom);
+ ARMARX_ERROR << msg;
+ throw std::invalid_argument {msg};
+ }
};
- }
- 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
+ switch (to)
{
- "Called function '" + fnc + "' in the Control Thread\nStack trace:\n" +
- LogSender::CreateBackTrace()
- };
+ 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;
}
- }
- ModuleBase::GuardType ModuleBase::getGuard() const
- {
- if (inControlThread())
+
+ bool ModuleBase::areDevicesReady() const
{
- throw LogicError
- {
- "Attempted to lock mutex in Control Thread\nStack trace:\n" +
- LogSender::CreateBackTrace()
- };
+ return DevicesReadyStates.count(state);
}
- GuardType guard {dataMutex, std::defer_lock};
- if(guard.try_lock())
+
+ bool ModuleBase::inControlThread() const
{
- return guard;
+ return getRobotUnitState() == RobotUnitState::Running &&
+ std::this_thread::get_id() == _module<ControlThread>().getControlThreadId();
}
- while(!guard.try_lock_for(std::chrono::microseconds{100}))
+
+ void ModuleBase::throwIfInControlThread(const std::string& fnc) const
{
- if(isShuttingDown())
+ if (inControlThread())
{
- throw LogicError{"Attempting to lock mutex during shutdown\nStacktrace\n" + LogSender::CreateBackTrace()};
+ throw LogicError
+ {
+ "Called function '" + fnc + "' in the Control Thread\nStack trace:\n" +
+ LogSender::CreateBackTrace()
+ };
}
}
- return guard;
- }
-
- void ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState> &stateSet, const std::string &fnc, bool onlyWarn) const
- {
- if (! stateSet.count(state))
+ ModuleBase::GuardType ModuleBase::getGuard() const
{
- std::stringstream ss;
- ss << fnc << ": Can't be called if state is not in {";
- bool fst = true;
- for (RobotUnitState st : stateSet)
+ if (inControlThread())
{
- if (!fst)
+ throw LogicError
{
- ss << ", ";
- }
- ss << st;
- fst = false;
+ "Attempted to lock mutex in Control Thread\nStack trace:\n" +
+ LogSender::CreateBackTrace()
+ };
}
- ss << "} (current state " << getRobotUnitState() << ")\n"
- << "Stacktrace:\n" << LogSender::CreateBackTrace();
- ARMARX_ERROR << ss.str();
- if (!onlyWarn)
+ GuardType guard {dataMutex, std::defer_lock};
+ if (guard.try_lock())
{
- throw LogicError {ss.str()};
+ 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(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))
+ void ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
{
- std::stringstream ss;
- ss << fnc << ": Can't be called if state is in {";
- bool fst = true;
- for (RobotUnitState st : stateSet)
+ if (! stateSet.count(state))
{
- if (!fst)
+ 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)
{
- ss << ", ";
+ throw LogicError {ss.str()};
}
- ss << st;
- fst = false;
}
- ss << "} (current state " << getRobotUnitState() << ")\n"
- << "Stacktrace:\n" << LogSender::CreateBackTrace();
- ARMARX_ERROR << ss.str();
- if (!onlyWarn)
+ }
+ 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))
{
- throw LogicError {ss.str()};
+ 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::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
+ {
+ throwIfStateIs(std::set<RobotUnitState> {s}, fnc, onlyWarn);
+ }
- void ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
- {
- throwIfStateIsNot(DevicesReadyStates, fnc);
- }
+ void ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
+ {
+ throwIfStateIsNot(DevicesReadyStates, fnc);
+ }
- std::atomic<ModuleBase*> ModuleBase::Instance_{nullptr};
+ std::atomic<ModuleBase*> ModuleBase::Instance_ {nullptr};
- ModuleBase::ModuleBase()
- {
- if(Instance_ && this != Instance_)
+ ModuleBase::ModuleBase()
{
- ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = " << Instance_ << ", this = " << this << ")";
- std::terminate();
+ if (Instance_ && this != Instance_)
+ {
+ ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = " << Instance_ << ", this = " << this << ")";
+ std::terminate();
+ }
+ Instance_ = this;
}
- Instance_ = this;
- }
-}
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 3094d06bb..7434e8d61 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -75,7 +75,7 @@ namespace armarx
static void AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
{
p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement =
- p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
+ p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
}
static void CommitActivatedControllers(ControlThread* p)
{
@@ -289,7 +289,7 @@ namespace armarx
}
- void ControlThread::rtReadSensorDeviceValues(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
+ void ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart();
for (const SensorDevicePtr& device : rtGetSensorDevices())
@@ -301,7 +301,7 @@ namespace armarx
}
- void ControlThread::rtRunJointControllers(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
+ void ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersStart();
for (const ControlDevicePtr& device : rtGetControlDevices())
@@ -312,7 +312,7 @@ namespace armarx
}
- void ControlThread::rtRunNJointControllers(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
+ void ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < rtGetActivatedNJointControllers().size(); ++nJointCtrlIndex)
@@ -417,16 +417,16 @@ namespace armarx
{
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(ctrlDevIndex) << "\n"
- << VAROUT(rtGetActivatedJointControllers()) << "\n"
- << VAROUT(rtGetActivatedNJointControllers()) << "\n"
- << VAROUT(rtGetActivatedJointToNJointControllerAssignement())
- );
+ nJointCtrlIndex < rtGetControlDevices().size(),
+ nJointCtrlIndex << " < " << rtGetControlDevices().size() <<
+ ": no NJoint controller controls this device (name = "
+ << rtGetControlDevices().at(ctrlDevIndex)->getDeviceName()
+ << ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
+ << VAROUT(ctrlDevIndex) << "\n"
+ << VAROUT(rtGetActivatedJointControllers()) << "\n"
+ << VAROUT(rtGetActivatedNJointControllers()) << "\n"
+ << VAROUT(rtGetActivatedJointToNJointControllerAssignement())
+ );
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex, false);
@@ -436,7 +436,7 @@ namespace armarx
}
}
- void ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
+ void ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferStart();
SensorAndControl& sc = _module<ControlThreadDataBuffer>().rtGetSensorAndControlBuffer();
@@ -465,17 +465,17 @@ namespace armarx
rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferEnd();
}
- const std::vector<ControlDevicePtr> &ControlThread::rtGetControlDevices()
+ const std::vector<ControlDevicePtr>& ControlThread::rtGetControlDevices()
{
return DevicesAttorneyForControlThread::GetControlDevices(this);
}
- const std::vector<SensorDevicePtr> &ControlThread::rtGetSensorDevices()
+ const std::vector<SensorDevicePtr>& ControlThread::rtGetSensorDevices()
{
return DevicesAttorneyForControlThread::GetSensorDevices(this);
}
- RTThreadTimingsSensorDevice &ControlThread::rtGetThreadTimingsSensorDevice()
+ RTThreadTimingsSensorDevice& ControlThread::rtGetThreadTimingsSensorDevice()
{
return DevicesAttorneyForControlThread::GetThreadTimingsSensorDevice(this);
}
@@ -483,17 +483,17 @@ namespace armarx
void ControlThread::rtUpdateVirtualRobot()
{
DevicesAttorneyForControlThread::UpdateRobotWithSensorValues(this, rtRobot, rtRobotNodes);
- for(NJointController* c: rtGetActivatedNJointControllers())
+ for (NJointController* c : rtGetActivatedNJointControllers())
{
- if(!c)
+ if (!c)
{
break;
}
- if(c->rtGetRobot())
+ if (c->rtGetRobot())
{
auto& from = rtRobotNodes;
auto& to = c->rtGetRobotNodes();
- for(std::size_t i = 0; i < from.size(); ++i)
+ for (std::size_t i = 0; i < from.size(); ++i)
{
to.at(i)->copyPoseFrom(from.at(i));
}
@@ -522,17 +522,17 @@ namespace armarx
}
}
- void ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current &)
+ void ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
{
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
}
- EmergencyStopState ControlThread::getEmergencyStopState(const Ice::Current &) const
+ EmergencyStopState ControlThread::getEmergencyStopState(const Ice::Current&) const
{
return emergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
}
- EmergencyStopState ControlThread::getRtEmergencyStopState(const Ice::Current &) const
+ EmergencyStopState ControlThread::getRtEmergencyStopState(const Ice::Current&) const
{
return rtIsInEmergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
}
@@ -542,12 +542,12 @@ namespace armarx
emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
}
- std::vector<JointController *> &ControlThread::rtGetActivatedJointControllers()
+ std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
}
- std::vector<NJointController *> &ControlThread::rtGetActivatedNJointControllers()
+ std::vector<NJointController*>& ControlThread::rtGetActivatedNJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
}
@@ -557,17 +557,17 @@ namespace armarx
ControlThreadDataBufferAttorneyForControlThread::CommitActivatedControllers(this);
}
- const std::vector<JointController *> &ControlThread::rtGetRequestedJointControllers()
+ const std::vector<JointController*>& ControlThread::rtGetRequestedJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointControllers(this);
}
- const std::vector<NJointController *> &ControlThread::rtGetRequestedNJointControllers()
+ const std::vector<NJointController*>& ControlThread::rtGetRequestedNJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetRequestedNJointControllers(this);
}
- std::vector<std::size_t> &ControlThread::rtGetActivatedJointToNJointControllerAssignement()
+ 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
index 5e901c5fd..c732eb482 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -162,7 +162,7 @@ namespace armarx
* 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*/){}
+ 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
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 593baca54..6d5e841a5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -63,19 +63,19 @@ namespace armarx
{
namespace RobotUnitModule
{
- Ice::StringSeq ControllerManagement::getRequestedNJointControllerNames(const Ice::Current &) const
+ Ice::StringSeq ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
{
auto guard = getGuard();
return GetNonNullNames(_module<ControlThreadDataBuffer>().copyRequestedNJointControllers());
}
- Ice::StringSeq ControllerManagement::getActivatedNJointControllerNames(const Ice::Current &) const
+ Ice::StringSeq ControllerManagement::getActivatedNJointControllerNames(const Ice::Current&) const
{
auto guard = getGuard();
return GetNonNullNames(_module<ControlThreadDataBuffer>().getActivatedNJointControllers());
}
- void ControllerManagement::checkNJointControllerClassName(const std::string &className) const
+ void ControllerManagement::checkNJointControllerClassName(const std::string& className) const
{
if (!NJointControllerRegistry::has(className))
{
@@ -87,7 +87,7 @@ namespace armarx
}
}
- std::vector<NJointControllerPtr> ControllerManagement::getNJointControllersNotNull(const std::vector<std::string> &names) const
+ std::vector<NJointControllerPtr> ControllerManagement::getNJointControllersNotNull(const std::vector<std::string>& names) const
{
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
@@ -100,7 +100,7 @@ namespace armarx
return ctrl;
}
- const NJointControllerPtr &ControllerManagement::getNJointControllerNotNull(const std::string &name) const
+ const NJointControllerPtr& ControllerManagement::getNJointControllerNotNull(const std::string& name) const
{
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
@@ -123,12 +123,12 @@ namespace armarx
return it->second;
}
- void ControllerManagement::deleteNJointController(const NJointControllerPtr &ctrl)
+ void ControllerManagement::deleteNJointController(const NJointControllerPtr& ctrl)
{
deleteNJointControllers({ctrl});
}
- StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current &) const
+ StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current&) const
{
std::map<std::string, NJointControllerPtr> nJointControllersCopy;
{
@@ -145,21 +145,21 @@ namespace armarx
}
NJointControllerInterfacePrx ControllerManagement::createNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- const Ice::Current&)
+ 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));
+ 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&)
+ const std::string& className,
+ const std::string& instanceName,
+ const StringVariantBaseMap& variants,
+ const Ice::Current&)
{
//no lock required
checkNJointControllerClassName(className);
@@ -174,17 +174,17 @@ namespace armarx
throw InvalidArgumentException {ss.str()};
}
return createNJointController(
- className, instanceName,
- NJointControllerRegistry::get(className)->GenerateConfigFromVariants(variants),
- GlobalIceCurrent/*to select ice overload*/);
+ 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)
+ const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool deletable,
+ bool internal)
{
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
@@ -210,11 +210,11 @@ namespace armarx
//create the controller
ARMARX_CHECK_EXPRESSION(factory);
NJointControllerPtr nJointCtrl = factory->create(
- this,
- config,
- controllerCreateRobot,
- deletable, internal
- );
+ 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)"
@@ -227,33 +227,33 @@ namespace armarx
}
- bool ControllerManagement::loadLibFromPath(const std::string &path, const Ice::Current &)
+ 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 &)
+ 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
+ Ice::StringSeq ControllerManagement::getNJointControllerClassNames(const Ice::Current&) const
{
return NJointControllerRegistry::getKeys();
}
- Ice::StringSeq ControllerManagement::getNJointControllerNames(const Ice::Current &) const
+ Ice::StringSeq ControllerManagement::getNJointControllerNames(const Ice::Current&) const
{
auto guard = getGuard();
return getMapKeys(nJointControllers);
}
- std::vector<std::string> ControllerManagement::getNJointControllerNames(const std::vector<NJointControllerPtr> &ctrls) const
+ std::vector<std::string> ControllerManagement::getNJointControllerNames(const std::vector<NJointControllerPtr>& ctrls) const
{
std::vector<std::string> result;
result.reserve(ctrls.size());
- for(const auto& ctrl : ctrls)
+ for (const auto& ctrl : ctrls)
{
- if(ctrl)
+ if (ctrl)
{
result.emplace_back(ctrl->getInstanceName());
}
@@ -261,7 +261,7 @@ namespace armarx
return result;
}
- void ControllerManagement::activateNJointController(const std::string &name, const Ice::Current &)
+ void ControllerManagement::activateNJointController(const std::string& name, const Ice::Current&)
{
activateNJointControllers(getNJointControllersNotNull({name}));
}
@@ -269,7 +269,7 @@ namespace armarx
{
activateNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::activateNJointController(const NJointControllerPtr &ctrl)
+ void ControllerManagement::activateNJointController(const NJointControllerPtr& ctrl)
{
activateNJointControllers({ctrl});
}
@@ -285,10 +285,13 @@ namespace armarx
std::set<NJointControllerPtr, std::greater<NJointControllerPtr>> ctrlsToAct {ctrlsToActVec.begin(), ctrlsToActVec.end()};
ARMARX_CHECK_EXPRESSION(!ctrlsToAct.count(nullptr));
//check if all already active
- if(
+ if (
std::all_of(
ctrlsToActVec.begin(), ctrlsToActVec.end(),
- [](const NJointControllerPtr& ctrl){return ctrl->isControllerActive();}
+ [](const NJointControllerPtr & ctrl)
+ {
+ return ctrl->isControllerActive();
+ }
)
)
{
@@ -353,7 +356,7 @@ namespace armarx
_module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrlsToAct);
}
- void ControllerManagement::deactivateNJointController(const std::string &name, const Ice::Current &)
+ void ControllerManagement::deactivateNJointController(const std::string& name, const Ice::Current&)
{
deactivateNJointControllers(getNJointControllersNotNull({name}));
}
@@ -361,7 +364,7 @@ namespace armarx
{
deactivateNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::deactivateNJointController(const NJointControllerPtr &ctrl)
+ void ControllerManagement::deactivateNJointController(const NJointControllerPtr& ctrl)
{
deactivateNJointControllers({ctrl});
}
@@ -380,14 +383,14 @@ namespace armarx
{
ctrls.erase(nJointCtrlToDeactivate);
}
- if(ctrls.size() == ctrlsNum)
+ if (ctrls.size() == ctrlsNum)
{
return;
}
_module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrls);
}
- void ControllerManagement::deleteNJointController(const std::string &name, const Ice::Current &)
+ void ControllerManagement::deleteNJointController(const std::string& name, const Ice::Current&)
{
deleteNJointControllers(getNJointControllersNotNull({name}));
}
@@ -440,15 +443,15 @@ namespace armarx
}
}
- void ControllerManagement::deactivateAndDeleteNJointController(const std::string &name, const Ice::Current &)
+ void ControllerManagement::deactivateAndDeleteNJointController(const std::string& name, const Ice::Current&)
{
deactivateAndDeleteNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq &names, const Ice::Current &)
+ void ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
{
deactivateAndDeleteNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerPtr &ctrl)
+ void ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerPtr& ctrl)
{
deactivateAndDeleteNJointControllers({ctrl});
}
@@ -461,25 +464,28 @@ namespace armarx
return;
}
deactivateNJointControllers(ctrlsToDelVec);
- while(
+ while (
std::any_of(
ctrlsToDelVec.begin(),
ctrlsToDelVec.end(),
- [](const NJointControllerPtr& ctrl) {return ctrl->isControllerActive();}
+ [](const NJointControllerPtr & ctrl)
+ {
+ return ctrl->isControllerActive();
+ }
)
)
{
- if(isShuttingDown())
+ if (isShuttingDown())
{
return;
}
- std::this_thread::sleep_for(std::chrono::microseconds{100});
+ std::this_thread::sleep_for(std::chrono::microseconds {100});
}
deleteNJointControllers(ctrlsToDelVec);
}
NJointControllerClassDescription ControllerManagement::getNJointControllerClassDescription(
- const std::string& className, const Ice::Current&) const
+ const std::string& className, const Ice::Current&) const
{
while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices)
{
@@ -495,10 +501,10 @@ namespace armarx
{
data.configDescription =
NJointControllerRegistry::get(className)->GenerateConfigDescription(
- controllerCreateRobot,
- _module<Devices>().getControlDevicesConstPtr(),
- _module<Devices>().getSensorDevicesConstPtr()
- );
+ controllerCreateRobot,
+ _module<Devices>().getControlDevicesConstPtr(),
+ _module<Devices>().getSensorDevicesConstPtr()
+ );
}
return data;
}
@@ -631,7 +637,7 @@ namespace armarx
for (auto& n2NJointCtrl : ctrls)
{
NJointControllerPtr& nJointCtrl = n2NJointCtrl.second;
- if(blocking)
+ if (blocking)
{
ARMARX_VERBOSE << "deleted NJointController " << n2NJointCtrl.first;
getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
@@ -641,7 +647,7 @@ namespace armarx
ARMARX_VERBOSE << "deleted NJointController " << n2NJointCtrl.first;
getArmarXManager()->removeObjectBlocking(nJointCtrl->getName());
}
- if(l)
+ if (l)
{
l->nJointControllerDeleted(n2NJointCtrl.first);
}
@@ -676,7 +682,7 @@ namespace armarx
controllerCreateRobot = _module<RobotData>().cloneRobot();
}
- void ControllerManagement::updateNJointControllerRequestedState(const std::set<NJointControllerPtr> &request)
+ void ControllerManagement::updateNJointControllerRequestedState(const std::set<NJointControllerPtr>& request)
{
ARMARX_DEBUG << "set requested state for NJoint controllers";
for (const auto& name2NJoint : nJointControllers)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index e8fa25f0b..fa21a0fce 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -31,7 +31,7 @@ namespace armarx
{
const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
- ControlDeviceDescription Devices::getControlDeviceDescription(const std::string &name, const Ice::Current &) const
+ ControlDeviceDescription Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
@@ -45,7 +45,7 @@ namespace armarx
return getControlDeviceDescription(controlDevices.index(name));
}
- ControlDeviceDescriptionSeq Devices::getControlDeviceDescriptions(const Ice::Current &) const
+ ControlDeviceDescriptionSeq Devices::getControlDeviceDescriptions(const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
if (!areDevicesReady())
@@ -90,21 +90,21 @@ namespace armarx
return controlDevices.index(deviceName);
}
- ConstSensorDevicePtr Devices::getSensorDevice(const std::string &sensorDeviceName) const
+ 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
+ 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
+ Ice::StringSeq Devices::getControlDeviceNames(const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
@@ -147,7 +147,7 @@ namespace armarx
return status;
}
- ControlDeviceStatus Devices::getControlDeviceStatus(const std::string &name, const Ice::Current &) const
+ ControlDeviceStatus Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
@@ -161,7 +161,7 @@ namespace armarx
return getControlDeviceStatus(controlDevices.index(name));
}
- ControlDeviceStatusSeq Devices::getControlDeviceStatuses(const Ice::Current &) const
+ ControlDeviceStatusSeq Devices::getControlDeviceStatuses(const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
if (!areDevicesReady())
@@ -177,7 +177,7 @@ namespace armarx
return r;
}
- Ice::StringSeq Devices::getSensorDeviceNames(const Ice::Current &) const
+ Ice::StringSeq Devices::getSensorDeviceNames(const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
@@ -208,7 +208,7 @@ namespace armarx
return status;
}
- SensorDeviceDescription Devices::getSensorDeviceDescription(const std::string &name, const Ice::Current &) const
+ SensorDeviceDescription Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
@@ -222,7 +222,7 @@ namespace armarx
return getSensorDeviceDescription(sensorDevices.index(name));
}
- SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current &) const
+ SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
if (!areDevicesReady())
@@ -238,7 +238,7 @@ namespace armarx
return r;
}
- SensorDeviceStatus Devices::getSensorDeviceStatus(const std::string &name, const Ice::Current &) const
+ SensorDeviceStatus Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
throwIfDevicesNotReady(__FUNCTION__);
@@ -252,7 +252,7 @@ namespace armarx
return getSensorDeviceStatus(sensorDevices.index(name));
}
- SensorDeviceStatusSeq Devices::getSensorDeviceStatuses(const Ice::Current &) const
+ SensorDeviceStatusSeq Devices::getSensorDeviceStatuses(const Ice::Current&) const
{
throwIfInControlThread(__FUNCTION__);
if (!areDevicesReady())
@@ -268,7 +268,7 @@ namespace armarx
return r;
}
- void Devices::addControlDevice(const ControlDevicePtr &cd)
+ void Devices::addControlDevice(const ControlDevicePtr& cd)
{
throwIfInControlThread(__FUNCTION__);
//this guards prevents the RobotUnitState to change
@@ -280,7 +280,7 @@ namespace armarx
ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
}
- void Devices::addSensorDevice(const SensorDevicePtr &sd)
+ void Devices::addSensorDevice(const SensorDevicePtr& sd)
{
ARMARX_DEBUG << "SensorDevice " << &sd;
throwIfInControlThread(__FUNCTION__);
@@ -311,7 +311,7 @@ namespace armarx
throw InvalidArgumentException
{
"You tried to add a SensorDevice with the name " + sd->getDeviceName() +
- " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
+ " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
};
}
//this checks if we already added such a device (do this before setting timingSensorDevice)
@@ -342,14 +342,14 @@ namespace armarx
controlDevices.clear();
}
- std::vector<JointController *> Devices::getStopMovementJointControllers() const
+ 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())
+ for (const ControlDevicePtr& dev : controlDevices.values())
{
ARMARX_CHECK_NOT_NULL(dev);
controllers.emplace_back(dev->rtGetJointStopMovementController());
@@ -359,14 +359,14 @@ namespace armarx
return controllers;
}
- std::vector<JointController *> Devices::getEmergencyStopJointControllers() const
+ 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())
+ for (const ControlDevicePtr& dev : controlDevices.values())
{
ARMARX_CHECK_NOT_NULL(dev);
controllers.emplace_back(dev->rtGetJointEmergencyStopController());
@@ -448,7 +448,7 @@ namespace armarx
{
ARMARX_CHECK_EXPRESSION(sensorValues.empty());
sensorValues.reserve(sensorDevices.values().size());
- for(const SensorDevicePtr& dev : sensorDevices.values())
+ for (const SensorDevicePtr& dev : sensorDevices.values())
{
ARMARX_CHECK_NOT_NULL(dev);
sensorValues.emplace_back(dev->getSensorValue());
@@ -459,12 +459,12 @@ namespace armarx
{
ARMARX_CHECK_EXPRESSION(controlTargets.empty());
controlTargets.reserve(controlDevices.values().size());
- for(const ControlDevicePtr& dev : controlDevices.values())
+ 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())
+ for (JointController* ctrl : dev->rtGetJointControllers())
{
ARMARX_CHECK_NOT_NULL(ctrl);
controlTargets.back().emplace_back(ctrl->getControlTarget());
@@ -570,16 +570,16 @@ namespace armarx
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)
+ for (std::size_t idxRobot = 0; idxRobot < nodes.size(); ++idxRobot)
{
const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
- if(node->isRotationalJoint() || node->isTranslationalJoint())
+ if (node->isRotationalJoint() || node->isTranslationalJoint())
{
const auto& name = node->getName();
- if(sensorDevices.has(name))
+ if (sensorDevices.has(name))
{
const auto& dev = sensorDevices.at(name);
- if(dev->getSensorValue()->isA<SensorValue1DoFActuatorPosition>())
+ if (dev->getSensorValue()->isA<SensorValue1DoFActuatorPosition>())
{
SimoxRobotSensorValueMapping m;
m.idxRobot = idxRobot;
@@ -611,9 +611,9 @@ namespace armarx
if (!controlDeviceHardwareControlModeGroupsStr.empty())
{
const auto numGroups = std::count(
- controlDeviceHardwareControlModeGroupsStr.begin(),
- controlDeviceHardwareControlModeGroupsStr.end(), ';'
- ) + 1;
+ controlDeviceHardwareControlModeGroupsStr.begin(),
+ controlDeviceHardwareControlModeGroupsStr.end(), ';'
+ ) + 1;
ctrlModeGroups.groups.reserve(numGroups);
std::vector<std::string> strGroups;
strGroups.reserve(numGroups);
@@ -627,13 +627,13 @@ namespace armarx
{
boost::algorithm::trim(device);
ARMARX_CHECK_EXPRESSION_W_HINT(
- !device.empty(),
- "The ControlDeviceHardwareControlModeGroups property contains empty device names"
- );
+ !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.count(device),
+ "The ControlDeviceHardwareControlModeGroups property contains duplicate device names: " << device
+ );
ctrlModeGroups.groupsMerged.emplace(device);
group.emplace(std::move(device));
}
@@ -642,11 +642,11 @@ namespace armarx
ARMARX_DEBUG << "adding device group:\n"
<< ARMARX_STREAM_PRINTER
{
- for (const auto& elem : group)
- {
- out << " " << elem << "\n";
- }
- };
+ for (const auto& elem : group)
+ {
+ out << " " << elem << "\n";
+ }
+ };
ctrlModeGroups.groups.emplace_back(std::move(group));
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 12a389a88..dc91877a6 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -32,41 +32,41 @@ namespace armarx
{
namespace RobotUnitModule
{
- const std::string &RobotData::getRobotPlatformName() const
+ const std::string& RobotData::getRobotPlatformName() const
{
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robotPlatformName;
}
- const std::string &RobotData::getRobotNodetSeName() const
+ const std::string& RobotData::getRobotNodetSeName() const
{
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robotNodeSetName;
}
- const std::string &RobotData::getRobotProjectName() const
+ const std::string& RobotData::getRobotProjectName() const
{
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robotProjectName;
}
- const std::string &RobotData::getRobotFileName() const
+ const std::string& RobotData::getRobotFileName() const
{
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot 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");
+ std::lock_guard<std::mutex> guard {robotMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
return robot->getName();
}
VirtualRobot::RobotPtr RobotData::cloneRobot(bool updateCollisionModel) const
{
- std::lock_guard<std::mutex> guard{robotMutex};
- ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _preOnInitRobotUnit was called");
+ std::lock_guard<std::mutex> guard {robotMutex};
+ ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called");
ARMARX_CHECK_EXPRESSION(robot);
const VirtualRobot::RobotPtr clone = robot->clone();
clone->setUpdateVisualization(false);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 1f4b83449..017015656 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -48,17 +48,17 @@ namespace armarx
std::set<std::string> nodes;
{
auto splittedRaw = armarx::Split(group, ",", true, true);
- if(splittedRaw.size() < 2)
+ if (splittedRaw.size() < 2)
{
continue;
}
- for(auto& subentry : splittedRaw)
+ for (auto& subentry : splittedRaw)
{
boost::trim_if(subentry, boost::is_any_of(" \t{}"));
- if(selfCollisionAvoidanceRobot->hasRobotNodeSet(subentry))
+ if (selfCollisionAvoidanceRobot->hasRobotNodeSet(subentry))
{
//the entry is a set
- for(const auto& node : selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)->getAllRobotNodes())
+ for (const auto& node : selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)->getAllRobotNodes())
{
if (!node->getCollisionModel())
{
@@ -70,7 +70,7 @@ namespace armarx
nodes.emplace(node->getName());
}
}
- else if(selfCollisionAvoidanceRobot->hasRobotNode(subentry))
+ else if (selfCollisionAvoidanceRobot->hasRobotNode(subentry))
{
//the entry is a node
if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)->getCollisionModel())
@@ -82,7 +82,7 @@ namespace armarx
}
nodes.emplace(subentry);
}
- else if(subentry == "FLOOR")
+ else if (subentry == "FLOOR")
{
//the entry is the floor
nodes.emplace(subentry);
@@ -99,11 +99,11 @@ namespace armarx
- for(auto it1 = nodes.begin(); it1 != nodes.end(); ++it1)
+ for (auto it1 = nodes.begin(); it1 != nodes.end(); ++it1)
{
auto it2 = it1;
++it2;
- for(; it2 != nodes.end(); ++it2)
+ for (; it2 != nodes.end(); ++it2)
{
namePairsToCheck.emplace(*it1, *it2);
}
@@ -120,19 +120,20 @@ namespace armarx
void SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current&)
{
- std::lock_guard<std::mutex> guard{selfCollisionDataMutex};
- if(distance < 0)
+ std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
+ if (distance < 0)
{
- throw InvalidArgumentException{
+ throw InvalidArgumentException
+ {
std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal distance:" + std::to_string(distance)
};
}
- if(distance == minSelfDistance && !nodePairsToCheck.empty())
+ if (distance == minSelfDistance && !nodePairsToCheck.empty())
{
return;
}
//reset data
- ARMARX_CHECK_GREATER(distance,0);
+ ARMARX_CHECK_GREATER(distance, 0);
minSelfDistance = distance;
selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
selfCollisionAvoidanceRobotNodes = selfCollisionAvoidanceRobot->getRobotNodes();
@@ -154,17 +155,17 @@ namespace armarx
}
}
//collect pairs
- for(const auto& pair : namePairsToCheck)
+ for (const auto& pair : namePairsToCheck)
{
VirtualRobot::SceneObjectPtr first =
- (pair.first == "FLOOR") ?
- floor->getSceneObject(0) :
- selfCollisionAvoidanceRobot->getRobotNode(pair.first );
+ (pair.first == "FLOOR") ?
+ floor->getSceneObject(0) :
+ selfCollisionAvoidanceRobot->getRobotNode(pair.first);
VirtualRobot::SceneObjectPtr second =
- (pair.second == "FLOOR") ?
- floor->getSceneObject(0) :
- selfCollisionAvoidanceRobot->getRobotNode(pair.second);
+ (pair.second == "FLOOR") ?
+ floor->getSceneObject(0) :
+ selfCollisionAvoidanceRobot->getRobotNode(pair.second);
nodePairsToCheck.emplace_back(first, second);
}
@@ -172,9 +173,10 @@ namespace armarx
void SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
{
- if(freq < 0)
+ if (freq < 0)
{
- throw InvalidArgumentException{
+ throw InvalidArgumentException
+ {
std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal frequency:" + std::to_string(freq)
};
}
@@ -196,13 +198,13 @@ namespace armarx
return minSelfDistance;
}
- void SelfCollisionChecker::componentPropertiesUpdated(const std::set<std::string> &changed)
+ void SelfCollisionChecker::componentPropertiesUpdated(const std::set<std::string>& changed)
{
- if(changed.count("SelfCollisionCheckFrequency"))
+ if (changed.count("SelfCollisionCheckFrequency"))
{
setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheckFrequency").getValue());
}
- if(changed.count("MinSelfDistance"))
+ if (changed.count("MinSelfDistance"))
{
setSelfCollisionAvoidanceDistance(getProperty<float>("MinSelfDistance").getValue());
}
@@ -210,38 +212,38 @@ namespace armarx
void SelfCollisionChecker::_postFinishControlThreadInitialization()
{
- selfCollisionAvoidanceThread = std::thread{[&]{selfCollisionAvoidanceTask();}};
+ selfCollisionAvoidanceThread = std::thread {[&]{selfCollisionAvoidanceTask();}};
}
void SelfCollisionChecker::selfCollisionAvoidanceTask()
{
- while(true)
+ while (true)
{
const auto startT = std::chrono::high_resolution_clock::now();
//done
- if(isShuttingDown())
+ if (isShuttingDown())
{
return;
}
const auto freq = checkFrequency.load();
if (
- _module<ControlThread>().getEmergencyStopState() ||
- freq == 0
- )
+ _module<ControlThread>().getEmergencyStopState() ||
+ freq == 0
+ )
{
//currently wait
- std::this_thread::sleep_for(std::chrono::microseconds{1000});
+ std::this_thread::sleep_for(std::chrono::microseconds {1000});
continue;
}
//update robot + check
{
- std::lock_guard<std::mutex> guard{selfCollisionDataMutex};
+ std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
//update robot
_module<ControlThreadDataBuffer>().updateVirtualRobot(selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
//check
- for(const auto& pair : nodePairsToCheck)
+ for (const auto& pair : nodePairsToCheck)
{
- if(selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
+ if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
{
ARMARX_WARNING << "COLLISION: '" << pair.first->getName() << "' and '" << pair.second->getName() << "'";
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
@@ -252,7 +254,7 @@ namespace armarx
}
}
//sleep remaining
- std::this_thread::sleep_until(startT + std::chrono::microseconds{static_cast<int64_t>(1000000 / freq)});
+ std::this_thread::sleep_until(startT + std::chrono::microseconds {static_cast<int64_t>(1000000 / freq)});
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
index 5fb8e2faa..7645cf87e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
@@ -143,7 +143,7 @@ namespace armarx
/// @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};
+ std::atomic<float> checkFrequency {0};
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// col data /////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -151,7 +151,7 @@ namespace armarx
///@ 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};
+ 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
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index accc6c494..9a963cd85 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -47,7 +47,7 @@ namespace armarx
public:
RobotUnitEmergencyStopMaster(ControlThread* controlThreadModule, std::string emergencyStopTopicName) :
controlThreadModule {controlThreadModule},
- emergencyStopTopicName {emergencyStopTopicName}
+ emergencyStopTopicName {emergencyStopTopicName}
{
ARMARX_CHECK_NOT_NULL(controlThreadModule);
ARMARX_CHECK_EXPRESSION(!emergencyStopTopicName.empty());
@@ -55,7 +55,7 @@ namespace armarx
virtual void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = GlobalIceCurrent) final
{
- if(getEmergencyStopState() == state)
+ if (getEmergencyStopState() == state)
{
return;
}
@@ -95,7 +95,7 @@ namespace armarx
{
namespace RobotUnitModule
{
- Ice::ObjectProxySeq Units::getUnits(const Ice::Current &) const
+ Ice::ObjectProxySeq Units::getUnits(const Ice::Current&) const
{
std::vector<ManagedIceObjectPtr> unitsCopy;
{
@@ -112,7 +112,7 @@ namespace armarx
return r;
}
- Ice::ObjectPrx Units::getUnit(const std::string &staticIceId, const Ice::Current &) const
+ Ice::ObjectPrx Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
{
//no lock required
ManagedIceObjectPtr unit = getUnit(staticIceId);
@@ -123,7 +123,7 @@ namespace armarx
return nullptr;
}
- const EmergencyStopMasterInterfacePtr &Units::getEmergencyStopMaster() const
+ const EmergencyStopMasterInterfacePtr& Units::getEmergencyStopMaster() const
{
return emergencyStopMaster;
}
@@ -175,36 +175,36 @@ namespace armarx
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();
+ _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) \
- { \
+#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); \
- } \
- }
+ { \
+ 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
+ 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)
+ if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
{
devs[controlDeviceName] = std::move(ad);
}
@@ -230,13 +230,13 @@ namespace armarx
//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)
- );
+ 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));
}
@@ -286,9 +286,9 @@ namespace armarx
config->initialVelocityRotation = 0;
config->platformName = _module<RobotData>().getRobotPlatformName();
auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformUnitVelocityPassThroughController", configName + "_VelPTContoller",
- config, false, true
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformUnitVelocityPassThroughController", configName + "_VelPTContoller",
+ config, false, true
)
);
ARMARX_CHECK_EXPRESSION(ctrl);
@@ -321,16 +321,16 @@ namespace armarx
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"
- );
+ !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() << "'"
- );
+ 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));
}
}
@@ -525,7 +525,7 @@ namespace armarx
ARMARX_DEBUG << "remove EmergencyStopMaster...done!";
}
- const ManagedIceObjectPtr &Units::getUnit(const std::string &staticIceId) const
+ const ManagedIceObjectPtr& Units::getUnit(const std::string& staticIceId) const
{
auto guard = getGuard();
for (const ManagedIceObjectPtr& unit : units)
--
GitLab