final tests on ARMAR-6

source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp

@@ -65,10 +65,11 @@
 #include <ArmarXCore/observers/filters/MedianFilter.h>
 
 // System
+#include <cstddef>
 #include <cstdio>
 #include <memory>
 #include <string>
-#include <string>
+#include <optional>
 #include <cstdlib>
 #include <iostream>
 #include <cmath>
@@ -115,7 +116,6 @@ KinematicUnitWidgetController::KinematicUnitWidgetController() :
 
 void KinematicUnitWidgetController::onInitComponent()
 {
-    dirty_squaresum_old.resize(5, 0);
     ARMARX_INFO << flush;
     verbose = true;
 
@@ -163,6 +163,7 @@ void KinematicUnitWidgetController::onConnectComponent()
 {
     // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
     jointCurrentHistory.clear();
+    jointCurrentHistory.set_capacity(5);
 
     // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
     kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
@@ -236,7 +237,7 @@ void KinematicUnitWidgetController::onConnectComponent()
     // Check robot name and disable setZero Button if necessary
     if (not simox::alg::starts_with(robot->getName(), "Armar3"))
     {
-        ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
+        ARMARX_VERBOSE << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
         ui.pushButtonKinematicUnitPos1->setDisabled(true);
     }
 
@@ -279,6 +280,8 @@ void KinematicUnitWidgetController::runUpdate()
 
 void KinematicUnitWidgetController::onDisconnectComponent()
 {
+    kinematicUnitInterfacePrx = nullptr;
+
     if(updateTask)
     {
         updateTask->stop();
@@ -301,10 +304,13 @@ void KinematicUnitWidgetController::onDisconnectComponent()
         robotVisu->removeAllChildren();
         debugLayerVisu->removeAllChildren();
     }
+
 }
 
 void KinematicUnitWidgetController::onExitComponent()
 {
+    kinematicUnitInterfacePrx = nullptr;
+
     if(updateTask)
     {
         updateTask->stop();
@@ -586,7 +592,7 @@ void KinematicUnitWidgetController::resetSliderToZeroPosition()
 
 void KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
 {
-    ARMARX_INFO << "Setting control mode of radio button group to " << controlMode;
+    ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
 
     switch(controlMode)
     {
@@ -811,7 +817,7 @@ VirtualRobot::CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisuali
 
     if (robot != NULL)
     {
-        ARMARX_INFO << "getting coin visualization" << flush;
+        ARMARX_VERBOSE << "getting coin visualization" << flush;
         coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
 
         if (!coinVisualization || !coinVisualization->getCoinVisualization())
@@ -1390,6 +1396,27 @@ void KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJoin
     robot->setJointValues(reportedJointAngles);
 }
 
+std::optional<float> mean(const boost::circular_buffer<NameValueMap>& buffer, const std::string& key)
+{
+    float sum = 0;
+    std::size_t count = 0;
+
+    for(const auto& element: buffer)
+    {
+        if(element.count(key) > 0)
+        {
+            sum += element.at(key);
+        }
+    }
+
+    if(count == 0)
+    {
+        return std::nullopt;
+    }
+
+    return sum / static_cast<float>(count);
+}
+
 void KinematicUnitWidgetController::highlightCriticalValues(const NameStatusMap& reportedJointStatuses)
 {
     if (!enableValueValidator)
@@ -1415,29 +1442,24 @@ void KinematicUnitWidgetController::highlightCriticalValues(const NameStatusMap&
     // check robot current value of nodes
     for (unsigned int i = 0; i < rn.size(); i++)
     {
-        bool isStatic = true;
-        float smoothedValue = 0;
+        const auto& jointName = rn[i]->getName();
 
-        for (auto historyIt = jointCurrentHistory.begin(); historyIt != jointCurrentHistory.end(); historyIt++)
+        const auto currentSmoothValOpt = mean(jointCurrentHistory, jointName);
+        if(not currentSmoothValOpt.has_value())
         {
-            NameValueMap reportedJointCurrents = historyIt->second;
-            NameValueMap::const_iterator it = reportedJointCurrents.find(rn[i]->getName());
-
-            if (it == reportedJointCurrents.end())
-            {
-                continue;
-            }
-
-            const float currentValue = std::fabs(it->second);
+            continue;
+        }
 
-            smoothedValue = 0.5f * currentValue + 0.5f * smoothedValue;
+        const float smoothValue = std::fabs(currentSmoothValOpt.value());
 
-            if (currentValue != jointCurrentHistory.begin()->second.find(rn[i]->getName())->second)
-            {
-                isStatic = false;
-            }
+        if(jointCurrentHistory.front().count(jointName) == 0)
+        {
+            continue;
         }
 
+        const float startValue = jointCurrentHistory.front().at(jointName);
+        const bool isStatic = (smoothValue == startValue);
+
         NameStatusMap::const_iterator it;
         it = reportedJointStatuses.find(rn[i]->getName());
         JointStatus currentStatus = it->second;
@@ -1451,7 +1473,7 @@ void KinematicUnitWidgetController::highlightCriticalValues(const NameStatusMap&
             }
 
         }
-        else if (smoothedValue > currentValueMax)
+        else if (std::abs(smoothValue) > currentValueMax)
         {
             // current value is too high
             ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
@@ -1493,15 +1515,9 @@ float KinematicUnitWidgetController::cutJitter(float value)
     return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
 }
 
-// void KinematicUnitWidgetController::timerEvent(QTimerEvent*)
-// {
-//     ARMARX_INFO << "timerEvent";
-//     updateGui();
-// }
-
 void KinematicUnitWidgetController::fetchData()
 {
-    ARMARX_INFO << "updateGui";
+    ARMARX_DEBUG << "updateGui";
 
     if(not kinematicUnitInterfacePrx)
     {
@@ -1516,7 +1532,7 @@ void KinematicUnitWidgetController::fetchData()
 
 void KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
 {
-    ARMARX_INFO << "debug info received";
+    ARMARX_DEBUG << "debug info received";
 
     updateModel(debugInfo.jointAngles);
 
@@ -1566,6 +1582,8 @@ void RangeValueDelegate::paint(QPainter* painter, const QStyleOptionViewItem& op
 
 KinematicUnitWidgetController::~KinematicUnitWidgetController() 
 {
+    kinematicUnitInterfacePrx = nullptr;
+
     if(updateTask)
     {
         updateTask->stop();

source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h

@@ -58,6 +58,7 @@
 #include <VirtualRobot/VirtualRobot.h>
 
 #include <mutex>
+#include <boost/circular_buffer.hpp>
 
 #include <ArmarXCore/core/time.h>
 
@@ -292,11 +293,10 @@ namespace armarx
         armarx::DateTime lastJointAngleUpdateTimestamp;
         float currentValueMax;
 
-        // std::deque<std::pair<Ice::Long, NameValueMap>> jointCurrentHistory;
+        boost::circular_buffer<NameValueMap> jointCurrentHistory;
 
         QPointer<QWidget> __widget;
         QPointer<KinematicUnitConfigDialog> dialog;
-        // ControlMode selectedControlMode;
 
         RangeValueDelegate delegate;
         /**