Merge branch 'feature/gamepad-upgrade' into 'master'

Gamepad: upgrade

See merge request sw/armarx/robot-api!358

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp

@@ -33,6 +33,7 @@ void GamepadUnit::onInitComponent()
     ARMARX_TRACE;
     offeringTopic(getProperty<std::string>("GamepadTopicName").getValue());
     deviceName = getProperty<std::string>("GamepadDeviceName").getValue();
+    deviceEventName = getProperty<std::string>("GamepadForceFeedbackName").getValue();
     readTask = new RunningTask<GamepadUnit>(this, &GamepadUnit::run, "GamepadUnit");
 }
 
@@ -69,15 +70,23 @@ void GamepadUnit::onConnectComponent()
             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();
     ARMARX_TRACE;
     openGamepadConnection();
 }
 
+void GamepadUnit::vibrate(const ::Ice::Current&)
+{
+    ARMARX_INFO << "vibration!";
+    js.executeEffect();
+}
+
 bool GamepadUnit::openGamepadConnection()
 {
-    if (js.open(deviceName))
+    if (js.open(deviceName, deviceEventName))
     {
+
         ARMARX_TRACE;
         ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis << " axis and " << js.numberOfButtons << " buttons.";
         if (js.numberOfAxis == 8 && js.numberOfButtons == 11)
@@ -185,4 +194,3 @@ armarx::PropertyDefinitionsPtr GamepadUnit::createPropertyDefinitions()
     return armarx::PropertyDefinitionsPtr(new GamepadUnitPropertyDefinitions(
             getConfigIdentifier()));
 }
-

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h

@@ -53,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<std::string>("GamepadForceFeedbackName", "", "device that will be used for force feedback, leave empty to disable. See RobotAPI/source/RobotAPI/drivers/GamepadUnit/README.md for more details.");
             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");
         }
     };
@@ -69,7 +70,8 @@ namespace armarx
      * Detailed description of class GamepadUnit.
      */
     class GamepadUnit :
