From 840c0f200bec42da0f5dc72c478970854a59a6f9 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Thu, 15 Oct 2020 08:23:58 +0200
Subject: [PATCH] Add ARMARX_TRACE
---
.../NJointControllers/NJointController.cpp | 11 +++++++++++
.../RobotUnitModules/RobotUnitModuleBase.cpp | 17 +++++++++++++++++
.../RobotUnitModules/RobotUnitModuleDevices.cpp | 8 ++++++++
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 1 +
4 files changed, 37 insertions(+)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index 57e87b49d..7f86a607d 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -69,6 +69,7 @@ namespace armarx
NJointControllerDescription NJointControllerBase::getControllerDescription(const Ice::Current&) const
{
+ ARMARX_TRACE;
NJointControllerDescription d;
d.className = getClassName();
ARMARX_CHECK_EXPRESSION(getProxy(-1));
@@ -82,6 +83,7 @@ namespace armarx
std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
{
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
auto result = used;
for (std::size_t i = 0; i < controlDeviceUsedBitmap.size(); ++i)
@@ -101,6 +103,7 @@ namespace armarx
NJointControllerStatus NJointControllerBase::getControllerStatus(const Ice::Current&) const
{
+ ARMARX_TRACE;
NJointControllerStatus s;
s.active = isActive;
s.instanceName = getInstanceName();
@@ -144,6 +147,7 @@ namespace armarx
void NJointControllerBase::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
{
+ ARMARX_TRACE;
const bool active = isActive;
if (publishActive.exchange(active) != active)
{
@@ -199,6 +203,7 @@ namespace armarx
void NJointControllerBase::deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
{
+ ARMARX_TRACE;
if (publishActive.exchange(false))
{
ARMARX_DEBUG << "forced deactivation of publishing for " << getInstanceName();
@@ -261,21 +266,25 @@ namespace armarx
void NJointControllerBase::onInitComponent()
{
+ ARMARX_TRACE;
onInitNJointController();
}
void NJointControllerBase::onConnectComponent()
{
+ ARMARX_TRACE;
onConnectNJointController();
}
void NJointControllerBase::onDisconnectComponent()
{
+ ARMARX_TRACE;
onDisconnectNJointController();
}
void NJointControllerBase::onExitComponent()
{
+ ARMARX_TRACE;
onExitNJointController();
// make sure the destructor of the handles does not throw an exception and triggers a termination of the process
ARMARX_DEBUG << "Deleting thread handles";
@@ -331,6 +340,7 @@ namespace armarx
const SensorValueBase* NJointControllerBase::useSensorValue(const std::string& deviceName) const
{
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(
NJointControllerRegistryEntry::ConstructorIsRunning()) <<
"The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
@@ -356,6 +366,7 @@ namespace armarx
ControlTargetBase* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode)
{
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(
NJointControllerRegistryEntry::ConstructorIsRunning()) <<
"The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
index 8432b4b53..aa324e0c1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/Preprocessor.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
#include "../NJointControllers/NJointController.h"
#include "RobotUnitModuleBase.h"
@@ -62,6 +63,7 @@ namespace armarx::RobotUnitModule
// /////////////////////////// call for each module /////////////////////////// //
// //////////////////////////////////////////////////////////////////////////// //
#define cast_to_and_call(Type, fn, rethrow) \
+ ARMARX_TRACE; \
try \
{ \
dynamic_cast<Type*>(this)->Type::fn; \
@@ -112,6 +114,7 @@ namespace armarx::RobotUnitModule
// ManagedIceObject life cycle
void ModuleBase::onInitComponent()
{
+ ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
@@ -134,6 +137,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::onConnectComponent()
{
+ ARMARX_TRACE;
checkDerivedClasses();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
@@ -148,6 +152,7 @@ namespace armarx::RobotUnitModule
}
void ModuleBase::onDisconnectComponent()
{
+ ARMARX_TRACE;
checkDerivedClasses();
#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
@@ -163,6 +168,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::onExitComponent()
{
+ ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
@@ -196,6 +202,7 @@ namespace armarx::RobotUnitModule
// RobotUnit life cycle
void ModuleBase::finishComponentInitialization()
{
+ ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
@@ -212,6 +219,7 @@ namespace armarx::RobotUnitModule
}
void ModuleBase::finishDeviceInitialization()
{
+ ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
@@ -229,6 +237,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::finishUnitInitialization()
{
+ ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
@@ -246,6 +255,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::finishControlThreadInitialization()
{
+ ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingControlThread, __FUNCTION__);
@@ -263,6 +273,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::finishRunning()
{
+ ARMARX_TRACE;
checkDerivedClasses();
shutDown();
GuardType guard {dataMutex}; //DO NOT USE getGuard here!
@@ -299,6 +310,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::setRobotUnitState(RobotUnitState to)
{
+ ARMARX_TRACE;
auto guard = getGuard();
const auto transitionErrorMessage = [ = ](RobotUnitState expectedFrom)
@@ -363,6 +375,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::throwIfInControlThread(const std::string& fnc) const
{
+ ARMARX_TRACE;
if (inControlThread())
{
std::stringstream str;
@@ -374,6 +387,7 @@ namespace armarx::RobotUnitModule
ModuleBase::GuardType ModuleBase::getGuard() const
{
+ ARMARX_TRACE;
if (inControlThread())
{
throw LogicError
@@ -400,6 +414,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
{
+ ARMARX_TRACE;
if (! stateSet.count(state))
{
std::stringstream ss;
@@ -430,6 +445,7 @@ namespace armarx::RobotUnitModule
void ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
{
+ ARMARX_TRACE;
if (stateSet.count(state))
{
std::stringstream ss;
@@ -468,6 +484,7 @@ namespace armarx::RobotUnitModule
ModuleBase::ModuleBase()
{
+ ARMARX_TRACE;
if (Instance_ && this != Instance_)
{
ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = " << Instance_ << ", this = " << this << ")";
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index c0874a794..6d5e0bdb0 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -451,11 +451,13 @@ namespace armarx::RobotUnitModule
void Devices::_postFinishDeviceInitialization()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<MutexType> guardS {sensorDevicesMutex};
std::lock_guard<MutexType> guardC {controlDevicesMutex};
ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
{
+ ARMARX_TRACE;
for (const ControlDevicePtr& controlDevice : controlDevices.values())
{
ARMARX_CHECK_EXPRESSION(controlDevice);
@@ -485,6 +487,7 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "checking " << controlDevices.size() << " SensorDevices:";
{
+ ARMARX_TRACE;
for (const SensorDevicePtr& sensorDevice : sensorDevices.values())
{
ARMARX_CHECK_EXPRESSION(sensorDevice);
@@ -501,6 +504,7 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "copying device ptrs to const ptr map";
{
+ ARMARX_TRACE;
for (const auto& dev : controlDevices.values())
{
controlDevicesConstPtr[dev->getDeviceName()] = dev;
@@ -512,6 +516,7 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "copy sensor values";
{
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(sensorValues.empty());
sensorValues.reserve(sensorDevices.values().size());
for (const SensorDevicePtr& dev : sensorDevices.values())
@@ -523,6 +528,7 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "copy control targets";
{
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(controlTargets.empty());
controlTargets.reserve(controlDevices.values().size());
for (const ControlDevicePtr& dev : controlDevices.values())
@@ -540,6 +546,7 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "setup ControlDeviceHardwareControlModeGroups";
{
+ ARMARX_TRACE;
ctrlModeGroups.groupIndices.assign(getNumberOfControlDevices(), IndexSentinel());
if (!ctrlModeGroups.groupsMerged.empty())
@@ -631,6 +638,7 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "create mapping from sensor values to robot nodes";
{
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(simoxRobotSensorValueMapping.empty());
VirtualRobot::RobotPtr r = _module<RobotData>().cloneRobot();
const auto nodes = r->getRobotNodes();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 76e2811dc..6ade7b907 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -193,6 +193,7 @@ namespace armarx::RobotUnitModule
const std::string& torqueControlMode
)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
/// TODO move init code to Kinematic sub unit
using UnitT = KinematicSubUnit;
--
GitLab