/KinematicUnitSimulation: +getDebugInfo

source/RobotAPI/components/units/KinematicUnitSimulation.cpp

@@ -33,6 +33,7 @@
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/core/util/algorithm.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
 
 
 #include <algorithm>
@@ -472,6 +473,26 @@ Ice::StringSeq KinematicUnitSimulation::getJoints(const Ice::Current& c) const
     return getMapKeys(sensorAngles);
 }
 
+DebugInfo KinematicUnitSimulation::getDebugInfo(const Ice::Current& c) const
+{
+    std::lock_guard g{jointStatesMutex};
+
+    DebugInfo debugInfo;
+    for(const auto& [jointName, info]: jointStates)
+    {
+        debugInfo.jointAngles[jointName] = info.angle;
+        debugInfo.jointVelocities[jointName] = info.velocity;
+
+        debugInfo.jointMotorTemperatures[jointName] = info.temperature;
+
+        debugInfo.jointTorques[jointName] = info.torque;
+        debugInfo.jointModes[jointName] = info.controlMode;
+    };
+
+    return debugInfo;
+}
+
+
 
 bool mapEntryEqualsString(std::pair<std::string, KinematicUnitSimulationJointState> iter, std::string compareString)
 {
@@ -499,4 +520,3 @@ void KinematicUnitSimulation::releaseJoints(const Ice::StringSeq& joints, const
         KinematicUnit::release(c);
     }
 }
-

source/RobotAPI/components/units/KinematicUnitSimulation.h

@@ -160,6 +160,8 @@ namespace armarx
         armarx::NameValueMap getJointVelocities(const Ice::Current& c) const override;
         Ice::StringSeq getJoints(const Ice::Current& c) const override;
 
+        DebugInfo getDebugInfo(const Ice::Current& c = Ice::emptyCurrent) const override;
+
         /// @see PropertyUser::createPropertyDefinitions()
         PropertyDefinitionsPtr createPropertyDefinitions() override;
 
@@ -167,7 +169,7 @@ namespace armarx
         KinematicUnitSimulationJointStates jointStates;
         KinematicUnitSimulationJointStates previousJointStates;
         KinematicUnitSimulationJointInfos jointInfos;
-        std::mutex jointStatesMutex;
+        mutable std::mutex jointStatesMutex;
         IceUtil::Time lastExecutionTime;
         float noise;
         int intervalMs;
@@ -180,7 +182,5 @@ namespace armarx
         mutable std::mutex sensorDataMutex;
         NameValueMap sensorAngles;
         NameValueMap sensorVelocities;
-
     };
 }
-