diff --git a/source/armarx/control/CMakeLists.txt b/source/armarx/control/CMakeLists.txt
index b6c7c54705e00ae685bedd82df947d5c7622a615..6c4dfbdb4b339daa6a43254f05bff3e0fd6f2221 100644
--- a/source/armarx/control/CMakeLists.txt
+++ b/source/armarx/control/CMakeLists.txt
@@ -24,6 +24,6 @@ add_subdirectory(skills)
add_subdirectory(ui)
# Components
+add_subdirectory(retrieve_hand)
add_subdirectory(components)
-add_subdirectory(retrieve_hand)
\ No newline at end of file
diff --git a/source/armarx/control/components/controller_creator/Component.cpp b/source/armarx/control/components/controller_creator/Component.cpp
index 7993846af9494d848e906e8247a626d290612801..b4bc1faea12d131f6e5199eea43d0fe8a079f844 100644
--- a/source/armarx/control/components/controller_creator/Component.cpp
+++ b/source/armarx/control/components/controller_creator/Component.cpp
@@ -826,7 +826,7 @@ namespace armarx::control::components::controller_creator
std::string trajName;
{
- IceUtil::Time now = IceUtil::Time::now();
+ IceUtil::Time now = armarx::rtNow();
time_t timer = now.toSeconds();
struct tm* ts;
char buffer[80];
diff --git a/source/armarx/control/components/retrieve_hand_skill_provider/CMakeLists.txt b/source/armarx/control/components/retrieve_hand_skill_provider/CMakeLists.txt
index b1057e36ced1783f253271fcf964ec66796a9cb3..6dfd829a915468ff16105827ae6b97f3d6e6e422 100644
--- a/source/armarx/control/components/retrieve_hand_skill_provider/CMakeLists.txt
+++ b/source/armarx/control/components/retrieve_hand_skill_provider/CMakeLists.txt
@@ -18,6 +18,7 @@ armarx_add_component(retrieve_hand_skill_provider
DEPENDENCIES
# ArmarXCore
ArmarXCore
+ ArmarXCoreInterfaces
## ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
@@ -28,7 +29,7 @@ armarx_add_component(retrieve_hand_skill_provider
RobotAPISkills
# armarx_control
- armarx_control::retrieve_hand
+ armarx_control::retrieve_hand_skills
# DEPENDENCIES_LEGACY
## Add libraries that do not provide any targets but ${FOO_*} variables.
diff --git a/source/armarx/control/components/retrieve_hand_skill_provider/Component.cpp b/source/armarx/control/components/retrieve_hand_skill_provider/Component.cpp
index 1e39204a6c27d0c7c06b71ce58566a38222d8e0d..0a4bf33cba271cd4288fdb103b3b288b5faad595 100644
--- a/source/armarx/control/components/retrieve_hand_skill_provider/Component.cpp
+++ b/source/armarx/control/components/retrieve_hand_skill_provider/Component.cpp
@@ -88,7 +88,7 @@ namespace armarx::control::components::retrieve_hand_skill_provider
namespace rh = ::armarx::control::retrieve_hand::skills;
- rh::skills::RetrieveHand::Remote r;
+ rh::RetrieveHand::Remote r;
// r.arviz = this->arviz;
r.robotUnit = remote.robotUnit;
@@ -102,7 +102,7 @@ namespace armarx::control::components::retrieve_hand_skill_provider
// addSkillFactory<ho::skills::HandOverObjectToHuman>(r, p);
// Option 1: Simply add a new instance of the skill.
- addSkillFactory<skills::RetrieveHand>(r);
+ addSkillFactory<rh::RetrieveHand>(r);
/* Option 2: Initialize skill after creation, e.g. set parameters, pass remote proxies.
{
skills::RetrieveHand* skill = addSkill<skills::RetrieveHand>();
diff --git a/source/armarx/control/components/retrieve_hand_skill_provider/Component.h b/source/armarx/control/components/retrieve_hand_skill_provider/Component.h
index 79c988c52155eb0c8e50f5487ea04836ed17f9b6..5fa709e45eee1ca3834d524a87ecf6847ee3ea0c 100644
--- a/source/armarx/control/components/retrieve_hand_skill_provider/Component.h
+++ b/source/armarx/control/components/retrieve_hand_skill_provider/Component.h
@@ -34,6 +34,7 @@
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
#include <armarx/control/components/retrieve_hand_skill_provider/ComponentInterface.h>
diff --git a/source/armarx/control/deprecated_njoint_mp_controller/task_space/DeprecatedNJointTSDMPController.cpp b/source/armarx/control/deprecated_njoint_mp_controller/task_space/DeprecatedNJointTSDMPController.cpp
index 8e2d793491705733c59faff84dff340c176e2e90..0274beff1b6efbeaf45ba74b6a632a88d73828ea 100644
--- a/source/armarx/control/deprecated_njoint_mp_controller/task_space/DeprecatedNJointTSDMPController.cpp
+++ b/source/armarx/control/deprecated_njoint_mp_controller/task_space/DeprecatedNJointTSDMPController.cpp
@@ -7,6 +7,20 @@
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
+
+ struct RTScopeTimer
+ {
+ RTScopeTimer(const std::string& name) : start(armarx::rtNow()), name(name)
+ {
+ }
+ ~RTScopeTimer()
+ {
+ //float us = (armarx::rtNow() - start).toMicroSecondsDouble();
+ //ARMARX_IMPORTANT << name << " took " << us << " micro seconds";
+ }
+ IceUtil::Time start;
+ std::string name;
+ };
NJointControllerRegistration<DeprecatedNJointTSDMPController>
registrationControllerDeprecatedNJointTSDMPController("DeprecatedNJointTSDMPController");
@@ -15,6 +29,8 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
const armarx::NJointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
+ RTScopeTimer timer("DeprecatedNJointTSDMPController_constructor");
+ RT_TIMING_START(DeprecatedNJointTSDMPController_constructor)
useSynchronizedRtRobot();
cfg = DeprecatedNJointTaskSpaceDMPControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
@@ -144,6 +160,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
RTToUserData initInterfaceData;
initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
rt2UserData.reinitAllBuffers(initInterfaceData);
+ RT_TIMING_END(DeprecatedNJointTSDMPController_constructor)
}
std::string
@@ -155,6 +172,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
void
DeprecatedNJointTSDMPController::controllerRun()
{
+ RTScopeTimer timer("controllerRun");
if (!started)
{
return;
@@ -222,6 +240,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
const Eigen::VectorXf& nullspaceVel,
VirtualRobot::IKSolver::CartesianSelection mode)
{
+ RTScopeTimer timer("calcIK");
Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, mode);
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
@@ -251,6 +270,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
DeprecatedNJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
+ RT_TIMING_START(DeprecatedNJointTSDMPController_rtRun)
Eigen::Matrix4f currentPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
rt2UserData.commitWrite();
@@ -407,12 +427,14 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
}
rtDebugData.getWriteBuffer().targetJointVels = jointTargetVelocities;
rtDebugData.commitWrite();
+ RT_TIMING_CEND(DeprecatedNJointTSDMPController_rtRun, 0.1)
}
void
DeprecatedNJointTSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
const Ice::Current&)
{
+ RTScopeTimer timer("learnDMPFromFiles");
ARMARX_INFO << "Learning DMP ... ";
LockGuardType guard{controllerMutex};
@@ -422,6 +444,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
void
DeprecatedNJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
{
+ RTScopeTimer timer("setSpeed");
LockGuardType guard{controllerMutex};
dmpCtrl->setSpeed(times);
}
@@ -431,6 +454,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
const Ice::DoubleSeq& viapoint,
const Ice::Current&)
{
+ RTScopeTimer timer("setViaPoints");
LockGuardType guard{controllerMutex};
dmpCtrl->setViaPose(u, viapoint);
}
@@ -439,6 +463,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
DeprecatedNJointTSDMPController::setTorqueKp(const StringFloatDictionary& torqueKp,
const Ice::Current&)
{
+ RTScopeTimer timer("setTorqueKp");
LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
@@ -453,6 +478,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
const StringFloatDictionary& nullspaceJointVelocities,
const Ice::Current&)
{
+ RTScopeTimer timer("setNullspaceJointVelocities");
LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
@@ -468,6 +494,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
NJointTaskSpaceDMPControllerMode::CartesianSelection mode,
const Ice::Current&)
{
+ RTScopeTimer timer("setControllerTarget");
LockGuardType guard{controlDataMutex};
getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
getWriterControlStruct().mode = ModeFromIce(mode);
@@ -477,6 +504,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
void
DeprecatedNJointTSDMPController::removeAllViaPoints(const Ice::Current&)
{
+ RTScopeTimer timer("removeAllViaPoints");
LockGuardType guard{controllerMutex};
ARMARX_INFO << "setting via points ";
dmpCtrl->removeAllViaPoints();
@@ -485,6 +513,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
void
DeprecatedNJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
{
+ RTScopeTimer timer("setGoals");
LockGuardType guard{controllerMutex};
dmpCtrl->setGoalPoseVec(goals);
}
@@ -492,18 +521,21 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
void
DeprecatedNJointTSDMPController::pauseDMP(const Ice::Current&)
{
+ RTScopeTimer timer("pauseDMP");
dmpCtrl->pauseController();
}
void
DeprecatedNJointTSDMPController::resumeDMP(const Ice::Current&)
{
+ RTScopeTimer timer("resumeDMP");
dmpCtrl->resumeController();
}
void
DeprecatedNJointTSDMPController::resetDMP(const Ice::Current&)
{
+ RTScopeTimer timer("resetDMP");
if (started)
{
ARMARX_INFO << "Cannot reset running DMP";
@@ -514,6 +546,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
void
DeprecatedNJointTSDMPController::stopDMP(const Ice::Current&)
{
+ RTScopeTimer timer("stopDMP");
started = false;
stopped = true;
}
@@ -521,7 +554,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
std::string
DeprecatedNJointTSDMPController::getDMPAsString(const Ice::Current&)
{
-
+ RTScopeTimer timer("getDMPAsString");
return dmpCtrl->saveDMPToString();
}
@@ -529,6 +562,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
DeprecatedNJointTSDMPController::createDMPFromString(const std::string& dmpString,
const Ice::Current&)
{
+ RTScopeTimer timer("createDMPFromString");
dmpCtrl->loadDMPFromString(dmpString);
return dmpCtrl->dmpPtr->defaultGoal;
}
@@ -537,6 +571,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
DeprecatedNJointTSDMPController::ModeFromIce(
const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
{
+ RTScopeTimer timer("ModeFromIce");
if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
{
return VirtualRobot::IKSolver::CartesianSelection::Position;
@@ -558,6 +593,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
double tau,
const Ice::Current&)
{
+ RTScopeTimer timer("runDMP");
ARMARX_INFO << "------dmp controller: " << VAROUT(goals);
firstRun = true;
@@ -593,6 +629,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
Ice::Double timeDuration,
const Ice::Current&)
{
+ RTScopeTimer timer("runDMPWithTime");
firstRun = true;
while (firstRun)
{
@@ -628,6 +665,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
const DebugDrawerInterfacePrx&,
const DebugObserverInterfacePrx& debugObs)
{
+ RTScopeTimer timer("onPublish");
std::string datafieldName = debugName;
StringVariantBaseMap datafields;
auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
@@ -684,6 +722,7 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
void
DeprecatedNJointTSDMPController::onInitNJointController()
{
+ RTScopeTimer timer("onInitNJointController");
ARMARX_INFO << "init ...";
started = false;
runTask("DeprecatedNJointTSDMPController",
@@ -711,12 +750,14 @@ namespace armarx::control::deprecated_njoint_mp_controller::task_space
void
DeprecatedNJointTSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
{
+ RTScopeTimer timer("setMPWeights");
dmpCtrl->setWeights(weights);
}
DoubleSeqSeq
DeprecatedNJointTSDMPController::getMPWeights(const Ice::Current&)
{
+ RTScopeTimer timer("getMPWeights");
DMP::DVec2d res = dmpCtrl->getWeights();
DoubleSeqSeq resvec;
for (size_t i = 0; i < res.size(); ++i)
diff --git a/source/armarx/control/ethercat/Bus.cpp b/source/armarx/control/ethercat/Bus.cpp
index d2340943d985b1ed6c20ae2528d3aab9df7e1706..42d6e89f3e0d3b8474d525ac715608e1d3059adf 100644
--- a/source/armarx/control/ethercat/Bus.cpp
+++ b/source/armarx/control/ethercat/Bus.cpp
@@ -1,5 +1,6 @@
#include "Bus.h"
+#include <algorithm>
#include <iomanip>
#include <ethercat.h>
@@ -7,8 +8,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <ArmarXCore/core/time/Clock.h>
-#include <ArmarXCore/core/time/ClockType.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
@@ -17,6 +16,7 @@
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/SlaveErrorRegistersDevice.h>
#include <armarx/control/ethercat/SlaveInterface.h>
+#include <armarx/control/ethercat/Timing.h>
#include "ErrorReporting.h"
#include "bus_io/SlaveRegisterReadingScheduler.h"
@@ -35,7 +35,7 @@ namespace armarx::control::ethercat
std::string
Bus::logErrorsToFile(std::filesystem::path path, unsigned int lastSeconds)
{
- auto ret = error::Reporting::getErrorReporting().dumpToFile(path, lastSeconds);
+ auto ret = Reporting::getErrorReporting().dumpToFile(path, lastSeconds);
if (ret.has_value())
{
return ret.value().string();
@@ -72,7 +72,8 @@ namespace armarx::control::ethercat
if (ec_slave[slave->getSlaveNumber()].outputs == nullptr ||
ec_slave[slave->getSlaveNumber()].inputs == nullptr)
{
- BUS_ERROR("There is a nullpointer in the mapping of slave %d",
+ BUS_ERROR(iterationCount,
+ "There is a nullpointer in the mapping of slave %d",
slave->getSlaveNumber());
ARMARX_IMPORTANT << "current slave" << slave->getSlaveNumber();
@@ -108,7 +109,8 @@ namespace armarx::control::ethercat
ec_errort error_type;
while (ec_poperror(&error_type))
{
- BUS_WARNING("SOEM error: %s", ec_errorToString(error_type).c_str());
+ BUS_WARNING(
+ iterationCount, "SOEM error: %s", ec_errorToString(error_type).c_str());
}
retVal = false;
}
@@ -146,7 +148,7 @@ namespace armarx::control::ethercat
ARMARX_TRACE;
// Error reporter for slave construction
- auto reporter = error::Reporting::getErrorReporting().getErrorReporter();
+ auto reporter = Reporting::getErrorReporting().getErrorReporter();
ARMARX_VERBOSE << "Constructing slaves.";
for (std::uint16_t currentSlaveIndex = 1; currentSlaveIndex <= ec_slavecount;
@@ -182,6 +184,7 @@ namespace armarx::control::ethercat
if (!maybeSid.has_value())
{
BUS_ERROR_LOCAL(reporter,
+ iterationCount,
"Failed to create slaveIdentifier from ESI for slave at index %d",
currentSlaveIndex);
continue;
@@ -196,6 +199,7 @@ namespace armarx::control::ethercat
try
{
+ BUS_INFO(iterationCount, "Number of slaveFactories is %d", slaveFactories.size());
for (SlaveFactory& factory : slaveFactories)
{
std::unique_ptr<SlaveInterface> slave = factory(slaveIdentifier);
@@ -256,8 +260,15 @@ namespace armarx::control::ethercat
return false;
}
+ BUS_INFO(iterationCount, "Number of slaves pushed into slavesView is %d", slaves.size());
+ // constructing a view of observerptrs on the slaves
+ std::transform(slaves.begin(),
+ slaves.end(),
+ std::back_inserter(slavesView),
+ [](auto& slave) { return std::experimental::make_observer(slave.get()); });
+
// New error reporter for device construction
- reporter = error::Reporting::getErrorReporting().getErrorReporter();
+ reporter = Reporting::getErrorReporting().getErrorReporter();
ARMARX_VERBOSE << "Constructing devices.";
for (const auto& [_, deviceConfig] : hwconfig.deviceConfigs)
@@ -285,6 +296,7 @@ namespace armarx::control::ethercat
else
{
BUS_WARNING_LOCAL(reporter,
+ iterationCount,
"Could not create device instance '%s' of class '%s'",
deviceInstanceName.c_str(),
deviceClassName.c_str());
@@ -293,6 +305,7 @@ namespace armarx::control::ethercat
else
{
BUS_ERROR_LOCAL(reporter,
+ iterationCount,
"No device factory found for xml-node `%s` with name `%s` in "
"hardware config.",
deviceClassName.c_str(),
@@ -302,6 +315,7 @@ namespace armarx::control::ethercat
catch (LocalException& e)
{
BUS_ERROR_LOCAL(reporter,
+ iterationCount,
"Exception during device creation. Device class: %s; Reason: %s",
deviceConfig->getName().c_str(),
e.getReason().c_str());
@@ -346,7 +360,7 @@ namespace armarx::control::ethercat
}
// New error reporter for assigning and validating
- reporter = error::Reporting::getErrorReporting().getErrorReporter();
+ reporter = Reporting::getErrorReporting().getErrorReporter();
ARMARX_VERBOSE << "Assigning slaves to devices.";
for (std::unique_ptr<SlaveInterface>& slave : slaves)
@@ -366,7 +380,8 @@ namespace armarx::control::ethercat
// Okay, just continue.
break;
case DeviceInterface::TryAssignResult::alreadyAssigned:
- BUS_FATAL_AND_THROW("Tried to assign ambiguous slave with name `%s` (idx: "
+ BUS_FATAL_AND_THROW(iterationCount,
+ "Tried to assign ambiguous slave with name `%s` (idx: "
"%u) again to device (%s) with name `%s`.",
slave->getSlaveIdentifier().getNameAsCStr(),
slave->getSlaveIdentifier().slaveIndex,
@@ -379,6 +394,7 @@ namespace armarx::control::ethercat
if (not slaveAssigned && slave->hasPDOMapping())
{
BUS_ERROR_LOCAL(reporter,
+ iterationCount,
"Could not find a device to assign following slave to:\n%s",
slave->getSlaveIdentifier().toString().c_str());
}
@@ -411,6 +427,7 @@ namespace armarx::control::ethercat
break;
case DeviceInterface::AllAssignedResult::slavesMissing:
BUS_FATAL_AND_THROW(
+ iterationCount,
"Device `%s` ("
"%s) reports not all slaves valid. Maybe a slave is missing on the bus",
device->getClassName().c_str(),
@@ -448,17 +465,30 @@ namespace armarx::control::ethercat
Bus::rtUpdateBus(bool doErrorHandling, size_t iteration)
{
ARMARX_TRACE;
+ if (iterationCount != iteration - 1)
+ {
+ BUS_WARNING(
+ iteration,
+ "Updating the bus has been skipped. The number of the last rt-thread iteration the "
+ "bus has been updated was %d and the current iteration is %d.",
+ iterationCount,
+ iteration);
+ }
iterationCount = iteration;
if (busState != EtherCATState::op)
{
- BUS_WARNING("UpdateBus can not be executed because the bus is not in op-mode");
+ BUS_WARNING(iterationCount,
+ "UpdateBus can not be executed because the bus is not in op-mode");
return false;
}
// handle emergency stop release
+ BUS_TIMING_START(bus_rtUpdateBus_isEmergencyStopActive);
bool const emergencyStopActive = rtIsEmergencyStopActive();
+ BUS_TIMING_CEND(bus_rtUpdateBus_isEmergencyStopActive, iterationCount, 0.3);
// the elmo sometimes go from emergency stop state back into fault state for a moment - the time variable is the fix for that
+ BUS_TIMING_START(bus_rtUpdateBus_recoverFromEmergencyStop)
if ((not emergencyStopActive and emergencyStopWasActivated))
{
bool recovered = true;
@@ -468,51 +498,54 @@ namespace armarx::control::ethercat
}
if (recovered)
{
- ARMARX_RT_LOGF_IMPORTANT("Recovered from STO (Safe Torque Off)");
+ BUS_WARNING(iterationCount, "Recovered from STO (Safe Torque Off)");
emergencyStopWasActivated = false;
}
}
else if (emergencyStopActive and not emergencyStopWasActivated)
{
- ARMARX_RT_LOGF_IMPORTANT("STO (Safe Torque Off) active");
+ BUS_WARNING(iterationCount, "STO (Safe Torque Off) active");
emergencyStopWasActivated = emergencyStopActive;
}
-
- const auto delay =
- armarx::core::time::Clock(armarx::core::time::ClockType::Monotonic).now() -
- busUpdateLastUpdateTime;
-
+ BUS_TIMING_CEND(bus_rtUpdateBus_recoverFromEmergencyStop, iterationCount, 0.3)
+ const auto delay = (armarx::rtNow() - busUpdateLastUpdateTime);
if (delay.toMilliSecondsDouble() > 40)
{
- ARMARX_RT_LOGF_WARN("Update bus was not called for a long time: %ld ms",
- delay.toMilliSeconds())
- .deactivateSpam(5);
+ BUS_WARNING(iterationCount,
+ "Update bus was not called for a long time: %ld ms",
+ delay.toMilliSeconds());
}
//send and receive process data
+ BUS_TIMING_START(bus_rtUpdateBus_updatePDO)
rtUpdatePDO();
- busUpdateLastUpdateTime =
- armarx::core::time::Clock(armarx::core::time::ClockType::Monotonic).now();
- ;
-
+ busUpdateLastUpdateTime = armarx::rtNow();
+ BUS_TIMING_CEND(bus_rtUpdateBus_updatePDO, iterationCount, 0.6)
// Execute every slave.
+ BUS_TIMING_START(bus_rtUpdateBus_executeSlaves)
for (std::unique_ptr<SlaveInterface>& slave : slaves)
{
slave->execute();
}
+ BUS_TIMING_CEND(bus_rtUpdateBus_executeSlaves, iterationCount, 0.3)
//error handling
if (doErrorHandling)
{
if (!emergencyStopActive)
{
+ BUS_TIMING_START(bus_rtUpdateBus_slaveErrorHandling)
errorHandler.rtHandleSlaveErrors();
+ BUS_TIMING_CEND(bus_rtUpdateBus_slaveErrorHandling, iterationCount, 0.3)
}
+ BUS_TIMING_START(bus_rtUpdateBus_busErrorHandling)
errorHandler.rtHandleBusErrors();
+ BUS_TIMING_CEND(bus_rtUpdateBus_busErrorHandling, iterationCount, 0.3)
}
// Update error registers
+ BUS_TIMING_START(bus_rtUpdateBus_updateErrorRegisters)
if (!errorHandler.hasError() && errorRegisterReadingScheduler)
{
if (errorRegisterReadingScheduler->allRegistersUpdated())
@@ -526,8 +559,10 @@ namespace armarx::control::ethercat
}
errorRegisterReadingScheduler->startReadingNextRegisters();
}
+ BUS_TIMING_CEND(bus_rtUpdateBus_updateErrorRegisters, iterationCount, 0.3)
// Update functional state
+ BUS_TIMING_START(bus_rtUpdateBus_updateFunctionalState)
if (errorHandler.isReinitializationActive())
{
functionalState = BusFunctionalState::Reinitializing;
@@ -540,6 +575,7 @@ namespace armarx::control::ethercat
{
functionalState = BusFunctionalState::Running;
}
+ BUS_TIMING_CEND(bus_rtUpdateBus_updateFunctionalState, iterationCount, 0.3)
return true;
}
@@ -549,7 +585,7 @@ namespace armarx::control::ethercat
{
if (busState == EtherCATState::init)
{
- BUS_WARNING("Bus is already shutdown");
+ BUS_WARNING(iterationCount, "Bus is already shutdown");
}
else
{
@@ -609,9 +645,9 @@ namespace armarx::control::ethercat
{EtherCATState::safeOp, &Bus::_safeOpToInit},
{EtherCATState::op, &Bus::_opToInit},
{EtherCATState::init,
- [](Bus*)
+ [](Bus* b)
{
- BUS_WARNING("Bus is already in init");
+ BUS_WARNING(b->iterationCount, "Bus is already in init");
return true;
}}};
@@ -628,9 +664,9 @@ namespace armarx::control::ethercat
{EtherCATState::safeOp, &Bus::_safeOpToPreOp},
{EtherCATState::op, &Bus::_opToPreOp},
{EtherCATState::preOp,
- [](Bus*)
+ [](Bus* b)
{
- BUS_WARNING("Bus is already in preOp");
+ BUS_WARNING(b->iterationCount, "Bus is already in preOp");
return true;
}},
};
@@ -648,9 +684,9 @@ namespace armarx::control::ethercat
{EtherCATState::preOp, &Bus::_preOpToSafeOp},
{EtherCATState::op, &Bus::_opToSafeOp},
{EtherCATState::safeOp,
- [](Bus*)
+ [](Bus* b)
{
- BUS_WARNING("Bus is already in safeOp");
+ BUS_WARNING(b->iterationCount, "Bus is already in safeOp");
return true;
}},
};
@@ -668,9 +704,9 @@ namespace armarx::control::ethercat
{EtherCATState::preOp, &Bus::_preOpToOp},
{EtherCATState::safeOp, &Bus::_safeOpToOp},
{EtherCATState::op,
- [](Bus*)
+ [](Bus* b)
{
- BUS_WARNING("Bus is already in op");
+ BUS_WARNING(b->iterationCount, "Bus is already in op");
return true;
}}};
@@ -690,13 +726,13 @@ namespace armarx::control::ethercat
if (!socketInitialized && !initSocket())
{
- BUS_ERROR("Could not init socket. Abort.");
+ BUS_ERROR(iterationCount, "Could not init socket. Abort.");
return false;
}
if (ec_config_init(FALSE) <= 0)
{
- BUS_ERROR("No slaves found on bus");
+ BUS_ERROR(iterationCount, "No slaves found on bus");
return false;
}
@@ -759,6 +795,7 @@ namespace armarx::control::ethercat
{
shutdown();
BUS_FATAL_AND_THROW(
+ iterationCount,
"Bus could not be validated. This indicates a severe hardware-error, "
"since the amount of slaves on the bus changed during the initialization "
"of the bus which can only happen if a PCB is behaving incorrectly or "
@@ -771,13 +808,13 @@ namespace armarx::control::ethercat
// Create devices and populate members.
if (!this->createDevices())
{
- BUS_ERROR("Error during slave and device creation");
+ BUS_ERROR(iterationCount, "Error during slave and device creation");
return false;
}
if (slaves.size() < 1)
{
- BUS_ERROR("There are no usable devices on the bus!");
+ BUS_ERROR(iterationCount, "There are no usable devices on the bus!");
return false;
}
ARMARX_INFO << "Devices were created";
@@ -816,6 +853,7 @@ namespace armarx::control::ethercat
if (!setPDOMappings())
{
BUS_ERROR(
+ iterationCount,
"Couldn't map the PDO, maybe the the pc is under to much load. Check if there are "
"other performance hungry programs running.\nOr just try to start again");
return false;
@@ -962,7 +1000,7 @@ namespace armarx::control::ethercat
}
ARMARX_IMPORTANT << "Used slaves: \n" << slaveInfo.str();
- busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic);
+ busUpdateLastUpdateTime = armarx::rtNow();
functionalState = BusFunctionalState::Running;
@@ -1015,6 +1053,7 @@ namespace armarx::control::ethercat
bool
Bus::changeBusState(EtherCATState state)
{
+ ARMARX_TRACE;
ARMARX_ON_SCOPE_EXIT
{
busState = readStates();
@@ -1083,7 +1122,7 @@ namespace armarx::control::ethercat
{
if (slaves.empty())
{
- BUS_WARNING("No slaves have been created. Cannot validate the bus.");
+ BUS_WARNING(iterationCount, "No slaves have been created. Cannot validate the bus.");
return false;
}
@@ -1091,13 +1130,14 @@ namespace armarx::control::ethercat
int slaveCount = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE);
if (slaveCount == EC_NOFRAME)
{
- BUS_ERROR("EC_NOFRAME when trying to get slave count.");
+ BUS_ERROR(iterationCount, "EC_NOFRAME when trying to get slave count.");
return false;
}
if (static_cast<std::uint16_t>(slaveCount) != slaves.size())
{
- BUS_ERROR("Could not validate bus! Amount of found slaves is not equal to the previous "
+ BUS_ERROR(iterationCount,
+ "Could not validate bus! Amount of found slaves is not equal to the previous "
"created slaves. (Found: %u, Expected: %u)",
slaveCount,
slaves.size());
@@ -1116,13 +1156,13 @@ namespace armarx::control::ethercat
{
if (socketInitialized)
{
- BUS_ERROR("Socket is already initialized.");
+ BUS_ERROR(iterationCount, "Socket is already initialized.");
return false;
}
if (socketFileDescriptor == -1)
{
- BUS_WARNING("socketFileDescriptor is -1 - did you forget to set it?");
+ BUS_WARNING(iterationCount, "socketFileDescriptor is -1 - did you forget to set it?");
return false;
}
@@ -1131,7 +1171,9 @@ namespace armarx::control::ethercat
ARMARX_IMPORTANT << "ec_init(" << ifname << ")";
if (!ec_init(ifname.c_str()))
{
- BUS_ERROR("Could not init EtherCAT on %s\nExecute as root\n", ifname.c_str());
+ BUS_ERROR(iterationCount,
+ "Could not init EtherCAT on %s\nExecute as root\n",
+ ifname.c_str());
return false;
}
}
@@ -1142,13 +1184,15 @@ namespace armarx::control::ethercat
//initialise SOEM, open socket
if (!ec_init_wsock(socketFileDescriptor))
{
- BUS_WARNING("No socket connection on %u\nExecute as root\n", socketFileDescriptor);
+ BUS_WARNING(iterationCount,
+ "No socket connection on %u\nExecute as root\n",
+ socketFileDescriptor);
return false;
}
}
else
{
- BUS_WARNING("Either socketFileDescriptor or ifname need to be set");
+ BUS_WARNING(iterationCount, "Either socketFileDescriptor or ifname need to be set");
return false;
}
@@ -1327,15 +1371,16 @@ namespace armarx::control::ethercat
this->hwconfig = parser.getHardwareConfig();
}
- std::vector<SlaveInterface*>
+ std::vector<std::experimental::observer_ptr<SlaveInterface>>
Bus::getSlaves() const
{
+ return slavesView;
std::vector<SlaveInterface*> slaves_raw;
- std::transform(slaves.begin(),
- slaves.end(),
- std::back_inserter(slaves_raw),
- [](auto& slave) { return slave.get(); });
- return slaves_raw;
+ // std::transform(slaves.begin(),
+ // slaves.end(),
+ // std::back_inserter(slaves_raw),
+ // [](auto& slave) { return slave.get(); });
+ // return slaves_raw;
}
const std::vector<std::shared_ptr<DeviceInterface>>&
@@ -1348,14 +1393,14 @@ namespace armarx::control::ethercat
{
}
- SlaveInterface*
+ std::experimental::observer_ptr<SlaveInterface>
Bus::getSlaveAtIndex(std::uint16_t slaveIndex) const
{
for (const std::unique_ptr<SlaveInterface>& slave : slaves)
{
if (slave->getSlaveNumber() == slaveIndex)
{
- return slave.get();
+ return std::experimental::make_observer(slave.get());
}
}
@@ -1381,7 +1426,10 @@ namespace armarx::control::ethercat
bool
Bus::rtHasError() const
{
- return errorHandler.hasError();
+ BUS_TIMING_START(bus_rtHasError)
+ const bool hasError = errorHandler.hasError();
+ BUS_TIMING_CEND(bus_rtHasError, iterationCount, 0.5)
+ return hasError;
}
void
@@ -1414,4 +1462,10 @@ namespace armarx::control::ethercat
}
}
+ std::uint64_t
+ Bus::getIterationCount() const
+ {
+ return iterationCount;
+ }
+
} // namespace armarx::control::ethercat
diff --git a/source/armarx/control/ethercat/Bus.h b/source/armarx/control/ethercat/Bus.h
index 7050324ac83ef9f6f53cf1dbe0c98ad50fe189a3..6e400c29324dbf9d8d7d9d2ff68309adbd5b220c 100644
--- a/source/armarx/control/ethercat/Bus.h
+++ b/source/armarx/control/ethercat/Bus.h
@@ -1,5 +1,5 @@
#pragma once
-
+#include <experimental/memory>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
#include <ArmarXCore/core/time/DateTime.h>
@@ -105,13 +105,13 @@ namespace armarx::control::ethercat
*/
void shutdown();
- SlaveInterface* getSlaveAtIndex(std::uint16_t slaveIndex) const;
+ std::experimental::observer_ptr<SlaveInterface> getSlaveAtIndex(std::uint16_t slaveIndex) const;
/**
* Returns all identifiied slaves on the bus
* @return
*/
- std::vector<SlaveInterface*> getSlaves() const;
+ std::vector<std::experimental::observer_ptr<SlaveInterface>> getSlaves() const;
/**
* Returns all initialized devices.
@@ -143,6 +143,9 @@ namespace armarx::control::ethercat
bool switchBusToSafeOp();
bool switchBusToOp();
+ // getter for iteration count
+ std::uint64_t getIterationCount() const;
+
private:
bool _initToPreOp();
bool _initToSafeOp();
@@ -206,6 +209,9 @@ namespace armarx::control::ethercat
/** List of all slaves */
std::vector<std::unique_ptr<SlaveInterface>> slaves;
+ /** View of all slaves that is given to the outside **/
+ std::vector<std::experimental::observer_ptr<SlaveInterface>> slavesView;
+
/** List of all devices */
std::vector<std::shared_ptr<DeviceInterface>> devices;
@@ -216,7 +222,7 @@ namespace armarx::control::ethercat
std::map<std::string, DeviceFactory> deviceFactories;
/** Current iteration count of the rtUpdateBus() function */
- size_t iterationCount = 0;
+ std::uint64_t iterationCount = 0;
std::unique_ptr<SlaveRegisterReadingScheduler> errorRegisterReadingScheduler;
diff --git a/source/armarx/control/ethercat/BusErrorHandler.cpp b/source/armarx/control/ethercat/BusErrorHandler.cpp
index 8fef778e7bd6b78c12e220b1dac08b274cc4ade1..fec81f03716b13e0bc79a4bffe01a275b217adaf 100644
--- a/source/armarx/control/ethercat/BusErrorHandler.cpp
+++ b/source/armarx/control/ethercat/BusErrorHandler.cpp
@@ -9,7 +9,10 @@
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
+#include <armarx/control/ethercat/Timing.h>
+
#include "Bus.h"
+#include "ErrorReporting.h"
#include "EtherCATState.h"
#include "SlaveInterface.h"
@@ -40,18 +43,29 @@ namespace armarx::control::ethercat
void
BusErrorHandler::rtHandleSlaveErrors()
{
- auto reporter = error::Reporting::getErrorReporting().getErrorReporter();
+ BUS_TIMING_START(bus_rtHandleSlaveErrors_getErrorReporter)
+ auto reporter = Reporting::getErrorReporting().getErrorReporter();
+ BUS_TIMING_CEND(bus_rtHandleSlaveErrors_getErrorReporter, bus->iterationCount, 0.3)
+
for (const auto& slave : bus->getSlaves())
{
//try to clear error if there exist some, the rest of the slaves can run normal
- if (slave->hasError())
+ SLAVE_TIMING_START(bus_rtHandleSlaveErrors_slaveHasError)
+ auto hasError = slave->hasError();
+ SLAVE_TIMING_CEND(
+ slave->getSlaveIdentifier(), bus_rtHandleSlaveErrors_slaveHasError, 0.3)
+ if (hasError)
{
+
+ SLAVE_TIMING_START(bus_rtHandleSlaveErrors_slaveHandleError)
if (!slave->handleErrors())
{
SLAVE_ERROR_LOCAL(
reporter, slave->getSlaveIdentifier(), "Unhandled error in slave")
.deactivateSpam(1);
}
+ SLAVE_TIMING_CEND(
+ slave->getSlaveIdentifier(), bus_rtHandleSlaveErrors_slaveHandleError, 0.3)
}
}
@@ -83,7 +97,7 @@ namespace armarx::control::ethercat
for (const auto& s : bus->getSlaves())
{
- slaveStates.insert({s, SlaveState::Operational});
+ slaveStates.insert({s.get(), SlaveState::Operational});
}
}
@@ -97,47 +111,64 @@ namespace armarx::control::ethercat
if (bus->lastWorkCounter == EC_NOFRAME && !allSlavesReinitialized.load())
{
// Panic!! Bus unsable?
- ARMARX_RT_LOGF_WARNING("EC_NOFRAME").deactivateSpam(1);
+ BUS_WARNING(bus->iterationCount,
+ "The last work counter is EC_NOFRAME not all slaves have been "
+ "reinitialized yet!");
}
else if (bus->lastWorkCounter != secondLastUnexpectedWorkCounter)
{
+ BUS_INFO(bus->iterationCount,
+ "Last Work Counter: %i, Second Last Unexpected Work Counter: %i, "
+ "Expected Work Counter: %i",
+ bus->lastWorkCounter,
+ secondLastUnexpectedWorkCounter,
+ expectedWorkCounter);
secondLastUnexpectedWorkCounter = bus->lastWorkCounter;
// Lost slaves or found slaves again or slaves are not in op
// Check for lost slaves and mark them
rtMarkLostSlavesInSOEMStruct();
+ std::stringstream lost_ss;
+ std::stringstream redisc_ss;
+ bool print_lost = false;
+ bool print_redisc = false;
+ lost_ss << "The following slaves have been lost: \n";
+ redisc_ss << "The following slaves have been rediscovered: \n";
for (const auto& slave : bus->getSlaves())
{
if (ec_slave[slave->getSlaveIdentifier().slaveIndex].islost)
{
- if (slaveStates.at(slave) != SlaveState::Lost)
+ if (slaveStates.at(slave.get()) != SlaveState::Lost)
{
- BUS_ERROR(
- "Slave at index %u of type %s was lost! Iteration count is %u",
- slave->getSlaveIdentifier().slaveIndex,
- slave->getSlaveIdentifier().getNameAsCStr(),
- bus->iterationCount);
+ lost_ss << slave->getSlaveIdentifier().getNameAsCStr() << "("
+ << slave->getSlaveIdentifier().slaveIndex << ")\n";
busErrorFound = true;
+ print_lost = true;
}
- slaveStates.at(slave) = SlaveState::Lost;
+ slaveStates.at(slave.get()) = SlaveState::Lost;
allSlavesReinitialized.store(false);
}
else
{
- if (slaveStates.at(slave) == SlaveState::Lost)
+ if (slaveStates.at(slave.get()) == SlaveState::Lost)
{
- slaveStates.at(slave) = SlaveState::Reinitializating;
-
- ARMARX_RT_LOGF_IMPORTANT(
- "Slave at index %u of type %s has been found again and needs "
- "reinitialization!",
- slave->getSlaveIdentifier().slaveIndex,
- slave->getSlaveIdentifier().getNameAsCStr());
+ slaveStates.at(slave.get()) = SlaveState::Reinitializating;
+ print_redisc = true;
+ redisc_ss << slave->getSlaveIdentifier().getNameAsCStr() << "("
+ << slave->getSlaveIdentifier().slaveIndex << ")\n";
}
}
}
+ if (print_lost)
+ {
+ BUS_ERROR(bus->iterationCount, lost_ss.str().c_str());
+ }
+ if (print_redisc)
+ {
+ BUS_INFO(bus->iterationCount, redisc_ss.str().c_str());
+ }
// Read bus states and add all slaves that need reinitialization to list
if (bus->readStates() != EtherCATState::invalid)
@@ -147,12 +178,13 @@ namespace armarx::control::ethercat
if (EtherCATState(
ec_slave[slave->getSlaveIdentifier().slaveIndex].state) !=
EtherCATState::op &&
- !ec_slave[slave->getSlaveIdentifier().slaveIndex].islost &&
- (slaveStates.at(slave) != SlaveState::Reinitializating))
+ ec_slave[slave->getSlaveIdentifier().slaveIndex].islost == 0 &&
+ (slaveStates.at(slave.get()) != SlaveState::Reinitializating))
{
- slaveStates.at(slave) = SlaveState::Reinitializating;
+ slaveStates.at(slave.get()) = SlaveState::Reinitializating;
- BUS_ERROR("Slave at index %u of type %s needs reinitialization!",
+ BUS_ERROR(bus->iterationCount,
+ "Slave at index %u of type %s needs reinitialization!",
slave->getSlaveIdentifier().slaveIndex,
slave->getSlaveIdentifier().getNameAsCStr());
}
@@ -175,7 +207,7 @@ namespace armarx::control::ethercat
BusErrorHandler::rtMarkLostSlavesInSOEMStruct() const
{
std::uint16_t w = 0;
- int slaveCount = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE);
+ const int slaveCount = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE);
if (slaveCount == EC_NOFRAME)
{
return;
@@ -183,6 +215,10 @@ namespace armarx::control::ethercat
if (slaveCount < ec_slavecount)
{
+ BUS_WARNING(bus->iterationCount,
+ "Number of Slaves (%i) in SOEM struct is less than configured Slaves (%i).",
+ slaveCount,
+ ec_slavecount);
// Create RegisterDataList with request to read register CONFIGURED_STATION_ADDRESS
std::vector<RegisterDataList> registers;
for (auto i = 1; i <= ec_slavecount; i++)
@@ -197,6 +233,8 @@ namespace armarx::control::ethercat
bus->readRegisters(registers);
// Iterate over read registers and check which slaves did not answer
+ std::stringstream ss;
+ ss << "Slaves with the following indexes are not reachable: \n";
for (const auto& dataList : registers)
{
auto data =
@@ -209,9 +247,13 @@ namespace armarx::control::ethercat
}
else
{
+ ss << dataList.slaveIndex << ", ";
ec_slave[dataList.slaveIndex].islost = true;
}
}
+ ss.seekp(ss.str().length() - 2);
+ ss << "\n The total number of registers read is " << registers.size() << "\n";
+ BUS_ERROR(bus->iterationCount, "%s", ss.str().c_str());
}
else
{
@@ -257,7 +299,8 @@ namespace armarx::control::ethercat
}
else
{
- BUS_ERROR("Could not recover slave: %s",
+ BUS_ERROR(bus->iterationCount,
+ "Could not recover slave: %s",
slave->getSlaveIdentifier().getNameAsCStr());
}
@@ -274,11 +317,11 @@ namespace armarx::control::ethercat
{
Bus& bus = Bus::getBus();
- SlaveInterface* slave = bus.getSlaveAtIndex(index);
+ auto localSlave = bus.getSlaveAtIndex(index);
- slave->doMappings();
- slave->prepareForSafeOp();
- slave->finishPreparingForSafeOp();
+ localSlave->doMappings();
+ localSlave->prepareForSafeOp();
+ localSlave->finishPreparingForSafeOp();
return 0;
};
diff --git a/source/armarx/control/ethercat/CMakeLists.txt b/source/armarx/control/ethercat/CMakeLists.txt
index 60df54f48111da9e6f61b17e408cf185ed99a147..5e9abd888e2f20027ca8c367ea94b987a11c9339 100644
--- a/source/armarx/control/ethercat/CMakeLists.txt
+++ b/source/armarx/control/ethercat/CMakeLists.txt
@@ -37,6 +37,7 @@ armarx_add_library(ethercat
ErrorReporting.h
SlaveErrorRegistersDevice.h
RTUtility.h
+ Timing.h
bus_io/Timeouts.h
bus_io/BusIO.h
bus_io/ESI.h
diff --git a/source/armarx/control/ethercat/ErrorReporting.cpp b/source/armarx/control/ethercat/ErrorReporting.cpp
index bc8e5205f6d90e002e96e874e2a59805812c0563..ebe477ff0fadad14d20aec30df5cf63217d85f0c 100644
--- a/source/armarx/control/ethercat/ErrorReporting.cpp
+++ b/source/armarx/control/ethercat/ErrorReporting.cpp
@@ -5,23 +5,27 @@
#include <boost/lockfree/policies.hpp>
#include <boost/lockfree/queue.hpp>
+#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
-namespace armarx::control::ethercat::error
+namespace armarx::control::ethercat::reporting
{
std::ostream&
operator<<(std::ostream& stream, const Type& rhs)
{
switch (rhs)
{
- case Type::BusError:
- stream << "BusError";
+ case Type::Bus:
+ stream << "Bus";
break;
- case Type::SlaveError:
- stream << "SlaveError";
+ case Type::Slave:
+ stream << "Slave";
break;
- case Type::DeviceError:
- stream << "DeviceError";
+ case Type::Device:
+ stream << "Device";
+ break;
+ case Type::General:
+ stream << "General";
break;
}
return stream;
@@ -32,8 +36,11 @@ namespace armarx::control::ethercat::error
{
switch (rhs)
{
- case Severity::None:
- stream << "None";
+ case Severity::Info:
+ stream << "INFO";
+ break;
+ case Severity::Debug:
+ stream << "DEBUG";
break;
case Severity::Warning:
stream << "WARNING";
@@ -50,7 +57,7 @@ namespace armarx::control::ethercat::error
struct Reporting::QueueImpl
{
- static constexpr size_t queueSize = 20;
+ static constexpr size_t queueSize = 100;
boost::lockfree::queue<Entry, boost::lockfree::capacity<queueSize>> queue;
};
@@ -58,9 +65,9 @@ namespace armarx::control::ethercat::error
Reporting::Reporting(std::size_t maxErrorHistorySize) :
pimpl(std::make_unique<QueueImpl>()), m_maxErrorHistorySize(maxErrorHistorySize)
{
- setTag("ethercat::ErrorReporting");
+ setTag("ethercat::Reporting");
- m_timestampReportingStart = IceUtil::Time::now();
+ m_timestampReportingStart = armarx::rtNow();
m_errorProcessingThread = std::thread(&Reporting::errorProcessingLoop, this);
}
@@ -94,8 +101,12 @@ namespace armarx::control::ethercat::error
void
Reporting::report(Entry&& error)
{
- error.m_timestamp = IceUtil::Time::now();
- pimpl->queue.push(error);
+ error.m_timestamp = armarx::rtNow();
+ bool result = pimpl->queue.push(error);
+ if (not result)
+ {
+ ARMARX_RT_LOGF_WARNING("Logging queue is full!").deactivateSpam(0.1);
+ }
m_errorProcessingCondition.notify_all();
}
@@ -108,16 +119,16 @@ namespace armarx::control::ethercat::error
std::optional<std::filesystem::path>
Reporting::dumpToFile(std::filesystem::path file, std::uint64_t lastNSeconds)
{
- IceUtil::Time endTime = IceUtil::Time::now();
+ const auto endTime = armarx::rtNow();
IceUtil::Time startTime =
endTime - IceUtil::Time::seconds(static_cast<std::int64_t>(lastNSeconds));
if (startTime < m_timestampReportingStart)
{
startTime = m_timestampReportingStart;
}
- std::string yearString = startTime.toString("%y-%m-%d");
- std::string startTimeString = startTime.toString("%H-%M-%S");
- std::string endTimeString = endTime.toString("%H-%M-%S");
+ const std::string yearString = startTime.toString("%y-%m-%d");
+ const std::string startTimeString = startTime.toString("%H-%M-%S");
+ const std::string endTimeString = endTime.toString("%H-%M-%S");
file.replace_filename(std::string(file.stem().c_str()) + "_" + yearString + "__" +
startTimeString + "_" + endTimeString);
@@ -192,7 +203,7 @@ namespace armarx::control::ethercat::error
{
while (pimpl->queue.pop(e))
{
- std::int64_t timestamp = e.m_timestamp.toMicroSeconds();
+ const std::int64_t timestamp = e.m_timestamp.toMicroSeconds();
std::uint16_t i = 0;
// Check for max size of history
@@ -213,50 +224,38 @@ namespace armarx::control::ethercat::error
m_errorHistory.insert({{timestamp, i}, e});
- std::stringstream currentTag;
- currentTag << "EtherCAT ";
- switch (e.m_type)
- {
- case Type::BusError:
- currentTag << "Bus";
- break;
- case Type::SlaveError:
- currentTag << "Slave #";
- currentTag << e.m_sid.slaveIndex;
- break;
- case Type::DeviceError:
- currentTag << "Device ";
- currentTag << e.m_device_name;
- break;
- }
-
auto logpointer =
(loghelper(e.m_metaInfo.file, e.m_metaInfo.line, e.m_metaInfo.function));
switch (e.m_severity)
{
- case error::Severity::None:
- *logpointer->setBacktrace(false)->setTag(currentTag.str())
+ case reporting::Severity::Info:
+ *logpointer->setBacktrace(false)
<< ::armarx::MessageTypeT::INFO << deactivateSpam(1.f, e.toString())
<< e.toString();
return;
- case error::Severity::Warning:
- *logpointer->setBacktrace(false)->setTag(currentTag.str())
- << ::armarx::MessageTypeT::WARN
+ case reporting::Severity::Debug:
+ *logpointer->setBacktrace(false)
+ << ::armarx::MessageTypeT::DEBUG
<< deactivateSpam(e.m_deactivate_spam_seconds, e.toString())
<< e.toString();
return;
- case error::Severity::Error:
- *logpointer->setBacktrace(false)->setTag(currentTag.str())
- << ::armarx::MessageTypeT::ERROR
+ case reporting::Severity::Warning:
+ *logpointer->setBacktrace(false)
+ << ::armarx::MessageTypeT::WARN
<< deactivateSpam(e.m_deactivate_spam_seconds, e.toString())
<< e.toString();
return;
- case error::Severity::Fatal:
- *logpointer->setTag(currentTag.str())
- << ::armarx::MessageTypeT::FATAL
+ case reporting::Severity::Error:
+ *logpointer->setBacktrace(false)
+ << ::armarx::MessageTypeT::ERROR
<< deactivateSpam(e.m_deactivate_spam_seconds, e.toString())
<< e.toString();
return;
+ case reporting::Severity::Fatal:
+ *logpointer << ::armarx::MessageTypeT::FATAL
+ << deactivateSpam(e.m_deactivate_spam_seconds, e.toString())
+ << e.toString();
+ return;
}
}
};
@@ -273,6 +272,20 @@ namespace armarx::control::ethercat::error
handleEntries();
}
+ Reporter::ReportWrapper
+ Reporter::reportDebug(Entry& info)
+ {
+ info.m_severity = Severity::Debug;
+ return Reporter::ReportWrapper{info, this};
+ }
+
+ Reporter::ReportWrapper
+ Reporter::reportInfo(Entry& info)
+ {
+ info.m_severity = Severity::Info;
+ return Reporter::ReportWrapper{info, this};
+ }
+
Reporter::ReportWrapper
Reporter::reportWarning(Entry& error)
{
@@ -309,14 +322,14 @@ namespace armarx::control::ethercat::error
}
Entry&
- Error::slaveIdentifier(SlaveIdentifier sid)
+ Entry::slaveIdentifier(SlaveIdentifier sid)
{
m_sid = sid;
return *this;
}
Entry&
- Error::errorType(Type type)
+ Entry::errorType(Type type)
{
m_type = type;
return *this;
@@ -352,10 +365,22 @@ namespace armarx::control::ethercat::error
{
ss << " " << std::left << std::setw(16) << "Name: " << std::string(m_device_name);
}
+ if (m_bus_iteration_number != 0)
+ {
+ ss << " " << std::left << std::setw(16)
+ << "Bus Iteration: " << std::to_string(m_bus_iteration_number);
+ }
ss << "\n";
return ss.str();
}
+ Entry&
+ Entry::busIterationNumber(std::uint64_t iteration)
+ {
+ m_bus_iteration_number = iteration;
+ return *this;
+ }
+
Reporter::ReportWrapper::ReportWrapper(Entry& error, Reporter* reporter) :
m_error(error), m_reporter(reporter)
{
@@ -373,4 +398,4 @@ namespace armarx::control::ethercat::error
return *this;
}
-} // namespace armarx::control::ethercat::error
+} // namespace armarx::control::ethercat::reporting
diff --git a/source/armarx/control/ethercat/ErrorReporting.h b/source/armarx/control/ethercat/ErrorReporting.h
index 573ce1d90062b5b3e7c08a3738509c31009ee23a..b6abf0a85f6134ec7d8b79e905b3898de4dbce38 100644
--- a/source/armarx/control/ethercat/ErrorReporting.h
+++ b/source/armarx/control/ethercat/ErrorReporting.h
@@ -10,8 +10,6 @@
#include <optional>
#include <thread>
-#include <IceUtil/Time.h>
-
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/PropagateConst.h>
@@ -22,7 +20,7 @@
* @ingroup Library-ethercat
* A description of the namespace error.
*/
-namespace armarx::control::ethercat::error
+namespace armarx::control::ethercat::reporting
{
/**
* @enum Type
@@ -32,9 +30,10 @@ namespace armarx::control::ethercat::error
*/
enum class Type
{
- BusError, //!< Bussdfnödf
- SlaveError, //!< sdfluieb,dj
- DeviceError
+ Bus, //!< Bussdfnödf
+ Slave, //!< sdfluieb,dj
+ Device,
+ General
};
std::ostream& operator<<(std::ostream& stream, const Type& rhs);
@@ -47,7 +46,8 @@ namespace armarx::control::ethercat::error
*/
enum class Severity
{
- None,
+ Debug,
+ Info,
Warning,
Error,
Fatal
@@ -69,7 +69,7 @@ namespace armarx::control::ethercat::error
{
static constexpr std::uint16_t DEVICE_NAME_BUFFER_SIZE = 64;
- static constexpr std::uint16_t MESSAGE_BUFFER_SIZE = 512;
+ static constexpr std::uint16_t MESSAGE_BUFFER_SIZE = 1024;
static constexpr char OVERFLOW_MESSAGE[] = {"[ERROR MESSAGE TOO LONG]"};
static_assert(MESSAGE_BUFFER_SIZE > sizeof(OVERFLOW_MESSAGE) + 1);
@@ -86,6 +86,7 @@ namespace armarx::control::ethercat::error
char m_device_name[DEVICE_NAME_BUFFER_SIZE];
Severity m_severity;
IceUtil::Time m_timestamp;
+ std::uint64_t m_bus_iteration_number = 0;
float m_deactivate_spam_seconds = 0;
struct MetaInfo
@@ -94,6 +95,7 @@ namespace armarx::control::ethercat::error
int line;
char function[FUNCTION_STRING_BUFFER_SIZE];
};
+
MetaInfo m_metaInfo;
public:
@@ -125,6 +127,7 @@ namespace armarx::control::ethercat::error
Entry& errorType(Type type);
Entry& metaInfo(const char* file, int line, const char* function);
Entry& deviceName(const char* deviceName);
+ Entry& busIterationNumber(std::uint64_t iteration);
std::string toString() const;
};
@@ -156,6 +159,8 @@ namespace armarx::control::ethercat::error
};
public:
+ ReportWrapper reportDebug(Entry& error);
+ ReportWrapper reportInfo(Entry& error);
ReportWrapper reportWarning(Entry& error);
ReportWrapper reportError(Entry& error);
[[noreturn]] void reportErrorAndThrow(Entry& error);
@@ -202,6 +207,7 @@ namespace armarx::control::ethercat::error
{
std::int64_t timestamp;
std::uint16_t subid;
+
bool
operator<(const ErrorTimestamp& x) const
{
@@ -220,80 +226,117 @@ namespace armarx::control::ethercat::error
IceUtil::Time m_timestampReportingStart;
};
-} // namespace armarx::control::ethercat::error
+
+} // namespace armarx::control::ethercat::reporting
namespace armarx::control::ethercat
{
- using Error = error::Entry;
- using ErrorType = error::Type;
+ using ReportingEntry = reporting::Entry;
+ using ReportingType = reporting::Type;
+ using Reporting = reporting::Reporting;
-#define _detail_BUS_ERROR_CONSTRUCTION(...) \
- armarx::control::ethercat::Error() \
- .errorType(armarx::control::ethercat::ErrorType::BusError) \
+////////////////////////////////////////////////////////////////////////////////////////////////////
+#define _detail_GENERAL_REPORT_CONSTRUCTION(...) \
+ armarx::control::ethercat::ReportingEntry() \
+ .errorType(armarx::control::ethercat::ReportingType::General) \
.metaInfo(__FILE__, __LINE__, ARMARX_FUNCTION) \
.message(__VA_ARGS__)
-#define BUS_WARNING(...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportWarning( \
- _detail_BUS_ERROR_CONSTRUCTION(__VA_ARGS__))
-#define BUS_ERROR(...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportError( \
- _detail_BUS_ERROR_CONSTRUCTION(__VA_ARGS__))
-#define BUS_FATAL_AND_THROW(...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportErrorAndThrow( \
- _detail_BUS_ERROR_CONSTRUCTION(__VA_ARGS__))
-
-#define BUS_WARNING_LOCAL(reporter, ...) \
- (reporter).reportWarning(_detail_BUS_ERROR_CONSTRUCTION(__VA_ARGS__))
-#define BUS_ERROR_LOCAL(reporter, ...) \
- (reporter).reportError(_detail_BUS_ERROR_CONSTRUCTION(__VA_ARGS__))
-
-
-#define _detail_SLAVE_ERROR_CONSTRUCTION(_sid, ...) \
- armarx::control::ethercat::Error() \
- .errorType(armarx::control::ethercat::ErrorType::SlaveError) \
+#define GENERAL_DEBUG(...) \
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportDebug( \
+ _detail_GENERAL_REPORT_CONSTRUCTION(__VA_ARGS__))
+#define GENERAL_INFO(...) \
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportInfo( \
+ _detail_GENERAL_REPORT_CONSTRUCTION(__VA_ARGS__))
+#define GENERAL_WARNING(...) \
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportWarning( \
+ _detail_GENERAL_REPORT_CONSTRUCTION(__VA_ARGS__))
+#define GENERAL_ERROR(...) \
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportError( \
+ _detail_GENERAL_REPORT_CONSTRUCTION(__VA_ARGS__))
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+#define _detail_BUS_REPORT_CONSTRUCTION(bin, ...) \
+ armarx::control::ethercat::ReportingEntry() \
+ .errorType(armarx::control::ethercat::ReportingType::Bus) \
+ .metaInfo(__FILE__, __LINE__, ARMARX_FUNCTION) \
+ .message(__VA_ARGS__) \
+ .busIterationNumber(bin)
+
+#define BUS_DEBUG(bin, ...) \
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportDebug( \
+ _detail_BUS_REPORT_CONSTRUCTION(bin, __VA_ARGS__))
+#define BUS_INFO(bin, ...) \
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportInfo( \
+ _detail_BUS_REPORT_CONSTRUCTION(bin, __VA_ARGS__))
+#define BUS_WARNING(bin, ...) \
+ armarx::control::ethercat::reporting::Reporting::getGlobalErrorReporter().reportWarning( \
+ _detail_BUS_REPORT_CONSTRUCTION(bin, __VA_ARGS__))
+#define BUS_ERROR(bin, ...) \
+ armarx::control::ethercat::reporting::Reporting::getGlobalErrorReporter().reportError( \
+ _detail_BUS_REPORT_CONSTRUCTION(bin, __VA_ARGS__))
+#define BUS_FATAL_AND_THROW(bin, ...) \
+ armarx::control::ethercat::reporting::Reporting::getGlobalErrorReporter().reportErrorAndThrow( \
+ _detail_BUS_REPORT_CONSTRUCTION(bin, __VA_ARGS__))
+
+#define BUS_WARNING_LOCAL(reporter, bin, ...) \
+ (reporter).reportWarning(_detail_BUS_REPORT_CONSTRUCTION(bin, __VA_ARGS__))
+#define BUS_ERROR_LOCAL(reporter, bin, ...) \
+ (reporter).reportError(_detail_BUS_REPORT_CONSTRUCTION(bin, __VA_ARGS__))
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+#define _detail_SLAVE_REPORT_CONSTRUCTION(_sid, ...) \
+ armarx::control::ethercat::ReportingEntry() \
+ .errorType(armarx::control::ethercat::ReportingType::Slave) \
.metaInfo(__FILE__, __LINE__, ARMARX_FUNCTION) \
.message(__VA_ARGS__) \
.slaveIdentifier(_sid)
+#define SLAVE_DEBUG(sid, ...) \
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportDebug( \
+ _detail_SLAVE_REPORT_CONSTRUCTION(sid, __VA_ARGS__))
+#define SLAVE_INFO(sid, ...) \
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportInfo( \
+ _detail_SLAVE_REPORT_CONSTRUCTION(sid, __VA_ARGS__))
#define SLAVE_WARNING(sid, ...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportWarning( \
- _detail_SLAVE_ERROR_CONSTRUCTION(sid, __VA_ARGS__))
+ armarx::control::ethercat::reporting::Reporting::getGlobalErrorReporter().reportWarning( \
+ _detail_SLAVE_REPORT_CONSTRUCTION(sid, __VA_ARGS__))
#define SLAVE_ERROR(sid, ...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportError( \
- _detail_SLAVE_ERROR_CONSTRUCTION(sid, __VA_ARGS__))
+ armarx::control::ethercat::reporting::Reporting::getGlobalErrorReporter().reportError( \
+ _detail_SLAVE_REPORT_CONSTRUCTION(sid, __VA_ARGS__))
#define SLAVE_FATAL_AND_THROW(sid, ...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportErrorAndThrow( \
- _detail_SLAVE_ERROR_CONSTRUCTION(sid, __VA_ARGS__))
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportErrorAndThrow( \
+ _detail_SLAVE_REPORT_CONSTRUCTION(sid, __VA_ARGS__))
#define SLAVE_WARNING_LOCAL(reporter, sid, ...) \
- (reporter).reportWarning(_detail_SLAVE_ERROR_CONSTRUCTION(sid, __VA_ARGS__))
+ (reporter).reportWarning(_detail_SLAVE_REPORT_CONSTRUCTION(sid, __VA_ARGS__))
#define SLAVE_ERROR_LOCAL(reporter, sid, ...) \
- (reporter).reportError(_detail_SLAVE_ERROR_CONSTRUCTION(sid, __VA_ARGS__))
-
+ (reporter).reportError(_detail_SLAVE_REPORT_CONSTRUCTION(sid, __VA_ARGS__))
-#define _detail_DEVICE_ERROR_CONSTRUCTION(devName, ...) \
- armarx::control::ethercat::Error() \
- .errorType(armarx::control::ethercat::ErrorType::DeviceError) \
+////////////////////////////////////////////////////////////////////////////////////////////////////
+#define _detail_DEVICE_REPORT_CONSTRUCTION(devName, ...) \
+ armarx::control::ethercat::ReportingEntry() \
+ .errorType(armarx::control::ethercat::ReportingType::Device) \
.metaInfo(__FILE__, __LINE__, ARMARX_FUNCTION) \
.message(__VA_ARGS__) \
.deviceName(devName)
#define DEVICE_WARNING(deviceName, ...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportWarning( \
- _detail_DEVICE_ERROR_CONSTRUCTION(deviceName, __VA_ARGS__))
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportWarning( \
+ _detail_DEVICE_REPORT_CONSTRUCTION(deviceName, __VA_ARGS__))
#define DEVICE_ERROR(deviceName, ...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportError( \
- _detail_DEVICE_ERROR_CONSTRUCTION(deviceName, __VA_ARGS__))
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportError( \
+ _detail_DEVICE_REPORT_CONSTRUCTION(deviceName, __VA_ARGS__))
#define DEVICE_FATAL_AND_THROW(deviceName, ...) \
- armarx::control::ethercat::error::Reporting::getGlobalErrorReporter().reportErrorAndThrow( \
- _detail_DEVICE_ERROR_CONSTRUCTION(deviceName, __VA_ARGS__))
+ armarx::control::ethercat::Reporting::getGlobalErrorReporter().reportErrorAndThrow( \
+ _detail_DEVICE_REPORT_CONSTRUCTION(deviceName, __VA_ARGS__))
#define DEVICE_WARNING_LOCAL(reporter, deviceName, ...) \
- (reporter).reportWarning(_detail_DEVICE_ERROR_CONSTRUCTION(deviceName, __VA_ARGS__))
+ (reporter).reportWarning(_detail_DEVICE_REPORT_CONSTRUCTION(deviceName, __VA_ARGS__))
#define DEVICE_ERROR_LOCAL(reporter, deviceName, ...) \
- (reporter).reportError(_detail_DEVICE_ERROR_CONSTRUCTION(deviceName, __VA_ARGS__))
+ (reporter).reportError(_detail_DEVICE_REPORT_CONSTRUCTION(deviceName, __VA_ARGS__))
} // namespace armarx::control::ethercat
diff --git a/source/armarx/control/ethercat/RTUnit.cpp b/source/armarx/control/ethercat/RTUnit.cpp
index a4d13a4ceba73fc695be066f959b74f960c51a46..20e78229b702def28a7f8e804c56d28ece223dc9 100644
--- a/source/armarx/control/ethercat/RTUnit.cpp
+++ b/source/armarx/control/ethercat/RTUnit.cpp
@@ -53,6 +53,7 @@
#include <armarx/control/ethercat/RTUtility.h>
#include <armarx/control/ethercat/SlaveErrorRegistersDevice.h>
#include <armarx/control/ethercat/SlaveInterface.h>
+#include <armarx/control/ethercat/Timing.h>
namespace armarx::control::ethercat
{
@@ -107,6 +108,7 @@ namespace armarx::control::ethercat
Bus& bus = Bus::getBus();
bus.setRobot(robot);
bus.setLocalMinimumLoggingLevel(this->getEffectiveLoggingLevel());
+ bus.setTimeouts(ethercatTimeouts); // Set configured timeouts.
}
void
@@ -267,6 +269,19 @@ namespace armarx::control::ethercat
def->optional(properties.robotJointsNodeSet, "nodeSet.robotJoints", "The node set which contains the actuated joints.");
def->optional(properties.robotColNodeSet, "nodeSet.robotCol", "The node set for e.g. gravity computation.");
+ def->optional(ethercatTimeouts.registerRead_us,
+ "ethercat.timeouts.register_read",
+ "EtherCAT timeout for a register frame to read certain registers in µs.");
+ def->optional(ethercatTimeouts.stateCheck_us,
+ "ethercat.timeouts.state_check",
+ "EtherCAT timeout to check for the current EtherCAT state of slaves in µs.");
+ def->optional(ethercatTimeouts.sdoRead_us,
+ "ethercat.timeouts.sdo_read",
+ "EtherCAT timeout to read an SDO in µs.");
+ def->optional(ethercatTimeouts.sdoWrite_us,
+ "ethercat.timeouts.sdo_write",
+ "EtherCAT timeout to write an SDO in µs.");
+
return def;
}
@@ -563,7 +578,7 @@ namespace armarx::control::ethercat
if (!bus.switchBusToOp())
{
- BUS_FATAL_AND_THROW("Switching bus to EtherCAT state 'operational' failed");
+ BUS_FATAL_AND_THROW(bus.getIterationCount(), "Switching bus to EtherCAT state 'operational' failed");
}
@@ -584,42 +599,75 @@ namespace armarx::control::ethercat
std::clamp<long>(realDeltaT.toMicroSeconds(), 1, 2000)); // TODO: Make property
if (rtIsCalibrating())
{
+ BUS_TIMING_START(calibrateActivateControlers)
rtCalibrateActivateControlers();
+ BUS_TIMING_CEND(calibrateActivateControlers,
+ bus.getIterationCount(),
+ 0.3 * loopDurationThreshold)
+
+ BUS_TIMING_START(switchControllerSetup)
rtSwitchControllerSetup(SwitchControllerMode::RTThread);
+ BUS_TIMING_CEND(
+ switchControllerSetup, bus.getIterationCount(), 0.3 * loopDurationThreshold)
+
+ BUS_TIMING_START(resetAllTargets)
rtResetAllTargets();
+ BUS_TIMING_CEND(
+ resetAllTargets, bus.getIterationCount(), 0.3 * loopDurationThreshold)
+
+ BUS_TIMING_START(calibrateInRt)
_calibrationStatus = rtCalibrate();
+ BUS_TIMING_CEND(
+ calibrateInRt, bus.getIterationCount(), 0.3 * loopDurationThreshold)
+
+ BUS_TIMING_START(handleInvalidTargets)
rtHandleInvalidTargets();
+ BUS_TIMING_CEND(
+ handleInvalidTargets, bus.getIterationCount(), 0.3 * loopDurationThreshold)
+
+ BUS_TIMING_START(runJointControllers)
rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
+ BUS_TIMING_CEND(
+ runJointControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
}
else
{
- RT_TIMING_START(RunControllers)
- RT_TIMING_START(SwitchControllers)
+ BUS_TIMING_START(RunControllers)
+ BUS_TIMING_START(SwitchControllers)
rtSwitchControllerSetup();
- RT_TIMING_CEND(SwitchControllers, 0.3 * loopDurationThreshold)
- RT_TIMING_START(ResettingTargets)
+ BUS_TIMING_CEND(
+ SwitchControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
+ BUS_TIMING_START(ResettingTargets)
rtResetAllTargets();
- RT_TIMING_CEND(ResettingTargets, 0.3 * loopDurationThreshold)
- RT_TIMING_START(RunNJointControllers)
+ BUS_TIMING_CEND(
+ ResettingTargets, bus.getIterationCount(), 0.3 * loopDurationThreshold)
+ BUS_TIMING_START(RunNJointControllers)
rtRunNJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(RunNJointControllers, 0.3 * loopDurationThreshold)
- RT_TIMING_START(CheckInvalidTargets)
+ BUS_TIMING_CEND(
+ RunNJointControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
+ BUS_TIMING_START(CheckInvalidTargets)
rtHandleInvalidTargets();
- RT_TIMING_CEND(CheckInvalidTargets, 0.3 * loopDurationThreshold)
+ BUS_TIMING_CEND(
+ CheckInvalidTargets, bus.getIterationCount(), 0.3 * loopDurationThreshold)
- RT_TIMING_START(RunJointControllers)
+ BUS_TIMING_START(RunJointControllers)
rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(RunJointControllers, 0.3 * loopDurationThreshold)
- RT_TIMING_CEND(RunControllers, 0.3 * loopDurationThreshold)
+ BUS_TIMING_CEND(
+ RunJointControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
+ BUS_TIMING_CEND(
+ RunControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
}
//bus update
+ BUS_TIMING_START(rtBusSendReceive)
rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveStart();
- RT_TIMING_START(updateBus)
if (bus.rtGetEtherCATState() == EtherCATState::op)
{
// error correction
+ BUS_TIMING_START(getEmergencyStopState)
auto currentSoftwareEmergencyStopState = rtGetEmergencyStopState();
+ BUS_TIMING_CEND(
+ getEmergencyStopState, bus.getIterationCount(), 0.5 * loopDurationThreshold)
if (lastSoftwareEmergencyStopState ==
EmergencyStopState::eEmergencyStopActive &&
currentSoftwareEmergencyStopState ==
@@ -629,28 +677,31 @@ namespace armarx::control::ethercat
}
lastSoftwareEmergencyStopState = currentSoftwareEmergencyStopState;
+ BUS_TIMING_START(rtUpdateBus)
bus.rtUpdateBus(true, _iterationCount);
+ BUS_TIMING_CEND(
+ rtUpdateBus, bus.getIterationCount(), 0.6 * loopDurationThreshold)
if (bus.rtIsEmergencyStopActive() || bus.rtHasError())
{
rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
}
}
- currentPDOUpdateTimestamp = TimeUtil::GetTime();
- RT_TIMING_CEND(updateBus, 0.7 * loopDurationThreshold)
+ currentPDOUpdateTimestamp = armarx::rtNow();
rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveEnd();
-
- RT_TIMING_START(ReadSensorValues)
+ BUS_TIMING_CEND(rtBusSendReceive, bus.getIterationCount(), 0.55)
+ BUS_TIMING_START(ReadSensorValues)
rtReadSensorDeviceValues(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(ReadSensorValues, 0.7 * loopDurationThreshold)
+ BUS_TIMING_CEND(
+ ReadSensorValues, bus.getIterationCount(), 0.7 * loopDurationThreshold)
lastMonoticTimestamp = currentMonotonicTimestamp;
currentMonotonicTimestamp = armarx::rtNow();
- RT_TIMING_START(Publish)
+ BUS_TIMING_START(Publish)
rtUpdateSensorAndControlBuffer(currentPDOUpdateTimestamp, cappedDeltaT);
- RT_TIMING_CEND(Publish, 0.15 * loopDurationThreshold)
+ BUS_TIMING_CEND(Publish, bus.getIterationCount(), 0.15 * loopDurationThreshold)
- RT_TIMING_START(ComputeGravityTorques)
+ BUS_TIMING_START(ComputeGravityTorques)
gravity.computeGravityTorque(gravityValues);
size_t i = 0;
for (auto& node : nodeJointDataVec)
@@ -663,15 +714,16 @@ namespace armarx::control::ethercat
i++;
}
- RT_TIMING_CEND(ComputeGravityTorques, 0.2 * loopDurationThreshold)
+ BUS_TIMING_CEND(
+ ComputeGravityTorques, bus.getIterationCount(), 0.2 * loopDurationThreshold)
rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopPreSleep();
const auto loopPreSleepTime = armarx::rtNow();
- RT_TIMING_START(RTLoopWaiting)
- [[maybe_unused]] auto diff = cycleKeeper.waitForCycleDuration();
- RT_TIMING_CEND(RTLoopWaiting, loopDurationThreshold)
+ BUS_TIMING_START(RTLoopWaiting)
+ cycleKeeper.waitForCycleDuration();
+ BUS_TIMING_CEND(RTLoopWaiting, bus.getIterationCount(), loopDurationThreshold)
const auto loopPostSleepTime = armarx::rtNow();
const auto loopDuration = armarx::rtNow() - loopStartTime;
@@ -686,67 +738,51 @@ namespace armarx::control::ethercat
.getSensorValue()
->asA<SensorValueRTThreadTimings>();
ARMARX_CHECK_NOT_NULL(sval);
- ARMARX_WARNING
- << "RT loop is under 1kHz control frequency:\n"
- << "-- cycle time PDO timestamp " << realDeltaT.toMicroSeconds() << " µs\n"
- << "-- cycle time loop " << loopDuration.toMicroSeconds()
- << " µs\n"
- << "-- sleep "
- << (loopPostSleepTime - loopPreSleepTime).toMicroSecondsDouble() << " µs\n"
-
- << "-- thread timing sensor value: \n"
-
- << "---- rtSwitchControllerSetup Duration "
- << sval->rtSwitchControllerSetupDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime "
- << sval->rtSwitchControllerSetupRoundTripTime.toMicroSecondsDouble()
- << " µs\n"
-
- << "---- rtRunNJointControllers Duration "
- << sval->rtRunNJointControllersDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime "
- << sval->rtRunNJointControllersRoundTripTime.toMicroSecondsDouble()
- << " µs\n"
-
- << "---- rtHandleInvalidTargets Duration "
- << sval->rtHandleInvalidTargetsDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime "
- << sval->rtHandleInvalidTargetsRoundTripTime.toMicroSecondsDouble()
- << " µs\n"
-
- << "---- rtRunJointControllers Duration "
- << sval->rtRunJointControllersDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime "
- << sval->rtRunJointControllersRoundTripTime.toMicroSecondsDouble()
- << " µs\n"
-
- << "---- rtBusSendReceive Duration "
- << sval->rtBusSendReceiveDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime "
- << sval->rtBusSendReceiveRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtReadSensorDeviceValues Duration "
- << sval->rtReadSensorDeviceValuesDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime "
- << sval->rtReadSensorDeviceValuesRoundTripTime.toMicroSecondsDouble()
- << " µs\n"
-
- << "---- rtUpdateSensorAndControlBuffer Duration "
- << sval->rtUpdateSensorAndControlBufferDuration.toMicroSecondsDouble()
- << " µs\t"
- << " RoundTripTime "
- << sval->rtUpdateSensorAndControlBufferRoundTripTime.toMicroSecondsDouble()
- << " µs\n"
-
- << "---- rtResetAllTargets Duration "
- << sval->rtResetAllTargetsDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime "
- << sval->rtResetAllTargetsRoundTripTime.toMicroSecondsDouble() << " µs\n"
-
- << "---- rtLoop Duration "
- << sval->rtLoopDuration.toMicroSecondsDouble() << " µs\t"
- << " RoundTripTime " << sval->rtLoopRoundTripTime.toMicroSecondsDouble()
- << " µs\n";
+ BUS_WARNING(
+ bus.getIterationCount(),
+ "RT loop is under 1kHz control frequency: %.1f \n"
+ "-- cycle time PDO timestamp %.1f µs\n"
+ "-- sleep %.1f µs\n"
+ "-- thread timing sensor value: \n"
+ "---- rtSwitchControllerSetup Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n"
+ "---- rtRunNJointControllers Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n"
+ "---- rtHandleInvalidTargets Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n"
+ "---- rtRunJointControllers Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n"
+ "---- rtBusSendReceive Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n"
+ "---- rtReadSensorDeviceValues Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n"
+ "---- rtUpdateSensorAndControlBuffer Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n"
+ "---- rtResetAllTargets Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n"
+ "---- rtLoop Duration %.1f µs\t"
+ " RoundTripTime %.1f µs\n",
+ realDeltaT.toMicroSecondsDouble(),
+ loopDuration.toMicroSecondsDouble(),
+ (loopPostSleepTime - loopPreSleepTime).toMicroSecondsDouble(),
+ sval->rtSwitchControllerSetupDuration.toMicroSecondsDouble(),
+ sval->rtSwitchControllerSetupRoundTripTime.toMicroSecondsDouble(),
+ sval->rtRunNJointControllersDuration.toMicroSecondsDouble(),
+ sval->rtRunNJointControllersRoundTripTime.toMicroSecondsDouble(),
+ sval->rtHandleInvalidTargetsDuration.toMicroSecondsDouble(),
+ sval->rtHandleInvalidTargetsRoundTripTime.toMicroSecondsDouble(),
+ sval->rtRunJointControllersDuration.toMicroSecondsDouble(),
+ sval->rtRunJointControllersRoundTripTime.toMicroSecondsDouble(),
+ sval->rtBusSendReceiveDuration.toMicroSecondsDouble(),
+ sval->rtBusSendReceiveRoundTripTime.toMicroSecondsDouble(),
+ sval->rtReadSensorDeviceValuesDuration.toMicroSecondsDouble(),
+ sval->rtReadSensorDeviceValuesRoundTripTime.toMicroSecondsDouble(),
+ sval->rtUpdateSensorAndControlBufferDuration.toMicroSecondsDouble(),
+ sval->rtUpdateSensorAndControlBufferRoundTripTime.toMicroSecondsDouble(),
+ sval->rtResetAllTargetsDuration.toMicroSecondsDouble(),
+ sval->rtResetAllTargetsRoundTripTime.toMicroSecondsDouble(),
+ sval->rtLoopDuration.toMicroSecondsDouble(),
+ sval->rtLoopRoundTripTime.toMicroSecondsDouble());
}
}
ARMARX_IMPORTANT << "RT loop stopped";
@@ -827,7 +863,7 @@ namespace armarx::control::ethercat
};
std::queue<FilePaths> fileHistory;
- IceUtil::Time lastIterationStart = IceUtil::Time::now();
+ IceUtil::Time lastIterationStart = armarx::rtNow();
std::string robotUnitLogPathFormat =
std::filesystem::path(properties.periodicLoggingDirectory)
@@ -849,9 +885,9 @@ namespace armarx::control::ethercat
fileHistory.pop();
}
- lastIterationStart = IceUtil::Time::now();
+ lastIterationStart = armarx::rtNow();
while (backupLoggingRunning.load() &&
- IceUtil::Time::now() <
+ armarx::rtNow() <
lastIterationStart +
IceUtil::Time::seconds(properties.periodicLoggingIntervalInSeconds))
{
diff --git a/source/armarx/control/ethercat/RTUnit.h b/source/armarx/control/ethercat/RTUnit.h
index c76b51698ae5c85b54036b7db1c8420cf17e0ba8..09177adefa80544d6d9ab16fe4084adee9d82577 100644
--- a/source/armarx/control/ethercat/RTUnit.h
+++ b/source/armarx/control/ethercat/RTUnit.h
@@ -35,6 +35,8 @@
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <armarx/control/ethercat/bus_io/Timeouts.h>
+
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
/**
@@ -209,6 +211,9 @@ namespace armarx::control::ethercat
std::string robotJointsNodeSet = "RobotJoints";
std::string robotColNodeSet = "RobotCol";
} properties;
+
+ armarx::control::ethercat::Timeouts ethercatTimeouts;
+
};
} // namespace armarx::control::ethercat
diff --git a/source/armarx/control/ethercat/SlaveInterface.cpp b/source/armarx/control/ethercat/SlaveInterface.cpp
index f95cda9799a014511393aec88427cd0712cb6710..c53836af235645a406878994e5c96e903c033012 100644
--- a/source/armarx/control/ethercat/SlaveInterface.cpp
+++ b/source/armarx/control/ethercat/SlaveInterface.cpp
@@ -26,9 +26,7 @@ namespace armarx::control::ethercat
bool
SlaveInterface::handleErrors()
{
- bool retVal;
- retVal = !hasError();
- return retVal;
+ return not hasError();
}
diff --git a/source/armarx/control/ethercat/Timing.h b/source/armarx/control/ethercat/Timing.h
new file mode 100644
index 0000000000000000000000000000000000000000..4c72252a6da595920b207c85a2cfad074c270de1
--- /dev/null
+++ b/source/armarx/control/ethercat/Timing.h
@@ -0,0 +1,206 @@
+/*
+* This file is part of ArmarX.
+*
+* Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX
+* @author Stefan Reither( stefan dot reither at kit dot edu)
+* @date 2021
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
+
+#include "ErrorReporting.h"
+
+namespace armarx::control::ethercat
+{
+ namespace timing
+ {
+ constexpr bool enableTimingInformation = false;
+
+ namespace general
+ {
+ class Timer
+ {
+ public:
+ void
+ start()
+ {
+ start_ = armarx::rtNow();
+ }
+
+ void
+ end(const char* comment)
+ {
+ GENERAL_INFO("%s - duration: %.3f ms",
+ comment,
+ (armarx::rtNow() - start_).toMilliSecondsDouble());
+ }
+
+ void
+ end(const char* comment, double thresholdMS)
+ {
+ const double diff = (armarx::rtNow() - start_).toMilliSecondsDouble();
+ if (diff >= thresholdMS)
+ {
+ GENERAL_INFO("%s - duration: %.3f ms", comment, diff);
+ }
+ }
+
+ private:
+ IceUtil::Time start_;
+ };
+ } // namespace general
+
+ namespace bus
+ {
+ class Timer
+ {
+ public:
+ void
+ start()
+ {
+ start_ = armarx::rtNow();
+ }
+
+ void
+ end(const std::uint64_t bin, const char* comment)
+ {
+ BUS_INFO(bin,
+ "%s - duration: %.3f ms",
+ comment,
+ (armarx::rtNow() - start_).toMilliSecondsDouble());
+ }
+
+ void
+ end(const std::uint64_t bin, const char* comment, double thresholdMS)
+ {
+ const auto diff = (armarx::rtNow() - start_).toMilliSecondsDouble();
+ if (diff >= thresholdMS)
+ {
+ BUS_INFO(bin, "%s - duration: %.3f ms", comment, diff);
+ }
+ }
+
+ private:
+ IceUtil::Time start_;
+ };
+ } // namespace bus
+
+ namespace slave
+ {
+ class Timer
+ {
+ public:
+ void
+ start()
+ {
+ start_ = armarx::rtNow();
+ }
+
+ void
+ end(const SlaveIdentifier sid, const char* comment)
+ {
+ SLAVE_INFO(sid,
+ "%s - duration: %.3f ms",
+ comment,
+ (armarx::rtNow() - start_).toMilliSecondsDouble());
+ }
+
+ void
+ end(const SlaveIdentifier sid, const char* comment, double thresholdMS)
+ {
+ const auto diff = (armarx::rtNow() - start_).toMilliSecondsDouble();
+ if (diff >= thresholdMS)
+ {
+ SLAVE_INFO(sid, "%s - duration: %.3f ms", comment, diff);
+ }
+ }
+
+ private:
+ IceUtil::Time start_;
+ };
+ } // namespace slave
+ } // namespace timing
+
+#define GENERAL_TIMING_START(name) \
+ auto name = armarx::control::ethercat::timing::general::Timer(); \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.start(); \
+ }
+#define GENERAL_TIMING_END_COMMENT(name, comment) \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.end(comment); \
+ }
+#define GENERAL_TIMING_END(name) GENERAL_TIMING_END_COMMENT(name, #name)
+#define GENERAL_TIMING_CEND_COMMENT(name, comment, thresholdMs) \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.end(comment, thresholdMs); \
+ }
+#define GENERAL_TIMING_CEND(name, thresholdMs) GENERAL_TIMING_CEND_COMMENT(name, #name, thresholdMs)
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define BUS_TIMING_START(name) \
+ auto name = armarx::control::ethercat::timing::bus::Timer(); \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.start(); \
+ }
+
+#define BUS_TIMING_END_COMMENT(name, bin, comment) \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.end(bin, comment); \
+ }
+#define BUS_TIMING_END(name, bin) BUS_TIMING_END_COMMENT(name, bin, #name)
+#define BUS_TIMING_CEND_COMMENT(name, bin, comment, thresholdMs) \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.end(bin, comment, thresholdMs); \
+ }
+#define BUS_TIMING_CEND(name, bin, thresholdMs) \
+ BUS_TIMING_CEND_COMMENT(name, bin, #name, thresholdMs)
+
+ /////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define SLAVE_TIMING_START(name) \
+ auto name = armarx::control::ethercat::timing::slave::Timer(); \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.start(); \
+ }
+#define SLAVE_TIMING_END_COMMENT(sid, name, comment) \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.end(sid, comment); \
+ }
+#define SLAVE_TIMING_END(sid, name) SLAVE_TIMING_END_COMMENT(sid, name, #name)
+#define SLAVE_TIMING_CEND_COMMENT(sid, name, comment, thresholdMs) \
+ if constexpr (armarx::control::ethercat::timing::enableTimingInformation) \
+ { \
+ name.end(sid, comment, thresholdMs); \
+ }
+#define SLAVE_TIMING_CEND(sid, name, thresholdMs) \
+ SLAVE_TIMING_CEND_COMMENT(sid, name, #name, thresholdMs)
+
+} // namespace armarx::control::ethercat
diff --git a/source/armarx/control/ethercat/bus_io/BusIO.cpp b/source/armarx/control/ethercat/bus_io/BusIO.cpp
index 8c4ce9b3e94d3332f1903c58adc267ae17a22d86..cabb50d1aebf47b8ab8e317d4daaa3904bb26f7f 100644
--- a/source/armarx/control/ethercat/bus_io/BusIO.cpp
+++ b/source/armarx/control/ethercat/bus_io/BusIO.cpp
@@ -9,6 +9,7 @@
#include <armarx/control/ethercat/ErrorReporting.h>
#include <armarx/control/ethercat/RTUtility.h>
+#include <armarx/control/ethercat/Timing.h>
#include "EtherCATFrame.h"
#include "requests/ChangeStateRequest.h"
@@ -63,7 +64,7 @@ namespace armarx::control::ethercat
{
if (!sdoAccessAvailable)
{
- BUS_WARNING("SDO-access is not yet available. Probably the bus was not started yet");
+ GENERAL_WARNING("SDO-access is not yet available. Probably the bus was not started yet");
return false;
}
return true;
@@ -100,7 +101,7 @@ namespace armarx::control::ethercat
if (workingCounter == EC_NOFRAME)
{
- BUS_ERROR("Sending and receiving EtherCAT frame timed out after %i µs (EC_NOFRAME). "
+ GENERAL_ERROR("Sending and receiving EtherCAT frame timed out after %i µs (EC_NOFRAME). "
"This timeout will most likely result in consecutive errors.",
timeouts.registerRead_us);
}
@@ -111,26 +112,27 @@ namespace armarx::control::ethercat
void
BusIO::rtUpdatePDO()
{
- RT_TIMING_START(updatePDOtiming)
+ GENERAL_TIMING_START(updatePDOtiming)
pdoUpdateRequested.store(true, std::memory_order_relaxed);
- RT_TIMING_START(send_processdata)
+ GENERAL_TIMING_START(send_processdata)
ec_send_processdata();
- RT_TIMING_CEND(send_processdata, TIMEOUT_UPDATE_PDO_US / 1000.0)
- RT_TIMING_START(receive_processdata);
+ GENERAL_TIMING_CEND(send_processdata, TIMEOUT_UPDATE_PDO_US / 1000.0 + 0.05)
+ GENERAL_TIMING_START(receive_processdata);
lastWorkCounter = ec_receive_processdata(TIMEOUT_UPDATE_PDO_US);
- RT_TIMING_CEND(receive_processdata, TIMEOUT_UPDATE_PDO_US / 1000.0)
+ GENERAL_TIMING_CEND(receive_processdata, TIMEOUT_UPDATE_PDO_US / 1000.0 + 0.05)
// Request Handler can continue to run
pdoUpdateRequested.store(false, std::memory_order_relaxed);
- RT_TIMING_CEND(updatePDOtiming, TIMEOUT_UPDATE_PDO_US / 1000.0)
+ GENERAL_TIMING_CEND(updatePDOtiming, TIMEOUT_UPDATE_PDO_US / 1000.0 + 0.05)
}
void
BusIO::requestHandlerLoop()
{
RTUtility::elevateThreadPriority(RTUtility::RT_THREAD_PRIORITY);
+ RTUtility::pinThreadToCPU(1);
while (requestHandlerThreadRunning.load() == true)
{
@@ -190,7 +192,7 @@ namespace armarx::control::ethercat
int ret = ec_writestate(request->slaveIndex);
if (ret == EC_NOFRAME)
{
- BUS_ERROR("Writing EtherCAT state %s to slave at index %u (0 = all slaves) timed out "
+ GENERAL_ERROR("Writing EtherCAT state %s to slave at index %u (0 = all slaves) timed out "
"(EC_NOFRAME). The timeout for this operation was %i µs, is hardcoded by "
"SOEM, and thus cannot be configured.",
request->slaveIndex,
@@ -216,7 +218,7 @@ namespace armarx::control::ethercat
bool const probablyTimedOut =
statecheckDuration.toMicroSeconds() > timeouts.stateCheck_us;
- BUS_WARNING(
+ GENERAL_WARNING(
"Could not validate EtherCAT state of slave at index %u (0 = all slaves).\n"
"Requested state is %s and the actual state is %s.\n"
"If the SOEM operation to check a slave's state timed out, the actual "
@@ -277,7 +279,7 @@ namespace armarx::control::ethercat
if (workingCounter == EC_NOFRAME)
{
- BUS_ERROR("Failed to reset error counters for slave at index %u.", request->slaveIndex);
+ GENERAL_ERROR("Failed to reset error counters for slave at index %u.", request->slaveIndex);
request->setFailed();
}
@@ -326,7 +328,8 @@ namespace armarx::control::ethercat
ss << std::to_string(slaveIndex) << ", ";
}
ss.seekp(ss.str().length() - 2);
- BUS_ERROR("%s", ss.str().c_str());
+ ss << "\n This happened on frame " << it.getCurrentIndex() << " out of " << count << " requested Frames. \n"; ss.seekp(ss.str().length() - 2);
+ GENERAL_ERROR("%s", ss.str().c_str());
request->setFailed();
}
@@ -345,10 +348,12 @@ namespace armarx::control::ethercat
return;
}
+ IceUtil::Time start = armarx::rtNow();
int wkc = -1;
int len = static_cast<int>(request->buflen);
if (request->readRequest)
{
+ GENERAL_TIMING_START(sdo_read_request);
wkc = ec_SDOread(request->sdoIdentifier.slaveIndex,
request->sdoIdentifier.index,
request->sdoIdentifier.subIndex,
@@ -356,9 +361,11 @@ namespace armarx::control::ethercat
&len,
request->buf,
timeouts.sdoRead_us);
+ GENERAL_TIMING_CEND(sdo_read_request, 1);
}
else
{
+ GENERAL_TIMING_START(sdo_write_request);
wkc = ec_SDOwrite(request->sdoIdentifier.slaveIndex,
request->sdoIdentifier.index,
request->sdoIdentifier.subIndex,
@@ -366,10 +373,12 @@ namespace armarx::control::ethercat
len,
request->buf,
timeouts.sdoWrite_us);
+ GENERAL_TIMING_CEND(sdo_write_request, 1);
}
+ IceUtil::Time end = armarx::rtNow();
if (wkc <= 0)
{
- BUS_ERROR("%s for slave at index %u at 0x%X:%u failed:\n"
+ GENERAL_ERROR("%s for slave at index %u at 0x%X:%u failed:\n"
"Work counter: %u\n"
"SOEM-errorlist:\n%s",
request->readRequest ? "SDORead" : "SDOWrite",
@@ -443,7 +452,7 @@ namespace armarx::control::ethercat
}
else
{
- BUS_WARNING("Trying to deactivate CoE Complete Access for slave at index %u which does "
+ GENERAL_WARNING("Trying to deactivate CoE Complete Access for slave at index %u which does "
"not exist on the bus.",
slaveIndex);
}
diff --git a/source/armarx/control/ethercat/bus_io/BusIO.h b/source/armarx/control/ethercat/bus_io/BusIO.h
index 4223c8b9acd95fcf5171e0952f107751d2d4c4e7..2a3d4f3385f90e70abe2b44d9bd8fec029fbf487 100644
--- a/source/armarx/control/ethercat/bus_io/BusIO.h
+++ b/source/armarx/control/ethercat/bus_io/BusIO.h
@@ -30,8 +30,6 @@
#include <mutex>
#include <thread>
-#include <IceUtil/Time.h>
-
#include "ESI.h"
#include "RequestQueue.h"
#include "SlaveRegisters.h"
diff --git a/source/armarx/control/ethercat/bus_io/ESI.cpp b/source/armarx/control/ethercat/bus_io/ESI.cpp
index bc1cae862fc116803d009c8b66e2064dbfe0c1ea..0bbb8efc63ff13df8bcf7ecfd9b5d5964e76eab7 100644
--- a/source/armarx/control/ethercat/bus_io/ESI.cpp
+++ b/source/armarx/control/ethercat/bus_io/ESI.cpp
@@ -20,7 +20,7 @@ namespace armarx::control::ethercat
bool pdiHadEEPROMControl = static_cast<bool>(ec_slave[slaveIndex].eep_pdi);
if (ec_eeprom2master(slaveIndex) == 0)
{
- BUS_ERROR(
+ GENERAL_ERROR(
"Failed to read ESI of slave at index %u - could not get control of the EEPROM.",
slaveIndex);
return std::nullopt;
diff --git a/source/armarx/control/ethercat/bus_io/EtherCATFrame.cpp b/source/armarx/control/ethercat/bus_io/EtherCATFrame.cpp
index 78128941dadc1b8163d76c332fc6edb79dfc59d7..c161fbab7a6ec6c6e4dbea7c37c907bf0a5523f6 100644
--- a/source/armarx/control/ethercat/bus_io/EtherCATFrame.cpp
+++ b/source/armarx/control/ethercat/bus_io/EtherCATFrame.cpp
@@ -37,4 +37,14 @@ namespace armarx::control::ethercat
{
return remaining == 0;
}
+
+ size_t
+ EtherCATFrameIterator::getCurrentIndex() const
+ {
+ if (not hasCompletedLoop())
+ {
+ return nextIndex - 1;
+ }
+ return 0;
+ }
} // namespace armarx::control::ethercat
diff --git a/source/armarx/control/ethercat/bus_io/EtherCATFrame.h b/source/armarx/control/ethercat/bus_io/EtherCATFrame.h
index f0b64644a991651022ca8cf6632eb540c2358cc5..4ea3f4d6b71fe331567d317865191c627783cb62 100644
--- a/source/armarx/control/ethercat/bus_io/EtherCATFrame.h
+++ b/source/armarx/control/ethercat/bus_io/EtherCATFrame.h
@@ -177,6 +177,8 @@ namespace armarx::control::ethercat
*/
bool atEnd() const;
+ size_t getCurrentIndex() const;
+
private:
EtherCATFrameList* list;
size_t nextIndex;
diff --git a/source/armarx/control/ethercat/bus_io/RequestQueue.cpp b/source/armarx/control/ethercat/bus_io/RequestQueue.cpp
index 04af48c6ae1faa7c0e50dda2855ea4a13140e360..d8852c4bb021bcb24b6ba5e779f7d94efefdf751 100644
--- a/source/armarx/control/ethercat/bus_io/RequestQueue.cpp
+++ b/source/armarx/control/ethercat/bus_io/RequestQueue.cpp
@@ -2,8 +2,10 @@
#include <boost/lockfree/policies.hpp>
#include <boost/lockfree/spsc_queue.hpp>
+#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
#include <armarx/control/ethercat/EtherCATState.h>
+#include <armarx/control/ethercat/ErrorReporting.h>
#include "requests/ChangeStateRequest.h"
#include "requests/ReadStatesRequest.h"
@@ -61,6 +63,12 @@ namespace armarx::control::ethercat
static std::mutex mtx;
std::unique_lock<std::mutex> lck(mtx);
+ if(pimpl->queue.write_available() < 10)
+ {
+ GENERAL_ERROR("Queue is running full.");
+ }
+
+
pimpl->queue.push(request);
requestCV.wait(lck,
[&, request]() { return request->isProcessed() || request->hasFailed(); });
diff --git a/source/armarx/control/ethercat/bus_io/SlaveRegisterReadingScheduler.cpp b/source/armarx/control/ethercat/bus_io/SlaveRegisterReadingScheduler.cpp
index e373c0fcc3aa6494f6db6092a78cf7ba6feef33a..46f82e3a810a3ef0b3abfec8836c4d7f2ce69597 100644
--- a/source/armarx/control/ethercat/bus_io/SlaveRegisterReadingScheduler.cpp
+++ b/source/armarx/control/ethercat/bus_io/SlaveRegisterReadingScheduler.cpp
@@ -393,6 +393,7 @@ namespace armarx::control::ethercat
}
}
+ // TODO: This most likely leaks memory
EtherCATFrameList* frameList = new EtherCATFrameList(); // NOLINT
int frameTotalSize = 0;
std::vector<std::tuple<uint16_t, int, int>> nextFrameIntervals;
diff --git a/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml b/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml
index 148dafe44f7b8d49bc72a4cb549fcf3d702d0d98..b757b1ba983143c70783276aeff86b2fd1140802 100644
--- a/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml
+++ b/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml
@@ -1,46 +1,47 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
-<!-- <AronIncludes>
- <Include include="<armarx/control/common/mp/aron/MPConfig.xml>" autoinclude="true" />
- </AronIncludes>-->
+ <AronIncludes>
+ <Include include="RobotAPI/libraries/aron/common/aron/framed.xml" />
+ <Include include="RobotAPI/libraries/aron/common/aron/PackagePath.xml" />
+ </AronIncludes>
<GenerateTypes>
<Object name="::armarx::control::retrieve_hand::skills::RetrieveHandParams">
<ObjectChild key="robotName">
- <String />
+ <String default="Armar7"/>
</ObjectChild>
<ObjectChild key="targetPose">
- <Matrix4f />
+ <armarx::arondto::FramedPositionAndOrientation />
</ObjectChild>
<ObjectChild key="robotNodeSet">
- <String />
+ <String default="LeftArm"/>
</ObjectChild>
<ObjectChild key="timeDurationInSec">
- <Double />
+ <Double default="10.0" />
</ObjectChild>
<ObjectChild key="kpLinear">
- <Float />
+ <Float default="10.0" />
</ObjectChild>
<ObjectChild key="kpAngular">
- <Float />
+ <Float default="8.0" />
</ObjectChild>
<ObjectChild key="kdLinear">
- <Float />
+ <Float default="1.0" />
</ObjectChild>
<ObjectChild key="kdAngular">
- <Float />
+ <Float default="1.0" />
</ObjectChild>
<ObjectChild key="fileNames">
<List>
- <String/>
+ <armarx::arondto::PackagePath/>
</List>
</ObjectChild>
diff --git a/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp b/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp
index 504418055fa99d1df6b53ff052fbb5c2efc45997..9cdd4f01738d7a7693bef581db7301c5269669bd 100644
--- a/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp
+++ b/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp
@@ -21,10 +21,12 @@
*/
#include "RetrieveHand.h"
+
#include <ArmarXCore/core/time/Metronome.h>
+
+#include <armarx/control/common/utils.h>
#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/task_space/NJointTSDMPController.h>
-#include <armarx/control/common/utils.h>
namespace armarx::control::retrieve_hand::core
{
@@ -37,9 +39,33 @@ namespace armarx::control::retrieve_hand::core
{
}
+ void
+ RetrieveHand::_deleteTSVMPController()
+ {
+ NJointControllerInterfacePrx controller = remote.robotUnit->getNJointController("dmpController_RetrieveHand");
+ if (controller)
+ {
+ ARMARX_DEBUG << "dmpController_RetrieveHand exists, but need to be deleted.";
+ if (controller->isControllerActive())
+ {
+ controller->deactivateController();
+ const auto metronomeTargetPeriod = armarx::Duration::MilliSeconds(10);
+ armarx::Metronome loopScheduler(metronomeTargetPeriod);
+ while (controller->isControllerActive())
+ {
+ loopScheduler.waitForNextTick();
+ }
+ }
+ controller->deleteController();
+ TimeUtil::SleepMS(10);
+ ARMARX_DEBUG << "dmpController_RetrieveHand is deleted";
+ }
+ }
+
bool
RetrieveHand::_runTSVMPController()
{
+ ARMARX_IMPORTANT << "Creating TSVMP Controller for Retrieve hand skill";
double phaseL = 100;
double phaseK = 1000;
double phaseDist0 = 50;
@@ -51,6 +77,7 @@ namespace armarx::control::retrieve_hand::core
double maxLinearVel = 1000;
double maxAngularVel = 10;
+ _deleteTSVMPController();
armarx::NJointTaskSpaceDMPControllerConfigPtr tsConfig =
new armarx::NJointTaskSpaceDMPControllerConfig(50,
@@ -100,9 +127,12 @@ namespace armarx::control::retrieve_hand::core
ARMARX_CHECK(r.get());
std::vector<double> goals;
- // Eigen::Matrix4f currentPose = r->getRobotNodeSet(properties.robotNodeSet)->getTCP()->getPoseInRootFrame();
- goals = armarx::control::common::mat4ToDVec(properties.targetPose);
-
+ Eigen::Matrix4f targetPoseInRoot = properties.targetPose.toRootFrame(r)->toEigen();
+ // Eigen::Matrix4f targetPoseInRoot = properties.targetPose.toEigen();
+ goals = armarx::control::common::mat4ToDVec(targetPoseInRoot);
+ ARMARX_INFO << "Running VMP with goal \n"
+ << VAROUT(targetPoseInRoot) << ", vec: \n"
+ << armarx::control::common::dVecToString(goals);
// if (in.isViaPoseListSet() && in.isViaPoseCanValSet() && in.getViaPoseList().size() == in.getViaPoseCanVal().size())
// {
@@ -111,18 +141,21 @@ namespace armarx::control::retrieve_hand::core
// dmpController->setViaPoints(in.getViaPoseCanVal().at(i), Helpers::pose2dvec(in.getViaPoseList().at(i)->toEigen()));
// }
// }
- //
+
dmpController->activateController();
dmpController->runDMP(goals, 1.0);
const auto metronomeTargetPeriod = armarx::Duration::MilliSeconds(10);
armarx::Metronome loopScheduler(metronomeTargetPeriod);
+ ARMARX_INFO << "VMP controller started";
while (running.load() && not dmpController->isFinished())
{
+ // ARMARX_INFO << VAROUT(dmpController->getCanVal());
loopScheduler.waitForNextTick();
}
+ ARMARX_DEBUG << "VMP Controller finished, try to delete controller";
dmpController->deactivateController();
while (dmpController->isControllerActive())
{
@@ -131,6 +164,7 @@ namespace armarx::control::retrieve_hand::core
dmpController->deleteController();
running.store(false);
+ ARMARX_INFO << "Retrieve hand skill finished";
return true;
}
diff --git a/source/armarx/control/retrieve_hand/core/RetrieveHand.h b/source/armarx/control/retrieve_hand/core/RetrieveHand.h
index a3da0e31fbe78c2e3716b547698bb6900eb2df87..3fc76ec84c2fb7afb5ecf6d165c0521b0cb4ffe6 100644
--- a/source/armarx/control/retrieve_hand/core/RetrieveHand.h
+++ b/source/armarx/control/retrieve_hand/core/RetrieveHand.h
@@ -58,7 +58,8 @@ namespace armarx::control::retrieve_hand::core
std::string robotName;
std::string robotNodeSet;
double timeDurationInSec;
- Eigen::Matrix4f targetPose;
+ // Eigen::Matrix4f targetPoseInRoot;
+ armarx::FramedPose targetPose;
float kpLinear = 10.0f;
float kpAngular = 10.0f;
float kdLinear = 1.0f;
@@ -81,6 +82,7 @@ namespace armarx::control::retrieve_hand::core
stop()
{
running.store(false);
+ _deleteTSVMPController();
}
bool execute()
@@ -96,6 +98,7 @@ namespace armarx::control::retrieve_hand::core
private:
bool _runTSVMPController();
+ void _deleteTSVMPController();
mutable std::mutex executionMutex;
std::atomic_bool running = false;
};
diff --git a/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp b/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp
index 2aeed21478dab22bdf159787467afb340f10295c..36542cd2937f29328c042583ac5fab839903e9f4 100644
--- a/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp
+++ b/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp
@@ -1,7 +1,11 @@
#include "RetrieveHand.h"
-#include <armarx/control/retrieve_hand/constants/constants.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/packagepath.cpp>
+
#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
+#include <armarx/control/retrieve_hand/constants/constants.h>
namespace armarx::control::retrieve_hand::skills
{
@@ -9,28 +13,40 @@ namespace armarx::control::retrieve_hand::skills
::armarx::skills::SkillDescription
RetrieveHand::GetSkillDescription()
{
- ParamType defaultParams;
+ ParamType defaults;
+ defaults.targetPose.header.agent = "Armar7";
+ defaults.targetPose.header.frame = "root";
+ defaults.targetPose.position = Eigen::Matrix<float, 3, 1>{-234.0f, 284.06f, 1103.0f};
+ defaults.targetPose.orientation =
+ Eigen::Quaternion<float>{0.7640368, -0.63349, 0.06744281, -0.10187};
+
+ armarx::arondto::PackagePath& packagePath = defaults.fileNames.emplace_back();
+ packagePath.package = "armar7_motions";
+ packagePath.path = "tcp_trajectories/dmp/default_left.csv";
+
+ // ARMARX_INFO << VAROUT(defaultParams.targetPose);
+ // ARMARX_INFO << VAROUT(defaultParams.targetPose.position) << VAROUT(defaultParams.targetPose.orientation);
+ // ARMARX_INFO << VAROUT(defaultParams.targetPose.header.frame) << VAROUT(defaultParams.targetPose.header.agent);
+
// defaultParams.exampleStringParam = "exampleDefaultValue";
- auto skillId = armarx::skills::SkillID{.skillName = armarx::control::retrieve_hand::constants::SKILL_NAME};
+ auto skillId = armarx::skills::SkillID{
+ .skillName = armarx::control::retrieve_hand::constants::SKILL_NAME};
- return ::armarx::skills::SkillDescription{
- .skillId = skillId,
- .description = "TODO: Description of skill RetrieveHand.",
- .rootProfileDefaults = defaultParams.toAron(),
- .timeout = ::armarx::Duration::MilliSeconds(1000),
- .parametersType = ParamType::ToAronType()};
+ return ::armarx::skills::SkillDescription{.skillId = skillId,
+ .description =
+ "TODO: Description of skill RetrieveHand.",
+ .rootProfileDefaults = defaults.toAron(),
+ .timeout = ::armarx::Duration::MilliSeconds(1000),
+ .parametersType = ParamType::ToAronType()};
}
- RetrieveHand::RetrieveHand(const Remote& r) :
- Base(GetSkillDescription()), remote(r)
+ RetrieveHand::RetrieveHand(const Remote& r) : Base(GetSkillDescription()), remote(r)
{
}
-
- RetrieveHand::RetrieveHand() :
- Base(GetSkillDescription())
+ RetrieveHand::RetrieveHand() : Base(GetSkillDescription())
{
}
@@ -47,20 +63,31 @@ namespace armarx::control::retrieve_hand::skills
RetrieveHand::main(const SpecializedMainInput& in)
{
// Enter main code of the skill here.
- core::RetrieveHand::Remote rem{
- .memoryNameSystem = remote.memoryNameSystem,
- .robotUnit = remote.robotUnit};
+ core::RetrieveHand::Remote rem{.memoryNameSystem = remote.memoryNameSystem,
+ .robotUnit = remote.robotUnit};
+
+ armarx::FramedPose targetPose{in.parameters.targetPose.position,
+ in.parameters.targetPose.orientation,
+ in.parameters.targetPose.header.frame,
+ in.parameters.targetPose.header.agent};
+
+ std::vector<std::string> fileNames;
+ for (const auto& aronPackagePath : in.parameters.fileNames)
+ {
+ armarx::PackagePath packagePath = fromAron<armarx::PackagePath>(aronPackagePath);
+ fileNames.push_back(packagePath.toSystemPath().string());
+ }
core::RetrieveHand::Properties props{
- .robotName = in.parameters.robotName,
- .robotNodeSet = in.parameters.robotNodeSet,
- .timeDurationInSec = in.parameters.timeDurationInSec,
- .targetPose = in.parameters.targetPose,
- .kpLinear = in.parameters.kpLinear,
- .kpAngular = in.parameters.kpAngular,
- .kdLinear = in.parameters.kdLinear,
- .kdAngular = in.parameters.kdAngular,
- .fileNames = in.parameters.fileNames,
+ .robotName = in.parameters.robotName,
+ .robotNodeSet = in.parameters.robotNodeSet,
+ .timeDurationInSec = in.parameters.timeDurationInSec,
+ .targetPose = targetPose,
+ .kpLinear = in.parameters.kpLinear,
+ .kpAngular = in.parameters.kpAngular,
+ .kdLinear = in.parameters.kdLinear,
+ .kdAngular = in.parameters.kdAngular,
+ .fileNames = fileNames,
};
@@ -74,6 +101,17 @@ namespace armarx::control::retrieve_hand::skills
return MakeFailedResult();
}
+
+ void
+ RetrieveHand::onStopRequested()
+ {
+ ARMARX_INFO << "requesting to stop TSVMP controller ";
+ if (impl)
+ {
+ impl->stop();
+ }
+ }
+
/*
::armarx::skills::Skill::ExitResult
RetrieveHand::exit(const SpecializedExitInput&)
@@ -82,4 +120,4 @@ namespace armarx::control::retrieve_hand::skills
}
*/
-}
+} // namespace armarx::control::retrieve_hand::skills
diff --git a/source/armarx/control/retrieve_hand/skills/RetrieveHand.h b/source/armarx/control/retrieve_hand/skills/RetrieveHand.h
index 6bfc9897c5d9f7409de640d54a39352f54e2ca2a..d72f14fd1a08f756000b722b1998422116068388 100644
--- a/source/armarx/control/retrieve_hand/skills/RetrieveHand.h
+++ b/source/armarx/control/retrieve_hand/skills/RetrieveHand.h
@@ -51,6 +51,9 @@ namespace armarx::control::retrieve_hand::skills
// Enable each function you want to override.
// ::armarx::skills::Skill::InitResult init(const SpecializedInitInput&) override;
::armarx::skills::Skill::MainResult main(const SpecializedMainInput& in) override;
+
+ void onStopRequested() override;
+
private:
Remote remote;
Properties properties;
diff --git a/source/armarx/control/rt_filters/RtMedianFilter.h b/source/armarx/control/rt_filters/RtMedianFilter.h
index 275ef52fc6c628e0b7279aaefe58318f4bc65aec..dee3715bee717b00ceaf19fa59e490f3123623b7 100644
--- a/source/armarx/control/rt_filters/RtMedianFilter.h
+++ b/source/armarx/control/rt_filters/RtMedianFilter.h
@@ -16,18 +16,12 @@ namespace armarx::control::rt_filters
public:
RtMedianFilter(size_t windowSize = 10);
- RtMedianFilter(RtMedianFilter&&) = default;
- RtMedianFilter(const RtMedianFilter&) = default;
- RtMedianFilter& operator=(RtMedianFilter&&) = default;
- RtMedianFilter& operator=(const RtMedianFilter&) = default;
-
float update(float value);
std::size_t getWindowSize() const;
private:
boost::circular_buffer<float> dataHistory;
- size_t windowSize;
std::vector<float> medianBuffer;
};