Highlight current field

The field gets highlighted if it is zero for 1/10s and the joint is enabled.

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

@@ -76,6 +76,7 @@ KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
 KinematicUnitWidgetController::KinematicUnitWidgetController() :
     kinematicUnitNode(nullptr),
     enableValueValidator(true),
+    historyTime(100000), // 1/10 s
     selectedControlMode(ePositionControl)
 {
     rootVisu = NULL;
@@ -1087,6 +1088,7 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
     }
     std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
     QTableWidgetItem* newItem;
+    NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
     NameValueMap::const_iterator it;
 
     for (unsigned int i = 0; i < rn.size(); i++)
@@ -1102,6 +1104,8 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
         newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
         ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
     }
+
+    highlightCriticalValues();
 }
 
 void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
@@ -1164,13 +1168,17 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
 
 void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
 {
-    if (!aValueChanged && reportedJointCurrents.size() > 0)
+    if (!aValueChanged && jointCurrents.size() > 0)
     {
         return;
     }
 
     boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
-    reportedJointCurrents = jointCurrents;
+    jointCurrentHistory.push_back(std::pair<Ice::Long, NameValueMap>(timestamp, jointCurrents));
+    if (jointCurrentHistory.back().first - jointCurrentHistory.front().first > historyTime)
+    {
+        jointCurrentHistory.pop_front();
+    }
 
     emit jointCurrentsReported();
 }
@@ -1225,6 +1233,64 @@ void KinematicUnitWidgetController::updateModel()
     robot->setJointValues(usedNodes, jv);
 }
 
+void KinematicUnitWidgetController::highlightCriticalValues()
+{
+    if (!enableValueValidator)
+    {
+        return;
+    }
+
+    boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
+
+    // show error on current value
+    std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+    static std::vector<QBrush> standardColors;
+    if (standardColors.size() == 0)
+    {
+        for (unsigned int i = 0; i < rn.size(); i++)
+        {
+            // all cells of a row have the same color
+            standardColors.push_back(ui.tableJointList->item(i, eTabelColumnCurrent)->background());
+        }
+    }
+
+    for (unsigned int i = 0; i < rn.size(); i++)
+    {
+        bool isZero = true;
+        for (auto historyIt = jointCurrentHistory.begin(); historyIt != jointCurrentHistory.end(); historyIt++)
+        {
+            NameValueMap reportedJointCurrents = historyIt->second;
+            NameValueMap::const_iterator it = reportedJointCurrents.find(rn[i]->getName());
+
+            if (it == reportedJointCurrents.end())
+            {
+                continue;
+            }
+
+            const float currentValue = it->second;
+
+            if (currentValue != 0)
+            {
+                isZero = false;
+                break;
+            }
+        }
+
+        NameStatusMap::const_iterator it;
+        it = reportedJointStatuses.find(rn[i]->getName());
+        JointStatus currentStatus = it->second;
+
+        if (isZero && currentStatus.enabled)
+        {
+            ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
+        }
+        else
+        {
+            ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
+        }
+    }
+}
+
 void KinematicUnitWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D)
 {
     //ARMARX_IMPORTANT << "KinematicUnitWidgetController controller " << getInstanceName() << ": set mutex " << mutex3D.get();

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

@@ -255,6 +255,8 @@ namespace armarx
 
         void updateModel();
 
+        void highlightCriticalValues();
+
     protected slots:
         void showVisuLayers(bool show);
     private:
@@ -268,12 +270,13 @@ namespace armarx
         bool initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet);
 
         bool enableValueValidator;
+        Ice::Long historyTime;
 
         NameValueMap reportedJointAngles;
         NameValueMap reportedJointVelocities;
         NameControlModeMap reportedJointControlModes;
         NameValueMap reportedJointTorques;
-        NameValueMap reportedJointCurrents;
+        std::deque<std::pair<Ice::Long, NameValueMap>> jointCurrentHistory;
         NameStatusMap reportedJointStatuses;
 
         std::vector<float> dirty_squaresum_old;