From c6f8d7efd2299f9fd687cda133ee0c84d36ef7c7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 5 Aug 2022 07:30:15 +0200
Subject: [PATCH] RobotUnit/KinematicSubUnit: +getDebugInfo
---
.../RobotUnit/Units/KinematicSubUnit.cpp | 21 +++++++++++++++++++
.../units/RobotUnit/Units/KinematicSubUnit.h | 3 +++
2 files changed, 24 insertions(+)
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index 0273f0cf6..490154fdd 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -21,6 +21,7 @@
*/
#include "KinematicSubUnit.h"
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
@@ -392,6 +393,26 @@ Ice::StringSeq armarx::KinematicSubUnit::getJoints(const Ice::Current& c) const
return getMapKeys(ang);
}
+armarx::DebugInfo armarx::KinematicSubUnit::getDebugInfo(const Ice::Current& c) const
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+
+ armarx::DebugInfo debugInfo
+ {
+ .jointModes = ctrlModes,
+ .jointAngles = ang,
+ .jointVelocities = vel,
+ .jointAccelerations = acc,
+ .jointTorques = motorCurrents,
+ .jointCurrents = motorCurrents,
+ .jointMotorTemperatures = motorTemperatures,
+ .jointStatus = statuses
+ };
+
+ return debugInfo;
+}
+
+
armarx::NJointControllerPtr armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) const
{
switch (c)
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index 2698240de..92a6caf3d 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -85,6 +85,9 @@ namespace armarx
NameValueMap getJointVelocities(const Ice::Current&) const override;
Ice::StringSeq getJoints(const Ice::Current& c) const override;
+ DebugInfo getDebugInfo(const Ice::Current& c = Ice::emptyCurrent) const override;
+
+
void onInitKinematicUnit() override {}
void onStartKinematicUnit() override {}
void onExitKinematicUnit() override {}
--
GitLab