RobotUnit/KinematicSubUnit: +getDebugInfo

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)

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 {}