feaT: Current state

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

@@ -40,8 +40,9 @@ namespace armarx
     {
         qRegisterMetaType<EmergencyStopState>("EmergencyStopState");
 
-
-        timer->setInterval(std::chrono::milliseconds(100));
+        // 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));
         connect(timer, &QTimer::timeout, this, &EmergencyStopWidget::updateEmergencyStopState);
         connect(this,
                 &EmergencyStopWidget::startPeriodicStateUpdate,
@@ -60,18 +61,28 @@ namespace armarx
         button->setToolTip(QString::fromStdString("Controls the EmergencyStop. When pressed the "
                                                   "EmergencyStop is active. Shortcut: Pause Key"));
         button->setVisible(false);
-        layout->addWidget(button, 0, 0);
-        layout->setMargin(0);
-        layout->setContentsMargins(0, 0, 0, 0);
+        gridLayout->addWidget(button, 0, 0);
+        gridLayout->setMargin(0);
+        gridLayout->setContentsMargins(0, 0, 0, 0);
         this->getWidget()->setLayout(gridLayout);
 
-        enableSS2Shortcut = new QShortcut(this->getWidget());
-        enableSS2Shortcut->setContext(Qt::ApplicationShortcut);
-        enableSS2Shortcut->setKey(Qt::Key_Pause);
+        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);
 
-        releaseSS2Shortcut = new QShortcut(this->getWidget());
-        releaseSS2Shortcut->setContext(Qt::ApplicationShortcut);
-        releaseSS2Shortcut->setKey(Qt::SHIFT | Qt::Key_Pause);
+        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);
         std::stringstream str;
@@ -96,17 +107,23 @@ namespace armarx
     void
     EmergencyStopWidget::onConnectComponent()
     {
+        ARMARX_INFO << "SS2 widget connected.";
         emergencyStopMasterPrx = getProxy<EmergencyStopMasterInterfacePrx>(EMERGENCY_STOP_PROXY);
         QMetaObject::invokeMethod(button, "setVisible", Qt::QueuedConnection, Q_ARG(bool, true));
 
-        connect(enableSS2Shortcut, &QShortcut::activated, this, &EmergencyStopWidget::enableSS2);
-        connect(releaseSS2Shortcut, &QShortcut::activated, this, &EmergencyStopWidget::releaseSS2);
-
-        QMetaObject::invokeMethod(
-            this,
-            "setChecked",
-            Qt::QueuedConnection,
-            Q_ARG(EmergencyStopState, emergencyStopMasterPrx->getEmergencyStopState()));
+        try
+        {
+            QMetaObject::invokeMethod(
+                this,
+                "setChecked",
+                Qt::QueuedConnection,
+                Q_ARG(EmergencyStopState, emergencyStopMasterPrx->getEmergencyStopState()));
+        }
+        catch (Ice::Exception const& e)
+        {
+            ARMARX_ERROR << "Could not query SS2 state." << deactivateSpam(2);
+            setChecked(EmergencyStopState::eEmergencyStopActive);
+        }
 
         emit startPeriodicStateUpdate();
     }
@@ -114,6 +131,8 @@ namespace armarx
     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();
@@ -122,17 +141,21 @@ namespace armarx
     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::reportEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+    {
+        QMetaObject::invokeMethod(
+            this, "setChecked", Qt::QueuedConnection, Q_ARG(EmergencyStopState, state));
+    }
 
     void
     EmergencyStopWidget::enableSS2()
@@ -165,16 +188,46 @@ namespace armarx
     {
         if (emergencyStopMasterPrx)
         {
-            EmergencyStopState state = emergencyStopMasterPrx->getEmergencyStopState();
+            EmergencyStopState const state = emergencyStopMasterPrx->getEmergencyStopState();
+            bool success = true;
             switch (state)
             {
                 case EmergencyStopState::eEmergencyStopActive:
-                    releaseSS2();
+                    if (clock_t::now() > timeLastActivated + deactivationWaitPeriod)
+                    {
+                        if (emergencyStopMasterPrx)
+                        {
+                            // 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
+                    {
+                        button->setChecked(true);
+                    }
                     break;
                 case EmergencyStopState::eEmergencyStopInactive:
-                    enableSS2();
+                    if (emergencyStopMasterPrx)
+                    {
+                        // Always enable SS2 without checking.
+                        enableSS2();
+                    }
                     break;
             }
+            if (not success)
+            {
+                ARMARX_WARNING << "Releasing SS2 failed.";
+            }
         }
     }
 
@@ -187,6 +240,8 @@ namespace armarx
     void
     EmergencyStopWidget::setChecked(const EmergencyStopState state)
     {
+        ARMARX_INFO << "STATE IS :"
+                    << (state == EmergencyStopState::eEmergencyStopActive ? "active" : "inactive");
         switch (state)
         {
             case EmergencyStopState::eEmergencyStopActive:
@@ -204,6 +259,7 @@ namespace armarx
     void
     EmergencyStopWidget::updateEmergencyStopState()
     {
+        ARMARX_INFO << "UPDATE!";
         try
         {
             if (emergencyStopMasterPrx)

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

@@ -42,7 +42,10 @@ namespace armarx
 {
     class ArmarXMainWindow;
 
-    class EmergencyStopWidget : public ArmarXComponentWidgetControllerTemplate<EmergencyStopWidget>
+    class EmergencyStopWidget :
+        public ArmarXComponentWidgetControllerTemplate<EmergencyStopWidget>,
+        public armarx::EmergencyStopListener
+
     {
         Q_OBJECT
     public:
@@ -77,8 +80,6 @@ namespace armarx
         QPixmap iconDark;
         QToolButton* button;
         QAction* emergencyStopAction;
-        QShortcut* enableSS2Shortcut;
-        QShortcut* releaseSS2Shortcut;
 
         EmergencyStopMasterInterfacePrx emergencyStopMasterPrx;
 
@@ -99,6 +100,8 @@ namespace armarx
         void saveSettings(QSettings* settings) override;
         std::string iceNameUUID = IceUtil::generateUUID();
         // EmergencyStopListener interface
+
+        void reportEmergencyStopState(EmergencyStopState, const Ice::Current&) override;
     };
 
     using EmergencyStopWidgetPtr = IceInternal::Handle<EmergencyStopWidget>;