From ebf6aa4156457f9571935fccbd1f1ce75b05240f Mon Sep 17 00:00:00 2001
From: Pascal Weiner <pascal.weiner@kit.edu>
Date: Fri, 10 Mar 2017 10:57:08 +0100
Subject: [PATCH] new gamepad driver and observer for the logitech f170 gamepad
---
scenarios/GamepadUnit/GamepadUnit.scx | 2 +-
.../GamepadUnit/config/GamepadUnitApp.cfg | 15 +--
.../config/GamepadUnitObserverApp.cfg | 7 +-
.../components/units/GamepadUnitObserver.cpp | 42 ++++++---
.../components/units/GamepadUnitObserver.h | 6 +-
.../drivers/GamepadUnit/CMakeLists.txt | 1 +
.../drivers/GamepadUnit/GamepadUnit.cpp | 73 +++++++++++---
.../drivers/GamepadUnit/GamepadUnit.h | 12 ++-
.../RobotAPI/drivers/GamepadUnit/Joystick.h | 94 +++++++++++++++++++
.../RobotAPI/interface/units/GamepadUnit.ice | 29 ++++--
10 files changed, 232 insertions(+), 49 deletions(-)
create mode 100644 source/RobotAPI/drivers/GamepadUnit/Joystick.h
diff --git a/scenarios/GamepadUnit/GamepadUnit.scx b/scenarios/GamepadUnit/GamepadUnit.scx
index baf2e16f6..8eaf1f00a 100644
--- a/scenarios/GamepadUnit/GamepadUnit.scx
+++ b/scenarios/GamepadUnit/GamepadUnit.scx
@@ -1,5 +1,5 @@
<?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>
diff --git a/scenarios/GamepadUnit/config/GamepadUnitApp.cfg b/scenarios/GamepadUnit/config/GamepadUnitApp.cfg
index b302f691a..7d704c0ef 100644
--- a/scenarios/GamepadUnit/config/GamepadUnitApp.cfg
+++ b/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::
+
diff --git a/scenarios/GamepadUnit/config/GamepadUnitObserverApp.cfg b/scenarios/GamepadUnit/config/GamepadUnitObserverApp.cfg
index 9c0055a60..0d1aa7e7e 100644
--- a/scenarios/GamepadUnit/config/GamepadUnitObserverApp.cfg
+++ b/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::
+
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.cpp b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
index b22c0dbb5..437648bee 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.cpp
+++ b/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()));
}
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.h b/source/RobotAPI/components/units/GamepadUnitObserver.h
index b246d1a78..0120cfc09 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.h
+++ b/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);
};
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/CMakeLists.txt b/source/RobotAPI/drivers/GamepadUnit/CMakeLists.txt
index 6539aeb74..bef4ab3ed 100644
--- a/source/RobotAPI/drivers/GamepadUnit/CMakeLists.txt
+++ b/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
)
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
index 98b2e0b11..f844a6006 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
+++ b/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()));
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index caf8bde93..96be5c123 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/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;
};
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/Joystick.h b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
new file mode 100644
index 000000000..e8e3e4e7d
--- /dev/null
+++ b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
@@ -0,0 +1,94 @@
+/*
+ * 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
diff --git a/source/RobotAPI/interface/units/GamepadUnit.ice b/source/RobotAPI/interface/units/GamepadUnit.ice
index c55199511..255e95d28 100644
--- a/source/RobotAPI/interface/units/GamepadUnit.ice
+++ b/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.
--
GitLab