| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | |
| 6 |
| See: https://www.mrpt.org/Authors - All rights reserved. | |
| 7 |
| Released under BSD License. See: https://www.mrpt.org/License | |
| 8 |
+------------------------------------------------------------------------+ */ |
| 9 |
|
| 10 |
#include "hwdrivers-precomp.h" // Precompiled headers |
| 11 |
// |
| 12 |
#include <mrpt/config.h> |
| 13 |
#include <mrpt/core/exceptions.h> |
| 14 |
|
| 15 |
#ifdef _WIN32 |
| 16 |
#define WIN32_LEAN_AND_MEAN |
| 17 |
#include <windows.h> |
| 18 |
// |
| 19 |
#include <mmsystem.h> |
| 20 |
|
| 21 |
#if !defined(__GNUC__) |
| 22 |
#pragma comment(lib, "WINMM.LIB") |
| 23 |
#endif |
| 24 |
#endif |
| 25 |
|
| 26 |
#if defined(MRPT_OS_LINUX) || defined(__APPLE__) |
| 27 |
// Linux |
| 28 |
#include <fcntl.h> |
| 29 |
#include <sys/ioctl.h> |
| 30 |
#include <sys/time.h> |
| 31 |
#include <sys/types.h> |
| 32 |
#include <unistd.h> |
| 33 |
|
| 34 |
#include <cerrno> |
| 35 |
#include <cstdio> |
| 36 |
#include <cstdlib> |
| 37 |
#include <cstring> |
| 38 |
|
| 39 |
#if defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H) |
| 40 |
#include <linux/input.h> |
| 41 |
#include <linux/joystick.h> |
| 42 |
#endif |
| 43 |
#endif |
| 44 |
|
| 45 |
#include <mrpt/hwdrivers/CJoystick.h> |
| 46 |
|
| 47 |
using namespace mrpt::hwdrivers; |
| 48 |
|
| 49 |
CJoystick::CJoystick() = default; |
| 50 |
|
| 51 |
/*--------------------------------------------------------------- |
| 52 |
Destructor |
| 53 |
---------------------------------------------------------------*/ |
| 54 |
CJoystick::~CJoystick() |
| 55 |
{ |
| 56 |
#if defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H) |
| 57 |
// Close joystick, if open: |
| 58 |
if (m_joy_fd > 0) |
| 59 |
{ |
| 60 |
close(m_joy_fd); |
| 61 |
m_joy_fd = -1; |
| 62 |
} |
| 63 |
#endif |
| 64 |
} |
| 65 |
|
| 66 |
/*--------------------------------------------------------------- |
| 67 |
getJoysticksCount |
| 68 |
Returns the number of Joysticks in the computer. |
| 69 |
---------------------------------------------------------------*/ |
| 70 |
int CJoystick::getJoysticksCount() |
| 71 |
{ |
| 72 |
MRPT_START |
| 73 |
#ifdef _WIN32 |
| 74 |
return joyGetNumDevs(); |
| 75 |
#elif defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H) |
| 76 |
// Try to open several joy devs: |
| 77 |
int joy_fd = -1; |
| 78 |
int nJoys = 0; |
| 79 |
|
| 80 |
do |
| 81 |
{ |
| 82 |
if (-1 != (joy_fd = open(format("/dev/input/js%i", nJoys).c_str(), O_RDONLY))) |
| 83 |
{ |
| 84 |
nJoys++; |
| 85 |
close(joy_fd); |
| 86 |
} |
| 87 |
} while (joy_fd != -1); |
| 88 |
|
| 89 |
return nJoys; |
| 90 |
#else |
| 91 |
// Apple: |
| 92 |
return 0; |
| 93 |
#endif |
| 94 |
MRPT_END |
| 95 |
} |
| 96 |
|
| 97 |
/*--------------------------------------------------------------- |
| 98 |
Gets joystick information. |
| 99 |
|
| 100 |
\return Returns true if successful, false on error, for example, if joystick |
| 101 |
is not present. |
| 102 |
---------------------------------------------------------------*/ |
| 103 |
bool CJoystick::getJoystickPosition(int nJoy, State& output) |
| 104 |
{ |
| 105 |
MRPT_START |
| 106 |
#ifdef _WIN32 |
| 107 |
JOYINFO jinfo; |
| 108 |
|
| 109 |
int ID = JOYSTICKID1 + nJoy; |
| 110 |
|
| 111 |
// Get joy pos: |
| 112 |
if (JOYERR_NOERROR != joyGetPos(ID, &jinfo)) return false; // Error. |
| 113 |
|
| 114 |
int calib_min[3], calib_max[3]; |
| 115 |
|
| 116 |
for (int i = 0; i < 3; i++) |
| 117 |
{ |
| 118 |
calib_min[i] = m_minPerAxis.size() > i ? m_minPerAxis[i] : 0; |
| 119 |
calib_max[i] = m_maxPerAxis.size() > i ? m_maxPerAxis[i] : 0xffff; |
| 120 |
} |
| 121 |
|
| 122 |
// Output data: |
| 123 |
float x = (jinfo.wXpos - calib_min[0]) / (float)(calib_max[0] - calib_min[0]); |
| 124 |
float y = (jinfo.wYpos - calib_min[1]) / (float)(calib_max[1] - calib_min[1]); |
| 125 |
float z = (jinfo.wZpos - calib_min[2]) / (float)(calib_max[2] - calib_min[2]); |
| 126 |
|
| 127 |
x = 2 * x - 1; |
| 128 |
y = 2 * y - 1; |
| 129 |
z = 2 * z - 1; |
| 130 |
|
| 131 |
output.buttons.resize(4); |
| 132 |
|
| 133 |
output.buttons[0] = 0 != (jinfo.wButtons & JOY_BUTTON1); |
| 134 |
output.buttons[1] = 0 != (jinfo.wButtons & JOY_BUTTON2); |
| 135 |
output.buttons[2] = 0 != (jinfo.wButtons & JOY_BUTTON3); |
| 136 |
output.buttons[3] = 0 != (jinfo.wButtons & JOY_BUTTON4); |
| 137 |
|
| 138 |
output.axes_raw.resize(3); |
| 139 |
output.axes_raw[0] = jinfo.wXpos; |
| 140 |
output.axes_raw[1] = jinfo.wYpos; |
| 141 |
output.axes_raw[2] = jinfo.wZpos; |
| 142 |
|
| 143 |
output.axes.resize(3); |
| 144 |
output.axes[0] = x; |
| 145 |
output.axes[1] = y; |
| 146 |
output.axes[2] = z; |
| 147 |
|
| 148 |
return true; |
| 149 |
#elif defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H) |
| 150 |
// Already open? |
| 151 |
if (m_joy_index == nJoy && m_joy_fd != -1) |
| 152 |
{ |
| 153 |
// Ok |
| 154 |
} |
| 155 |
else |
| 156 |
{ |
| 157 |
// Close previous opened joy? |
| 158 |
if (m_joy_fd != -1) close(m_joy_fd); |
| 159 |
|
| 160 |
// Go, try open joystick: |
| 161 |
if ((m_joy_fd = open(format("/dev/input/js%i", nJoy).c_str(), O_RDONLY)) < 0) return false; |
| 162 |
|
| 163 |
// Perfect! |
| 164 |
m_joy_index = nJoy; |
| 165 |
|
| 166 |
// Read in non-blocking way: **** Refer to sources of "jstest"!!!! *** |
| 167 |
fcntl(m_joy_fd, F_SETFL, O_NONBLOCK); |
| 168 |
} |
| 169 |
|
| 170 |
struct js_event js; |
| 171 |
|
| 172 |
while (read(m_joy_fd, &js, sizeof(struct js_event)) == sizeof(struct js_event)) |
| 173 |
{ |
| 174 |
// js.number: Button number |
| 175 |
const size_t jsNum = static_cast<size_t>(js.number); |
| 176 |
|
| 177 |
// Button? |
| 178 |
if (js.type & JS_EVENT_BUTTON) |
| 179 |
{ |
| 180 |
if (m_joystate_btns.size() < jsNum + 1) m_joystate_btns.resize(jsNum + 1); |
| 181 |
|
| 182 |
m_joystate_btns[jsNum] = (js.value != 0); |
| 183 |
} |
| 184 |
|
| 185 |
// Axes? |
| 186 |
if (js.type & JS_EVENT_AXIS) |
| 187 |
{ |
| 188 |
if (m_joystate_axes.size() < jsNum + 1) m_joystate_axes.resize(jsNum + 1); |
| 189 |
|
| 190 |
m_joystate_axes[jsNum] = static_cast<int>(js.value); |
| 191 |
} |
| 192 |
} |
| 193 |
|
| 194 |
if (errno != EAGAIN) |
| 195 |
{ |
| 196 |
// Joystick disconnected? |
| 197 |
m_joy_fd = -1; |
| 198 |
m_joy_index = -1; |
| 199 |
return false; |
| 200 |
} |
| 201 |
|
| 202 |
// Fill out data: |
| 203 |
output.buttons = m_joystate_btns; |
| 204 |
|
| 205 |
const size_t nAxis = m_joystate_axes.size(); |
| 206 |
output.axes_raw.resize(nAxis); |
| 207 |
output.axes.resize(nAxis); |
| 208 |
|
| 209 |
for (size_t i = 0; i < nAxis; i++) |
| 210 |
{ |
| 211 |
output.axes_raw[i] = m_joystate_axes[i]; |
| 212 |
|
| 213 |
const int calib_min = m_minPerAxis.size() > i ? m_minPerAxis[i] : -32767; |
| 214 |
const int calib_max = m_maxPerAxis.size() > i ? m_maxPerAxis[i] : 32767; |
| 215 |
|
| 216 |
output.axes[i] = |
| 217 |
-1.0f + 2.0f * (m_joystate_axes[i] - calib_min) / static_cast<float>(calib_max - calib_min); |
| 218 |
} |
| 219 |
|
| 220 |
return true; |
| 221 |
#else |
| 222 |
// Apple. |
| 223 |
return false; |
| 224 |
#endif |
| 225 |
MRPT_END |
| 226 |
} |
| 227 |
|
| 228 |
void CJoystick::setLimits(const std::vector<int>& minPerAxis, const std::vector<int>& maxPerAxis) |
| 229 |
{ |
| 230 |
m_minPerAxis = minPerAxis; |
| 231 |
m_maxPerAxis = maxPerAxis; |
| 232 |
} |