Merge remote-tracking branch 'origin/armar6demo' into SecondHandsDemo

source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp

@@ -70,7 +70,14 @@ armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions()
 void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
 {
     //scales are for the robot axis
-    platformUnitPrx->move(data.leftStickY  * scaleX, data. leftStickX  * scaleY, data.rightStickX * scaleRotation);
+    if (data.rightTrigger > 0)
+    {
+        platformUnitPrx->move(data.leftStickY  * scaleX, data. leftStickX  * scaleY, data.rightStickX * scaleRotation);
+    }
+    else
+    {
+        platformUnitPrx->move(0, 0, 0);
+    }
     //ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
 }
 

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp

@@ -37,6 +37,25 @@ void GamepadUnit::onInitComponent()
 void GamepadUnit::onConnectComponent()
 {
     topicPrx = getTopic<GamepadUnitListenerPrx>(getProperty<std::string>("GamepadTopicName").getValue());
+    sendTask = new SimplePeriodicTask<>([&]
+    {
+        ScopedLock lock(mutex);
+        if (!js.opened())
+            return;
+        auto age = IceUtil::Time::now() - dataTimestamp->toTime();
+        if (age.toMilliSeconds() < getProperty<int>("PublishTimeout").getValue())
+            topicPrx->reportGamepadState(deviceName, js.name, data, dataTimestamp);
+        else
+        {
+            ARMARX_INFO << deactivateSpam(100000, std::to_string(dataTimestamp->getTimestamp())) << "No new signal from gamepad for " << age.toMilliSecondsDouble() << " milliseconds. Not sending data. Timeout: " <<  getProperty<int>("PublishTimeout").getValue() << " ms";
+        }
+    }, 30);
+    sendTask->start();
+    openGamepadConnection();
+}
+
+bool GamepadUnit::openGamepadConnection()
+{
     if (js.open(deviceName))
     {
         ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis << " axis and " << js.numberOfButtons << " buttons.";
@@ -46,29 +65,37 @@ void GamepadUnit::onConnectComponent()
         }
         else
         {
-            ARMARX_WARNING << "this is not our trusty logitech gamepad you moron.";
+            ARMARX_WARNING << "this is not our trusty logitech gamepad.";
+            js.close();
+            return false;
         }
     }
     else
     {
         ARMARX_WARNING << "Could not open gamepad device " << deviceName;
+        js.close();
+        return false;
     }
+    return true;
 }
 
 void GamepadUnit::run()
 {
     while (readTask->isRunning())
     {
-        GamepadData data;
-
         if (!js.pollEvent())
         {
-            ARMARX_WARNING << "failed to read gamepad data";
-            break;
+            ARMARX_WARNING << "failed to read gamepad data - trying to reconnect";
+            js.close();
+            sleep(1);
+            while (readTask->isRunning() && !openGamepadConnection())
+            {
+                sleep(1);
+            }
         }
-
+        ScopedLock lock(mutex);
         IceUtil::Time now = IceUtil::Time::now();
-        TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+        dataTimestamp = new TimestampVariant(now);
 
         //mapping found with command line tool jstest <device file>
 
@@ -95,9 +122,7 @@ void GamepadUnit::run()
         data.leftStickButton = js.buttonsPressed[9];
         data.rightStickButton = js.buttonsPressed[10];
 
-        ARMARX_IMPORTANT << "left x (integer): " << js.axis[0] << " left x (float): " << data.leftStickX;
-
-        topicPrx->reportGamepadState(deviceName, js.name, data, nowTimestamp);
+        ARMARX_VERBOSE << "left x (integer): " << js.axis[0] << " left x (float): " << data.leftStickX << " right trigger: " << data.rightTrigger;
 
         //usleep(1000); // 10ms
     }
@@ -106,7 +131,14 @@ void GamepadUnit::run()
 
 void GamepadUnit::onDisconnectComponent()
 {
-
+    if (sendTask)
+    {
+        sendTask->stop();
+    }
+    if (readTask)
+    {
+        readTask->stop();
+    }
 }
 
 

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h

@@ -25,10 +25,12 @@
 #include<linux/joystick.h>
 #include<sys/stat.h>
 #include<fcntl.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
 #include <ArmarXCore/core/Component.h>
 
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
 
 #include <RobotAPI/interface/units/GamepadUnit.h>
 
@@ -51,6 +53,7 @@ namespace armarx
             //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
             defineOptionalProperty<std::string>("GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
             defineOptionalProperty<std::string>("GamepadDeviceName", "/dev/input/js2", "device that will be opened as a gamepad");
+            defineOptionalProperty<int>("PublishTimeout", 2000, "In Milliseconds. Timeout after which the gamepad data is not published after, if no new data was read from the gamepad");
         }
     };
 
@@ -103,12 +106,19 @@ namespace armarx
          */
         armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
+        bool openGamepadConnection();
+
     private:
         GamepadUnitListenerPrx topicPrx;
         RunningTask<GamepadUnit>::pointer_type readTask;
+        SimplePeriodicTask<>::pointer_type sendTask;
+
         void run();
+        Mutex mutex;
         std::string deviceName;
         Joystick js;
+        GamepadData data;
+        TimestampVariantPtr dataTimestamp;
     };
 }
 

source/RobotAPI/drivers/GamepadUnit/Joystick.h

@@ -62,6 +62,11 @@ namespace armarx
             return fd != -1;
         }
 
+        bool opened() const
+        {
+            return fd != -1;
+        }
+
         bool pollEvent()
         {
             int bytes = read(fd, &event, sizeof(event));
@@ -87,6 +92,7 @@ namespace armarx
         void close()
         {
             ::close(fd);
+            fd = -1;
         }
     };
 }

source/RobotAPI/libraries/CMakeLists.txt

@@ -8,6 +8,7 @@ add_subdirectory(RobotAPIVariantWidget)
 add_subdirectory(RobotAPINJointControllers)
 
 add_subdirectory(DMPController)
-add_subdirectory(DSControllers)
+# disabled until Mathlib include is fixed...
+#add_subdirectory(DSControllers)
 
 add_subdirectory(RobotStatechartHelpers)