new gamepad driver and observer for the logitech f170 gamepad

scenarios/GamepadUnit/GamepadUnit.scx

 <?xml version="1.0" encoding="utf-8"?>
-<scenario name="GamepadUnit" lastChange="2017-03-09.11:59:19" creation="2017-03-09.11:59:13" globalConfigName="./config/global.cfg" package="RobotAPI">
+<scenario name="GamepadUnit" lastChange="2017-03-10.10:16:57" creation="2017-03-09.11:59:13" globalConfigName="./config/global.cfg" package="RobotAPI">
 	<application name="GamepadUnitApp" instance="" package="RobotAPI"/>
 	<application name="GamepadUnitObserverApp" instance="" package="RobotAPI"/>
 </scenario>

scenarios/GamepadUnit/config/GamepadUnitApp.cfg

@@ -82,6 +82,14 @@
 # ArmarX.GamepadUnit.EnableProfiling = 0
 
 
+# ArmarX.GamepadUnit.GamepadDeviceName:  device that will be opened as a gamepad
+#  Attributes:
+#  - Default:            /dev/input/js2
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.GamepadUnit.GamepadDeviceName = /dev/input/js2
+
+
 # ArmarX.GamepadUnit.GamepadTopicName:  Name of the Gamepad Topic
 #  Attributes:
 #  - Default:            GamepadValues
@@ -154,11 +162,6 @@
 # ArmarX.Verbosity = Info
 
 
-# Ice.Config:  Custom Property
-#  Attributes:
-#  - Default:            ::NOT_DEFINED::
-#  - Case sensitivity:   no
-#  - Required:           no
-# Ice.Config = ::NOT_DEFINED::
+
 
 

scenarios/GamepadUnit/config/GamepadUnitObserverApp.cfg

@@ -186,11 +186,6 @@
 # ArmarX.Verbosity = Info
 
 
-# Ice.Config:  Custom Property
-#  Attributes:
-#  - Default:            ::NOT_DEFINED::
-#  - Case sensitivity:   no
-#  - Required:           no
-# Ice.Config = ::NOT_DEFINED::
+
 
 

source/RobotAPI/components/units/GamepadUnitObserver.cpp

@@ -64,7 +64,13 @@ void GamepadUnitObserver::onExitObserver()
     debugDrawerPrx->removeLineVisu("IMU", "acceleration");
 }
 
-void GamepadUnitObserver::reportAnalogSticks(const std::string& device, const std::string& name, const AnalogStickData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
+
+PropertyDefinitionsPtr GamepadUnitObserver::createPropertyDefinitions()
+{
+    return PropertyDefinitionsPtr(new GamepadUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
+
+void GamepadUnitObserver::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
 {
     ScopedLock lock(dataMutex);
 
@@ -75,19 +81,31 @@ void GamepadUnitObserver::reportAnalogSticks(const std::string& device, const st
         offerChannel(device, "Gamepad data");
     }
 
-    offerOrUpdateDataField(device, "analog_x", Variant(values.x), "X value of the analog sticks");
-    offerOrUpdateDataField(device, "analog_y", Variant(values.y), "Y value of the analog sticks");
-    offerOrUpdateDataField(device, "analog_z", Variant(values.z), "Z value of the analog sticks");
-    offerOrUpdateDataField(device, "analog_a", Variant(values.a), "A value of the analog sticks");
+    //ARMARX_IMPORTANT << deactivateSpam(1) << "observed " << device << " with name " << name;
+
+    //axis
+    offerOrUpdateDataField(device, "leftStickX", Variant(data.leftStickX), "X value of the left analog stick");
+    offerOrUpdateDataField(device, "leftStickY", Variant(data.leftStickY), "Y value of the left analog stick");
+    offerOrUpdateDataField(device, "rightStickX", Variant(data.rightStickX), "X value of the right analog stick");
+    offerOrUpdateDataField(device, "rightStickY", Variant(data.rightStickY), "Y value of the right analog stick");
+    offerOrUpdateDataField(device, "dPadX", Variant(data.dPadX), "X value of the digital pad");
+    offerOrUpdateDataField(device, "dPadY", Variant(data.dPadY), "y value of the digital pad");
+    offerOrUpdateDataField(device, "leftTrigger", Variant(data.leftTrigger), "value of the left analog trigger");
+    offerOrUpdateDataField(device, "rightTrigger", Variant(data.rightTrigger), "value of the right analog trigger");
+    //buttons
+    offerOrUpdateDataField(device, "aButton", Variant(data.aButton), "A button pressed");
+    offerOrUpdateDataField(device, "backButton", Variant(data.backButton), "Back button pressed");
+    offerOrUpdateDataField(device, "bButton", Variant(data.bButton), "B button pressed");
+    offerOrUpdateDataField(device, "leftButton", Variant(data.leftButton), "Left shoulder button pressed");
+    offerOrUpdateDataField(device, "leftStickButton", Variant(data.leftStickButton), "Left stick button pressed");
+    offerOrUpdateDataField(device, "rightButton", Variant(data.rightButton), "Right shoulder button pressed");
+    offerOrUpdateDataField(device, "rightStickButton", Variant(data.rightStickButton), "Right stick button pressed");
+    offerOrUpdateDataField(device, "startButton", Variant(data.startButton), "Start button pressed");
+    offerOrUpdateDataField(device, "theMiddleButton", Variant(data.theMiddleButton), "The middle button pressed");
+    offerOrUpdateDataField(device, "xButton", Variant(data.xButton), "X button pressed");
+    offerOrUpdateDataField(device, "yButton", Variant(data.yButton), "Y button pressed");
 
     updateChannel(device);
-
-}
-
-
-PropertyDefinitionsPtr GamepadUnitObserver::createPropertyDefinitions()
-{
-    return PropertyDefinitionsPtr(new GamepadUnitObserverPropertyDefinitions(getConfigIdentifier()));
 }
 
 

source/RobotAPI/components/units/GamepadUnitObserver.h

@@ -73,8 +73,6 @@ namespace armarx
         virtual void onConnectObserver();
         virtual void onExitObserver();
 
-        void reportAnalogSticks(const std::string& device, const std::string& name, const AnalogStickData& values, const TimestampBasePtr& timestamp, const Ice::Current& c = ::Ice::Current());
-
         /**
          * @see PropertyUser::createPropertyDefinitions()
          */
@@ -85,6 +83,10 @@ namespace armarx
         Mutex dataMutex;
         DebugDrawerInterfacePrx debugDrawerPrx;
 
+
+        // GamepadUnitListener interface
+    public:
+        void reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c);
     };
 }
 

