Merge branch 'fix/emergency_stop_button' into 'master'

Fix rogue emergency stop button

See merge request !99

source/ArmarXGui/applications/ArmarXGui/CMakeLists.txt

@@ -40,6 +40,15 @@ set(HEADERS ArmarXGuiApp.h
     Widgets/GuiUseCaseSelector.ui
 )
 
+if ( CMAKE_COMPILER_IS_GNUCC )
+    # old gcc 8.4.0 on ubuntu 18.04 does not support this flag
+    check_cxx_compiler_flag("-Wno-error=deprecated-enum-enum-conversion" SUPPORTS_DEPRECATED_ENUM_ENUM_CONVERSION)
+
+    if (SUPPORTS_DEPRECATED_ENUM_ENUM_CONVERSION)
+        set(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -Wno-error=deprecated-enum-enum-conversion")
+    endif()
+endif()
+
 find_package(OpenMP)
 if (OPENMP_FOUND)
     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")

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

@@ -24,6 +24,8 @@
 
 #include <QGridLayout>
 #include <QShortcut>
+#include <QTimer>
+#include <QToolTip>
 
 
 #define EMERGENCY_STOP_PROXY "EmergencyStopMaster"
@@ -31,118 +33,242 @@
 
 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")))),
-        iconDark(QPixmap::fromImage(QImage(QString::fromUtf8(":/icons/emergency-stop-dark.png"))))
+        iconDark(QPixmap::fromImage(QImage(QString::fromUtf8(":/icons/emergency-stop-dark.png")))),
+        lastKnownEmergencyStopState(EmergencyStopState::eEmergencyStopInactive),
+        timer(new QTimer(this))
     {
         qRegisterMetaType<EmergencyStopState>("EmergencyStopState");
 
+        // Redundant timer apart from the topic in order to recover from bad states if a topic
+        // message was lost.
+        timer->setInterval(std::chrono::milliseconds(1'000));
+        connect(timer, &QTimer::timeout, this, &EmergencyStopWidget::updateEmergencyStopState);
+        connect(this,
+                &EmergencyStopWidget::startPeriodicStateUpdate,
+                timer,
+                qOverload<>(&QTimer::start));
+        connect(this, &EmergencyStopWidget::stopPeriodicStateUpdate, timer, &QTimer::stop);
         QIcon icon;
         icon.addPixmap(iconNormal, QIcon::Normal, QIcon::Off);
         icon.addPixmap(iconDark, QIcon::Normal, QIcon::On);
 
-        QGridLayout* layout = new QGridLayout(this->getWidget());
         button = new QToolButton();
         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);
-        layout->addWidget(button, 0, 0);
-        layout->setMargin(0);
-        layout->setContentsMargins(0, 0, 0, 0);
-        this->getWidget()->setLayout(layout);
-        emergencyStopShortcut = new QShortcut(this->getWidget());
-        emergencyStopShortcut->setContext(Qt::ApplicationShortcut);
-        emergencyStopShortcut->setKey(Qt::Key_Pause);
-        connect(emergencyStopShortcut, SIGNAL(activated()), this, SLOT(clicked()));
-
-        connect(button, SIGNAL(clicked(bool)), this, SLOT(clicked(bool)));
-        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()));
+        QGridLayout* l = new QGridLayout(this->getWidget());
+        l->setMargin(0);
+        l->setContentsMargins(0, 0, 0, 0);
+        this->getWidget()->setLayout(l);
+        this->getWidget()->layout()->addWidget(button);
+
+        QShortcut* enableSS2Shortcut1 = new QShortcut(this->getWidget());
+        enableSS2Shortcut1->setContext(Qt::ApplicationShortcut);
+        enableSS2Shortcut1->setKey(Qt::Key_Pause);
+        QShortcut* enableSS2Shortcut2 = new QShortcut(this->getWidget());
+        enableSS2Shortcut2->setContext(Qt::ApplicationShortcut);
+        enableSS2Shortcut2->setKey(Qt::Key_End);
+        connect(enableSS2Shortcut1, &QShortcut::activated, this, &EmergencyStopWidget::enableSS2);
+        connect(enableSS2Shortcut2, &QShortcut::activated, this, &EmergencyStopWidget::enableSS2);
+
+        QShortcut* releaseSS2Shortcut1 = new QShortcut(this->getWidget());
+        releaseSS2Shortcut1->setContext(Qt::ApplicationShortcut);
+        releaseSS2Shortcut1->setKey(Qt::SHIFT | Qt::Key_Pause);
+        QShortcut* releaseSS2Shortcut2 = new QShortcut(this->getWidget());
+        releaseSS2Shortcut2->setContext(Qt::ApplicationShortcut);
+        releaseSS2Shortcut2->setKey(Qt::SHIFT | Qt::Key_End);
+        connect(releaseSS2Shortcut1, &QShortcut::activated, this, &EmergencyStopWidget::releaseSS2);
+        connect(releaseSS2Shortcut2, &QShortcut::activated, this, &EmergencyStopWidget::releaseSS2);
+
+        connect(button, &QPushButton::clicked, this, &EmergencyStopWidget::clicked);
     }
 
