Skip to content
Snippets Groups Projects
Commit 840c0f20 authored by Raphael Grimm's avatar Raphael Grimm
Browse files

Add ARMARX_TRACE

parent b0240ad5
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+ 11
− 0
View file @ 840c0f20
......@@ -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.";
......
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+ 17
− 0
View file @ 840c0f20
......@@ -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 << ")";
......
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+ 8
− 0
View file @ 840c0f20
......@@ -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();
......
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+ 1
− 0
View file @ 840c0f20
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment