diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
index 1431b8b5219ef4622d21024132397690a81d9526..1e7dacc348c3101705f4ae650ab978b031cd23d7 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
+++ b/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()));
}
-
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index 109ce81a17a98c5154b396e09064d9943be6ff0a..a2008600dc1f50432bbbf3f00c646153c7d7e83e 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/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;
};
}
-
diff --git a/source/RobotAPI/drivers/GamepadUnit/Joystick.h b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
index 1f39074b7a1449f9d58b643d42be68f2c123c445..8befbe9d5c08bd6d13220cd7c709884c906afc7b 100644
--- a/source/RobotAPI/drivers/GamepadUnit/Joystick.h
+++ b/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
diff --git a/source/RobotAPI/interface/units/GamepadUnit.ice b/source/RobotAPI/interface/units/GamepadUnit.ice
index 671e1de9b99591f0d4e23ab923feac8a5e1d535d..c63ddcfdad36942cfcca6c65155c8e96c6847f79 100644
--- a/source/RobotAPI/interface/units/GamepadUnit.ice
+++ b/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
};
};
-