source/RobotAPI/drivers/GamepadUnit/CMakeLists.txt

@@ -16,6 +16,7 @@ set(SOURCES
 #@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
 )
 set(HEADERS
+./Joystick.h
 ./GamepadUnit.h
 #@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
 )

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp

@@ -23,38 +23,83 @@
 #include "GamepadUnit.h"
 
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
-
+#include <linux/joystick.h>
 
 using namespace armarx;
 
-
 void GamepadUnit::onInitComponent()
 {
     offeringTopic(getProperty<std::string>("GamepadTopicName").getValue());
+    deviceName = getProperty<std::string>("GamepadDeviceName").getValue();
     readTask = new RunningTask<GamepadUnit>(this, &GamepadUnit::run, "GamepadUnit");
 }
 
-
 void GamepadUnit::onConnectComponent()
 {
     topicPrx = getTopic<GamepadUnitListenerPrx>(getProperty<std::string>("GamepadTopicName").getValue());
-    readTask->start();
+    if (js.open(deviceName))
+    {
+        ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis << " axis and " << js.numberOfButtons << " buttons.";
+        if (js.numberOfAxis == 8 && js.numberOfButtons == 11)
+        {
+            readTask->start();
+        }
+        else
+        {
+            ARMARX_WARNING << "this is not our trusty logitech gamepad you moron.";
+        }
+    }
+    else
+    {
+        ARMARX_WARNING << "Could not open gamepad device " << deviceName;
+    }
 }
 
 void GamepadUnit::run()
 {
-    while(readTask->isRunning())
+    while (readTask->isRunning())
     {
+        GamepadData data;
+
+        if (!js.pollEvent())
+        {
+            ARMARX_WARNING << "failed to read gamepad data";
+            break;
+        }
+
         IceUtil::Time now = IceUtil::Time::now();
         TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
-        AnalogStickData data;
-        data.x = std::cos(now.toSecondsDouble());
-        data.y = std::sin(now.toSecondsDouble());
-        data.z = 0;
-        data.a = 0;
-        topicPrx->reportAnalogSticks("DeviceNameHere", "NameHere", data, nowTimestamp);
-
-        usleep(10000); // 10ms
+
+        //mapping found with command line tool jstest <device file>
+
+        float axisFactor = 1.0f / 32768.f;
+
+        data.leftStickY = js.axis[0] * axisFactor;
+        data.leftStickX = js.axis[1] * axisFactor;
+        data.rightStickX = js.axis[3] * axisFactor;
+        data.rightStickY = js.axis[4] * axisFactor;
+        data.dPadX = js.axis[7] * axisFactor;
+        data.dPadY = js.axis[6] * axisFactor;
+        data.leftTrigger = js.axis[2] * axisFactor;
+        data.rightTrigger = js.axis[5] * axisFactor;
+
+        data.leftButton = js.buttonsPressed[4];
+        data.rightButton = js.buttonsPressed[5];
+        data.backButton = js.buttonsPressed[6];
+        data.startButton = js.buttonsPressed[7];
+        data.xButton = js.buttonsPressed[2];
+        data.yButton = js.buttonsPressed[3];
+        data.aButton = js.buttonsPressed[0];
+        data.bButton = js.buttonsPressed[1];
+        data.theMiddleButton = js.buttonsPressed[8];
+        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);
+
+        //usleep(1000); // 10ms
     }
 }
 
