fix: Improve SS2 resiliancy

source/ArmarXGui/applications/ArmarXGui/Widgets/EmergencyStopWidget.cpp

@@ -25,6 +25,7 @@
 #include <QGridLayout>
 #include <QShortcut>
 #include <QTimer>
+#include <QToolTip>
 
 
 #define EMERGENCY_STOP_PROXY "EmergencyStopMaster"
@@ -32,6 +33,13 @@
 
 namespace armarx
 {
+
+    static const std::string ss2_active_tooltip =
+        "Release SS2 emergency stop (only possible after a short cooldown period. Shortcut: "
+        "Shift+Pause or Shift+End.";
+    static const std::string ss2_inactive_tooltip =
+        "Enable SS2 emergency stop. Shortcut: Pause or End.";
+
     EmergencyStopWidget::EmergencyStopWidget(QWidget* parent, ArmarXMainWindow* mainWindow) :
         mainWindow(mainWindow),
         iconNormal(QPixmap::fromImage(QImage(QString::fromUtf8(":/icons/emergency-stop.png")))),
@@ -43,7 +51,7 @@ namespace armarx
 
         // Redundant timer apart from the topic in order to recover from bad states if a topic
         // message was lost.
-        timer->setInterval(std::chrono::milliseconds(2'000));
+        timer->setInterval(std::chrono::milliseconds(1'000));
         connect(timer, &QTimer::timeout, this, &EmergencyStopWidget::updateEmergencyStopState);
         connect(this,
                 &EmergencyStopWidget::startPeriodicStateUpdate,
@@ -58,8 +66,7 @@ namespace armarx
         button->setCheckable(true);
         button->setIcon(icon);
         button->setIconSize(QSize(68, 28));
-        button->setToolTip(QString::fromStdString("Controls the EmergencyStop. When pressed the "
-                                                  "EmergencyStop is active. Shortcut: Pause Key"));
+        button->setToolTip(QString::fromStdString(ss2_active_tooltip));
         button->setVisible(false);
         QGridLayout* l = new QGridLayout(this->getWidget());
         l->setMargin(0);
@@ -86,10 +93,6 @@ namespace armarx
         connect(releaseSS2Shortcut2, &QShortcut::activated, this, &EmergencyStopWidget::releaseSS2);
 
         connect(button, &QPushButton::clicked, this, &EmergencyStopWidget::clicked);
-        std::stringstream str;
-        str << "After EmergencyStop was activated, you have to wait "
-            << deactivationWaitPeriod.count() << " ms before you can deactivate it.";
-        button->setToolTip(QString::fromStdString(str.str()));
     }
 
     QWidget*
@@ -163,18 +166,36 @@ namespace armarx
         if (emergencyStopMasterPrx)
         {
             emergencyStopMasterPrx->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+
+            if (lastKnownEmergencyStopState == EmergencyStopState::eEmergencyStopActive)
+            {
+                std::string const message =
+                    "SS2 already active. Press Shift+Pause or Shift+End to release SS2.";
+
+                QPoint globalPos =
+                    button->mapToGlobal(QPoint(button->width() / 2, button->height() / 2));
+                QToolTip::showText(globalPos, QString::fromStdString(message), button);
+                ARMARX_INFO << message;
+            }
         }
     }
 
     void
     EmergencyStopWidget::releaseSS2()
     {
-        if (clock_t::now() > timeLastActivated + deactivationWaitPeriod)
+        if (emergencyStopMasterPrx)
         {
-            if (emergencyStopMasterPrx)
+            lastKnownEmergencyStopState = emergencyStopMasterPrx->trySetEmergencyStopState(
+                EmergencyStopState::eEmergencyStopInactive);
+
+            if (lastKnownEmergencyStopState == EmergencyStopState::eEmergencyStopActive)
             {
-                emergencyStopMasterPrx->setEmergencyStopState(
-                    EmergencyStopState::eEmergencyStopInactive);
+                std::string const message = "SS2 cannot be released since it was just activated.";
+
+                QPoint globalPos =
+                    button->mapToGlobal(QPoint(button->width() / 2, button->height() / 2));
+                QToolTip::showText(globalPos, QString::fromStdString(message), button);
+                ARMARX_INFO << message;
             }
         }
     }
@@ -182,34 +203,25 @@ namespace armarx
     void
     EmergencyStopWidget::clicked(bool checked)
     {
-        if (!emergencyStopMasterPrx)
+        if (not emergencyStopMasterPrx)
         {
             return;
         }
 
-        EmergencyStopState const state = emergencyStopMasterPrx->getEmergencyStopState();
-        switch (state)
+        switch (lastKnownEmergencyStopState)
         {
             case EmergencyStopState::eEmergencyStopActive:
-                if (clock_t::now() > timeLastActivated + deactivationWaitPeriod)
-                {
-                    // Only release SS2 if the state we received still is "Active". Fail
-                    // otherwise.
-                    bool const success = emergencyStopMasterPrx->checkAndSetEmergencyStopState(
-                        EmergencyStopState::eEmergencyStopInactive, state);
-                    if (not success)
-                    {
-                        ARMARX_WARNING << "Toggling SS2 failed since it probably has "
-                                          "already been activated from another session.  "
-                                          "This mechanism is to prevent two simultanous "
-                                          "toggles of the SS2 to cancel out.";
-                    }
-                }
-                else
+            {
+                // Only release SS2 if the state we received still is "Active".  Fail otherwise.
+                lastKnownEmergencyStopState = emergencyStopMasterPrx->trySetEmergencyStopState(
+                    EmergencyStopState::eEmergencyStopInactive);
+                if (lastKnownEmergencyStopState == EmergencyStopState::eEmergencyStopActive)
                 {
+                    ARMARX_INFO << "SS2 cannot be released since it was just activated.";
                     button->setChecked(true);
                 }
-                break;
+            }
+            break;
             case EmergencyStopState::eEmergencyStopInactive:
                 // Always enable SS2 without checking.
                 enableSS2();
@@ -230,16 +242,13 @@ namespace armarx
         {
             case EmergencyStopState::eEmergencyStopActive:
                 button->setChecked(true);
-                if (lastKnownEmergencyStopState != EmergencyStopState::eEmergencyStopActive)
-                {
-                    timeLastActivated = clock_t::now();
-                }
+                button->setToolTip(QString::fromStdString(ss2_active_tooltip));
                 break;
             case EmergencyStopState::eEmergencyStopInactive:
-                button->setChecked(false);
-                break;
             default:
                 button->setChecked(false);
+                button->setToolTip(QString::fromStdString(ss2_inactive_tooltip));
+                break;
         }
 
         lastKnownEmergencyStopState = state;

source/ArmarXGui/applications/ArmarXGui/Widgets/EmergencyStopWidget.h

@@ -77,15 +77,12 @@ namespace armarx
         ArmarXMainWindow* mainWindow;
         QPixmap iconNormal;
         QPixmap iconDark;
+
         QToolButton* button;
         QAction* emergencyStopAction;
 
         EmergencyStopMasterInterfacePrx emergencyStopMasterPrx;
-
-        using clock_t = std::chrono::high_resolution_clock;
-        static constexpr std::chrono::milliseconds deactivationWaitPeriod{2500};
         EmergencyStopState lastKnownEmergencyStopState;
-        clock_t::time_point timeLastActivated;
         QTimer* timer;
 
         // ArmarXWidgetController interface