-    QWidget* EmergencyStopWidget::getButtonWidget()
+    QWidget*
+    EmergencyStopWidget::getButtonWidget()
     {
         return this->getWidget();
     }
 
-    void EmergencyStopWidget::onInitComponent()
+    void
+    EmergencyStopWidget::onInitComponent()
     {
         usingProxy(EMERGENCY_STOP_PROXY);
         usingTopic(EMERGENCY_STOP_TOPIC_NAME);
     }
 
-    void EmergencyStopWidget::onConnectComponent()
+    void
+    EmergencyStopWidget::onConnectComponent()
     {
-        emergencyStopMasterPrx = getProxy < EmergencyStopMasterInterfacePrx>(EMERGENCY_STOP_PROXY);
+        ARMARX_INFO << "SS2 widget connected.";
+        emergencyStopMasterPrx = getProxy<EmergencyStopMasterInterfacePrx>(EMERGENCY_STOP_PROXY);
         QMetaObject::invokeMethod(button, "setVisible", Qt::QueuedConnection, Q_ARG(bool, true));
-        QMetaObject::invokeMethod(this, "setChecked", Qt::QueuedConnection, Q_ARG(EmergencyStopState, emergencyStopMasterPrx->getEmergencyStopState()));
+        EmergencyStopState state = EmergencyStopState::eEmergencyStopActive;
+
+        try
+        {
+            state = emergencyStopMasterPrx->getEmergencyStopState();
+        }
+        catch (Ice::Exception const& e)
+        {
+            ARMARX_ERROR << "Could not query SS2 state." << deactivateSpam(2);
+        }
+
+        QMetaObject::invokeMethod(
+            this, "setChecked", Qt::QueuedConnection, Q_ARG(EmergencyStopState, state));
+
+        emit startPeriodicStateUpdate();
     }
 
-    void EmergencyStopWidget::onDisconnectComponent()
+    void
+    EmergencyStopWidget::onDisconnectComponent()
     {
+        ARMARX_IMPORTANT << "SS2 widget disconnected.  This is expected if the earlier connected "
+                            "robot unit shut down.";
         QMetaObject::invokeMethod(button, "setVisible", Qt::QueuedConnection, Q_ARG(bool, false));
+
+        emit stopPeriodicStateUpdate();
     }
 
+    void
+    EmergencyStopWidget::loadSettings(QSettings* settings)
+    {
+        ;
+    }
 
-    void EmergencyStopWidget::loadSettings(QSettings* settings)
+    void
+    EmergencyStopWidget::saveSettings(QSettings* settings)
     {
+        ;
+    }
 
+    void
+    EmergencyStopWidget::reportEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+    {
+        QMetaObject::invokeMethod(
+            this, "setChecked", Qt::QueuedConnection, Q_ARG(EmergencyStopState, state));
     }
 