@@ -73,6 +118,6 @@ void GamepadUnit::onExitComponent()
 armarx::PropertyDefinitionsPtr GamepadUnit::createPropertyDefinitions()
 {
     return armarx::PropertyDefinitionsPtr(new GamepadUnitPropertyDefinitions(
-                                      getConfigIdentifier()));
+            getConfigIdentifier()));
 }
 

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h

@@ -23,6 +23,9 @@
 #ifndef _ARMARX_COMPONENT_RobotAPI_GamepadUnit_H
 #define _ARMARX_COMPONENT_RobotAPI_GamepadUnit_H
 
+#include<linux/joystick.h>
+#include<sys/stat.h>
+#include<fcntl.h>
 
 #include <ArmarXCore/core/Component.h>
 
@@ -30,6 +33,8 @@
 
 #include <RobotAPI/interface/units/GamepadUnit.h>
 
+#include "Joystick.h"
+
 namespace armarx
 {
     /**
@@ -46,6 +51,7 @@ namespace armarx
             //defineRequiredProperty<std::string>("PropertyName", "Description");
             //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");
         }
     };
 
@@ -53,11 +59,11 @@ namespace armarx
      * @defgroup Component-GamepadUnit GamepadUnit
      * @ingroup RobotAPI-Components
      * A description of the component GamepadUnit.
-     * 
+     *
      * @class GamepadUnit
      * @ingroup Component-GamepadUnit
      * @brief Brief description of class GamepadUnit.
-     * 
+     *
      * Detailed description of class GamepadUnit.
      */
     class GamepadUnit :
@@ -102,6 +108,8 @@ namespace armarx
         GamepadUnitListenerPrx topicPrx;
         RunningTask<GamepadUnit>::pointer_type readTask;
         void run();
+        std::string deviceName;
+        Joystick js;
     };
 }
 

source/RobotAPI/drivers/GamepadUnit/Joystick.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::GamepadUnit
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotAPI_Joystick_H
+#define _ARMARX_COMPONENT_RobotAPI_Joystick_H
+
+#include<linux/joystick.h>
+#include<sys/stat.h>
+#include<fcntl.h>
+
+#include <ArmarXCore/core/Component.h>
+
+namespace armarx
+{
+
+    class Joystick
+    {
+
+    private:
+        int fd = -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)
+        {
+            fd = ::open(deviceName.c_str(), O_RDONLY);
+            if (fd != -1)
+            {
+                ioctl(fd, JSIOCGAXES, &numberOfAxis);
+                ioctl(fd, JSIOCGBUTTONS, &numberOfButtons);
+                name.resize(255);
+                ioctl(fd, JSIOCGNAME(255), &name[0]);
+                axis.resize(numberOfAxis, 0);
+                name = name.c_str();
+                buttonsPressed.resize(numberOfButtons, false);
+            }
+            return fd != -1;
+        }
+
+        bool pollEvent()
+        {
+            int bytes = read(fd, &event, sizeof(event));
+
+            // NOTE if this condition is not met, we're probably out of sync and this
+            // Joystick instance is likely unusable
+            if (bytes == -1 || bytes != sizeof(event))
+            {
+                return false;
+            }
+
+            if (event.type & JS_EVENT_BUTTON)
+            {
+                buttonsPressed[event.number] = event.value != 0;
+            }
+            else if (event.type & JS_EVENT_AXIS)
+            {
+                axis[event.number] = event.value;
+            }
+            return true;
+        }
+
+        void close()
+        {
+            ::close(fd);
+        }
+    };
+}
+#endif

source/RobotAPI/interface/units/GamepadUnit.ice

@@ -40,11 +40,28 @@
 
 module armarx
 {
-    struct AnalogStickData {
-        float x;
-        float y;
-        float z;
-        float a;
+    struct GamepadData {
+        float leftStickX;
+        float leftStickY;
+        float rightStickX;
+        float rightStickY;
+        float dPadX;
+        float dPadY;
+        float leftTrigger;
+        float rightTrigger;
+
+        bool leftButton;
+        bool rightButton;
+        bool backButton;
+        bool startButton;
+        bool xButton;
+        bool yButton;
+        bool aButton;
+        bool bButton;
+        bool theMiddleButton;
+        bool leftStickButton;
+        bool rightStickButton;
+
     };
     interface GamepadUnitInterface extends armarx::SensorActorUnitInterface
     {
@@ -52,7 +69,7 @@ module armarx
 
     interface GamepadUnitListener
     {	
-        void reportAnalogSticks(string device, string name, AnalogStickData values, TimestampBase timestamp);
+        void reportGamepadState(string device, string name, GamepadData values, TimestampBase timestamp);
     };
 	/**
 	* Implements an interface to an GamepadUnitObserver.