-        virtual public armarx::Component
+        virtual public armarx::Component,
+        virtual public GamepadUnitInterface
     {
     public:
         /**
@@ -108,6 +110,8 @@ namespace armarx
 
         bool openGamepadConnection();
 
+        void vibrate(const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
     private:
         GamepadUnitListenerPrx topicPrx;
         RunningTask<GamepadUnit>::pointer_type readTask;
@@ -116,9 +120,9 @@ namespace armarx
         void run();
         std::mutex mutex;
         std::string deviceName;
+        std::string deviceEventName;
         Joystick js;
         GamepadData data;
         TimestampVariantPtr dataTimestamp;
     };
 }
-

source/RobotAPI/drivers/GamepadUnit/Joystick.h

@@ -22,12 +22,17 @@
 
 #pragma once
 
-#include<linux/joystick.h>
-#include<sys/stat.h>
-#include<fcntl.h>
+#include <cstdint>
+
+#include <fcntl.h>
+#include <sys/poll.h>
+#include <sys/stat.h>
+#include <unistd.h>
 
 #include <ArmarXCore/core/Component.h>
 
+#include <linux/joystick.h>
+
 namespace armarx
 {
 
@@ -36,21 +41,39 @@ namespace armarx
 
     private:
         int fd = -1;
+        int fdEvent = -1;
         js_event event;
 
     public:
-
         std::vector<int16_t> axis;
         std::vector<bool> buttonsPressed;
         int numberOfAxis;
         int numberOfButtons;
         std::string name;
 
-        bool open(std::string const& deviceName)
+        bool
+        open(std::string const& deviceName, std::string const& deviceEventName)
         {
+
             fd = ::open(deviceName.c_str(), O_RDONLY);
+
+            if (!deviceEventName.empty())
+            {
+                ARMARX_INFO << "Force feedback enabled";
+                fdEvent = ::open(deviceEventName.c_str(), O_RDWR | O_CLOEXEC);
+
+                ARMARX_CHECK(fdEvent != -1);
+            } else {
+                ARMARX_INFO << "Force feedback disabled";
+            }
+
             if (fd != -1)
             {
+                // ARMARX_INFO << "before";
+                // executeEffect();
+                // ARMARX_INFO << "after";
+
+
                 ioctl(fd, JSIOCGAXES, &numberOfAxis);
                 ioctl(fd, JSIOCGBUTTONS, &numberOfButtons);
                 name.resize(255);
@@ -59,15 +82,21 @@ namespace armarx
                 name = name.c_str();
                 buttonsPressed.resize(numberOfButtons, false);
             }
+
+            ARMARX_INFO << "execute effect";
+            executeEffect();
+
             return fd != -1;
         }
 
-        bool opened() const
+        bool
+        opened() const
         {
             return fd != -1;
         }
 
-        bool pollEvent()
+        bool
+        pollEvent()
         {
             int bytes = read(fd, &event, sizeof(event));
 
@@ -89,10 +118,165 @@ namespace armarx
             return true;
         }
 
-        void close()
+        void
+        executeEffect(int gain = 100, const int nTimes = 1)
+        {
+            // this feature is disabled
+            if(fdEvent < 0) return;
+
+            // see https://docs.kernel.org/input/ff.html
+
+
+            // https://xnux.eu/devices/feature/vibrator.html
+
+            int ret;
+            // pollfd pfds[1];
+            int effects;
+
+            // fd = open_event_dev("vibrator", O_RDWR | O_CLOEXEC);
+            // syscall_error(fd < 0, "Can't open vibrator event device");
+
+            ret = ioctl(fdEvent, EVIOCGEFFECTS, &effects);
+            ARMARX_CHECK(ret >= 0);
+            // syscall_error(ret < 0, "EVIOCGEFFECTS failed");
+
+            // ARMARX_CHECK(effects & FF_RUMBLE);
+
+            // Set the gain of the device
+            {
+                // int gain; between 0 and 100
+                struct input_event ie; // structure used to communicate with the driver
+
+                ie.type = EV_FF;
+                ie.code = FF_GAIN;
+                ie.value = 0xFFFFUL * gain / 100;
+
+                if (write(fdEvent, &ie, sizeof(ie)) == -1)
+                {
+                    perror("set gain");
+                }
+            }
+
+
+            ff_effect e;
+            // e.type = FF_RUMBLE;
+            // e.id = -1;
+            // e.replay.length = 5000;
+            // e.replay.delay = 500;
+            // e.u.rumble.strong_magnitude = 1;
+
+            e.type = FF_PERIODIC;
+            e.id = -1;
+            e.replay.length = 5000;
+            e.replay.delay = 500;
+            e.u.periodic.waveform = FF_SQUARE;
+            e.u.periodic.period = 1000;
+            e.u.periodic.magnitude = 0xFF;
+            e.u.periodic.offset = 0xFF;
+
+            ret = ioctl(fdEvent, EVIOCSFF, &e);
+            ARMARX_CHECK(ret >= 0);
+
+            // syscall_error(ret < 0, "EVIOCSFF failed");
+
+            ARMARX_INFO << VAROUT(e.id);
+
+            input_event play;
+            play.type = EV_FF;
+            play.code = static_cast<std::uint16_t>(e.id);
+            play.value = 1;
+
+            ret = write(fdEvent, &play, sizeof(play));
+            ARMARX_CHECK(ret >= 0);
+
+            ARMARX_INFO << "Executing effect";
+
+
+            for (int i = 0; i < 5; i++)
+            {
+                
+                input_event statusIe; // structure used to communicate with the driver
+                statusIe.type = EV_FF_STATUS;
+                statusIe.code = e.id;
+                // statusIe.value = 0;
+
+                ret = write(fdEvent, &statusIe, sizeof(statusIe));
+
+                // ARMARX_CHECK()
+                // ret should be FF_STATUS_PLAYING
+                ARMARX_INFO << VAROUT(ret);
+
+                sleep(1);
+            }
+
+
+            // syscall_error(ret < 0, "write failed");
+
+            ARMARX_INFO << "Executing effect";
+            // sleep(6);
+
+
+            input_event stop;
+            stop.type = EV_FF;
+            stop.code = e.id;
+            stop.value = 0;
+            const int stopStatus = write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
+
+
+            ret = ioctl(fdEvent, EVIOCRMFF, e.id);
+            ARMARX_CHECK(ret >= 0);
+
+            // syscall_error(ret < 0, "EVIOCRMFF failed");
+
+            // close(fdEvent);
+
+
+            /**/
+            // Set the gain of the device
+            // {
+            //     // int gain; between 0 and 100
+            //     struct input_event ie; // structure used to communicate with the driver
+
+            //     ie.type = EV_FF;
+            //     ie.code = FF_GAIN;
+            //     ie.value = 0xFFFFUL * gain / 100;
+
+            //     if (write(fd, &ie, sizeof(ie)) == -1)
+            //     {
+            //         perror("set gain");
+            //     }
+            // }
+
+
+            // struct input_event play;
+            // struct input_event stop;
+
+            // // upload request to device
+            // ff_effect effect;
+            // const auto uploadStatus = ioctl(fd, EVIOCSFF, &effect);
+
+            // // Play n times
+            // play.type = EV_FF;
+            // play.code = effect.id;
+            // play.value = nTimes;
+
+            // const int playStatus = write(fd, static_cast<const void*>(&play), sizeof(play));
+
+            // // Stop an effect
+            // stop.type = EV_FF;
+            // stop.code = effect.id;
+            // stop.value = 0;
+            // const int stopStatus = write(fd, static_cast<const void*>(&stop), sizeof(stop));
+
+            // // remove effect
+            // ioctl(fd, EVIOCRMFF, effect.id);
+        }
+
+        void
+        close()
         {
             ::close(fd);
             fd = -1;
         }
     };
-}
+} // namespace armarx

source/RobotAPI/interface/units/GamepadUnit.ice

@@ -62,8 +62,9 @@ module armarx
         bool rightStickButton;
 
     };
-    interface GamepadUnitInterface extends armarx::SensorActorUnitInterface
+    interface GamepadUnitInterface // extends armarx::SensorActorUnitInterface
     {
+        void vibrate();
     };
 
     interface GamepadUnitListener
@@ -78,4 +79,3 @@ module armarx
     };
 
 };
-