-    void EmergencyStopWidget::saveSettings(QSettings* settings)
+    void
+    EmergencyStopWidget::enableSS2()
     {
+        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::reportEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+    void
+    EmergencyStopWidget::releaseSS2()
     {
-        QMetaObject::invokeMethod(this, "setChecked", Qt::QueuedConnection, Q_ARG(EmergencyStopState, state));
+        if (emergencyStopMasterPrx)
+        {
+            lastKnownEmergencyStopState = emergencyStopMasterPrx->trySetEmergencyStopState(
+                EmergencyStopState::eEmergencyStopInactive);
+
+            if (lastKnownEmergencyStopState == EmergencyStopState::eEmergencyStopActive)
+            {
+                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;
+            }
+        }
     }
 
-    void EmergencyStopWidget::clicked(bool checked)
+    void
+    EmergencyStopWidget::clicked(bool checked)
     {
-        EmergencyStopState state = emergencyStopMasterPrx->getEmergencyStopState();
-        switch (state)
+        if (not emergencyStopMasterPrx)
         {
-            case EmergencyStopState::eEmergencyStopActive :
-                if (clock_t::now() > timeLastActivated + deactivationWaitPeriod)
-                {
-                    emergencyStopMasterPrx->setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
-                }
-                else
+            return;
+        }
+
+        switch (lastKnownEmergencyStopState)
+        {
+            case EmergencyStopState::eEmergencyStopActive:
+            {
+                // 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;
-            case EmergencyStopState::eEmergencyStopInactive :
-                emergencyStopMasterPrx->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+            }
+            break;
+            case EmergencyStopState::eEmergencyStopInactive:
+                // Always enable SS2 without checking.
+                enableSS2();
                 break;
         }
     }
 
-    std::string EmergencyStopWidget::getDefaultName() const
+    std::string
+    EmergencyStopWidget::getDefaultName() const
     {
         return "EmergencyStopGuiWidget" + iceNameUUID;
     }
 
-    void EmergencyStopWidget::setChecked(const EmergencyStopState state)
+    void
+    EmergencyStopWidget::setChecked(const EmergencyStopState state)
     {
         switch (state)
         {
-            case EmergencyStopState::eEmergencyStopActive :
+            case EmergencyStopState::eEmergencyStopActive:
                 button->setChecked(true);
+                button->setToolTip(QString::fromStdString(ss2_active_tooltip));
                 break;
-            case EmergencyStopState::eEmergencyStopInactive :
+            case EmergencyStopState::eEmergencyStopInactive:
+            default:
                 button->setChecked(false);
-                timeLastActivated = clock_t::now();
+                button->setToolTip(QString::fromStdString(ss2_inactive_tooltip));
                 break;
-            default :
-                button->setChecked(false);
+        }
+
+        lastKnownEmergencyStopState = state;
+    }
+
+    void
+    EmergencyStopWidget::updateEmergencyStopState()
+    {
+        try
+        {
+            if (emergencyStopMasterPrx)
+            {
+                setChecked(emergencyStopMasterPrx->getEmergencyStopState());
+            }
+        }
+        catch (Ice::Exception const& e)
+        {
+            ARMARX_ERROR << "Could not query SS2 state." << deactivateSpam(2);
+            setChecked(EmergencyStopState::eEmergencyStopActive);
         }
     }
-}
+
+} // namespace armarx

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

@@ -22,15 +22,17 @@
 
 #pragma once
 
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXCore/components/EmergencyStop/EmergencyStop.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <IceUtil/UUID.h>
-
-#include <QWidget>
 #include <QAction>
 #include <QPushButton>
 #include <QToolButton>
+#include <QWidget>
+
+#include <IceUtil/UUID.h>
+
+#include <ArmarXCore/components/EmergencyStop/EmergencyStop.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
 
 class QLabel;
 class QGridLayout;
@@ -43,13 +45,21 @@ namespace armarx
     class EmergencyStopWidget :
         public ArmarXComponentWidgetControllerTemplate<EmergencyStopWidget>,
         public armarx::EmergencyStopListener
+
     {
         Q_OBJECT
     public:
         explicit EmergencyStopWidget(QWidget* parent = 0, ArmarXMainWindow* mainWindow = 0);
         QWidget* getButtonWidget();
+        //        void reportEmergencyStopState(EmergencyStopState, const Ice::Current&) override;
+
+    signals:
+        void startPeriodicStateUpdate();
+        void stopPeriodicStateUpdate();
 
     public slots:
+        void enableSS2();
+        void releaseSS2();
         void clicked(bool = true);
 
         // ManagedIceObject interface
@@ -61,34 +71,35 @@ namespace armarx
 
     private slots:
         void setChecked(const EmergencyStopState);
+        void updateEmergencyStopState();
 
     private:
         ArmarXMainWindow* mainWindow;
-        QGridLayout* layout;
         QPixmap iconNormal;
         QPixmap iconDark;
+
         QToolButton* button;
         QAction* emergencyStopAction;
-        QShortcut* emergencyStopShortcut;
 
         EmergencyStopMasterInterfacePrx emergencyStopMasterPrx;
-
-        using clock_t = std::chrono::high_resolution_clock;
-        static constexpr std::chrono::milliseconds deactivationWaitPeriod{2500};
-        clock_t::time_point timeLastActivated;
+        EmergencyStopState lastKnownEmergencyStopState;
+        QTimer* timer;
 
         // ArmarXWidgetController interface
     public:
-        static QString GetWidgetName()
+        static QString
+        GetWidgetName()
         {
             return "EmergencyStopWidget";
         }
+
         void loadSettings(QSettings* settings) override;
         void saveSettings(QSettings* settings) override;
         std::string iceNameUUID = IceUtil::generateUUID();
         // EmergencyStopListener interface
-    public:
+
         void reportEmergencyStopState(EmergencyStopState, const Ice::Current&) override;
     };
-    using EmergencyStopWidgetPtr = IceInternal::Handle <EmergencyStopWidget>;
-}
+
+    using EmergencyStopWidgetPtr = IceInternal::Handle<EmergencyStopWidget>;
+} // namespace armarx