YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
SDLJoypad.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
6#include "SDLJoypad.h"
7#include <cstdio>
8#include <yarp/os/LogStream.h>
9#include <cmath>
10#include <yarp/os/Time.h>
11#include <iostream>
12
13
14using namespace yarp::dev;
15using namespace yarp::sig;
16using namespace yarp::os;
17using namespace SDLJoypadImpl;
18
19namespace {
20YARP_LOG_COMPONENT(SDLJOYPAD, "yarp.device.SDLJoypad")
22}
23
24
25
27
29
30bool SDLJoypad::open(yarp::os::Searchable& rf)
31{
32/* if(rf.check("help"))
33 {
34 yCInfo(SDLJOYPAD) << "parameters:";
35 yCInfo(SDLJOYPAD);
36 yCInfo(SDLJOYPAD) << "UseAllJoypadAsOne - set it to 1 to have all the connected joypad as one";
37 yCInfo(SDLJOYPAD) << "DefaultJoystickNumber - select the id of the joypad to use if there are more than one joypad and UseAllJoypadAsOne is set to 0";
38 yCInfo(SDLJOYPAD) << "stick - the number of stick to configure. a stick is simply a wrapping of 2 or more axes so for every stick";
39 yCInfo(SDLJOYPAD) << " a group named STICK*ID* containing the stick's parameters is searched. ";
40 yCInfo(SDLJOYPAD);
41 yCInfo(SDLJOYPAD) << "stick groups parameters:";
42 yCInfo(SDLJOYPAD) << "axes - axes count for this stick";
43 yCInfo(SDLJOYPAD) << "axis[ID]_id - axis id for current stick related axis";
44 yCInfo(SDLJOYPAD) << "invert_axis_[ID] - invert the current axis";
45 yCInfo(SDLJOYPAD) << "deadZone - set the deadzone for this stick";
46 return false;
47 }
48*/
49
50 int joy_id;
51 size_t joystick_num;
52 int actionCount;
53
55 {
56 yCError(SDLJOYPAD, "Unable to initialize Joystick: %s", SDL_GetError());
57 return false;
58 }
59
60 joy_id = 0;
62 actionCount = 0;
63
64 if (joystick_num == 0)
65 {
66 yCError(SDLJOYPAD, "No joysticks found");
67 return false;
68 }
69 else if (joystick_num == 1)
70 {
71 joy_id = 0;
72 yCInfo(SDLJOYPAD, "One joystick found");
73 yCInfo(SDLJOYPAD, "Using joystick: %s", SDL_JoystickName(joy_id));
74 }
75 else
76 {
77 yCInfo(SDLJOYPAD, "More than one joystick found:");
78 for (size_t i = 0; i < joystick_num; i++)
79 {
80 yCInfo(SDLJOYPAD) << i << ":" << SDL_JoystickName(i);
81 }
83 if(!rf.check("UseAllJoypadAsOne"))
84 {
85 if(rf.find("UseAllJoypadAsOne").asBool())
86 {
87 // choose between multiple joysticks
88 if (rf.check("DefaultJoystickNumber"))
89 {
90 joy_id = rf.find("DefaultJoystickNumber").asInt32();
91 yCInfo(SDLJOYPAD, "Multiple joysticks found, using #%d, as specified in the configuration options", joy_id);
92 }
93 else
94 {
95 yCWarning(SDLJOYPAD, "No default joystick specified in the configuration options");
96 yCWarning(SDLJOYPAD, "Which joystick you want to use? (choose number)");
97 std::cin >> joy_id;
98 }
99 m_allJoypad = false;
100 }
101 else
102 {
103 m_allJoypad = true;
104 }
105 }
106 else
107 {
108 yCError(SDLJOYPAD) << "Missing UseAllJoypadAsOne parameter";
109 return false;
110 }
111 }
112
113 if(m_allJoypad)
114 {
115 for(size_t i = 0; i < joystick_num; ++i)
116 {
117 m_device.push_back(SDL_JoystickOpen(i));
118 }
119 }
120 else
121 {
122 m_device.push_back(SDL_JoystickOpen(joy_id));
123 }
124
125 for(size_t i = 0; i < m_device.size(); ++i)
126 {
127 if ( m_device[i] == nullptr )
128 {
129 yCError(SDLJOYPAD) << "Could not open joystick with id" << i;
130 return false;
131 }
132
133 // Obtaining Joystick capabilities
134 m_axisCount += SDL_JoystickNumAxes(m_device[i]);
135 m_ballCount += SDL_JoystickNumBalls(m_device[i]);
136 m_hatCount += SDL_JoystickNumHats(m_device[i]);
137 m_buttonCount += SDL_JoystickNumButtons(m_device[i]);
138 }
139
140 if(m_actionsHandler.parseActions(rf, m_buttonCount, &actionCount))
141 {
142 if(actionCount)
143 {
145 {
146 yCError(SDLJOYPAD) << "SDLJoypad:" << SDL_GetError();
147 return false;
148 }
149 yCInfo(SDLJOYPAD) << "Actions successfully parsed and linked to the joypad";
150 }
151 }
152 else
153 {
154 yCError(SDLJOYPAD) << "Error while parsing actions";
155 return false;
156 }
157
158 if(!parseStickInfo(rf)){return false;}
159 return true;
160}
161
162bool SDLJoypad::parseStickInfo(const yarp::os::Searchable& cfg)
163{
164 if(!cfg.check("sticks") || !cfg.find("sticks").isInt32())
165 {
166 yCError(SDLJOYPAD) << "Missing 'sticks' parameter or not an integer";
167 return false;
168 }
169
170 for(size_t i = 0; i < m_axisCount; i++)
171 {
172 m_axes.push_back(true);
173 }
174
175 m_stickCount = cfg.find("sticks").asInt32();
176 for(size_t i = 0; i < m_stickCount; i++)
177 {
178 std::string stickName;
179 int axesCount;
181
182 stickName = "STICK"+std::to_string(i);
183
184 if(!cfg.check(stickName))
185 {
186 yCError(SDLJOYPAD) << "Missing" << stickName << "group in configuration";
187 return false;
188 }
189
190 Bottle& stickParams = cfg.findGroup(stickName);
191
192 if(0 == stickParams.size())
193 {
194 yCError(SDLJOYPAD) << "Group" << stickName << "is empty";
195 return false;
196 }
197
198 if(!stickParams.check("axes") || !stickParams.find("axes").isInt32())
199 {
200 yCError(SDLJOYPAD) << "Missing 'axes' count in" << stickName << "group or not an integer";
201 return false;
202 }
203
204 axesCount = stickParams.find("axes").asInt32();
205
206 for(int j = 0; j < axesCount; j++)
207 {
208 std::string axisName, invertName;
209 size_t axis_id;
210 axisName = "axis" + std::to_string(j) + "_id";
211 invertName = "invert_axis_" + std::to_string(j);
212
213 if(!stickParams.check(axisName) || !stickParams.find(axisName).isInt32())
214 {
215 yCError(SDLJOYPAD) << "Missing" << axisName << "param in" << stickName << "group or not an integer.";
216 return false;
217 }
218
219 axis_id = (size_t)stickParams.find(axisName).asInt32();
220 if(axis_id > m_axisCount - 1)
221 {
222 yCError(SDLJOYPAD) << "Axis id out of bound";
223 return false;
224 }
225
226 if(!stickParams.check(invertName) || !stickParams.find(invertName).isBool())
227 {
228 yCError(SDLJOYPAD) << "Missing" << invertName << "param in" << stickName << "group or not an bool.";
229 return false;
230 }
231
232 currentStick.axes_ids.push_back(axis_id);
233 currentStick.direction.push_back(stickParams.find(invertName).asBool() ? -1 : 1);
234 m_axes[axis_id] = false;
235 }
236
237 if(!stickParams.check("deadZone") || !stickParams.find("deadZone").isFloat64())
238 {
239 yCError(SDLJOYPAD) << "Missing deadZone param in" << stickName << "group or not an double.";
240 return false;
241 }
242
243 currentStick.deadZone = stickParams.find("deadZone").asFloat64();
244 m_sticks.push_back(currentStick);
245 }
246 return true;
247}
248
250{
251 return false;
252}
253
255{
256 axes_count = m_axisCount;
257 return ReturnValue_ok;
258}
259
261{
262 button_count = m_buttonCount;
263 return ReturnValue_ok;
264}
265
271
273{
274 hat_count = m_hatCount;
275 return ReturnValue_ok;
276}
277
279{
280 touch_count = m_touchCount;
281 return ReturnValue_ok;
282}
283
285{
286 stick_count = m_stickCount;
287 return ReturnValue_ok;
288}
289
290ReturnValue SDLJoypad::getButton(size_t button_id, double& value)
291{
292 if(button_id > m_buttonCount - 1)
293 {
294 yCError(SDLJOYPAD) << "Button id out of bound!";
296 }
297 updateJoypad();
298 size_t i;
299 for(i = 0; i < m_device.size(); ++i)
300 {
301 size_t localCount = SDL_JoystickNumButtons(m_device[i]);
302 if(button_id > localCount - 1)
303 {
304 button_id -= localCount;
305 }
306 else
307 {
308 break;
309 }
310 }
311
312 value = float(SDL_JoystickGetButton(m_device[i], button_id));
313 if(value > 0.5 && m_actionsHandler.actionDetected(button_id) &&
314 yarp::os::Time::now() - m_actionTimestamp > actionsExecutionTime)
315 {
316 m_actionsHandler.executeAction(button_id);
317 m_actionTimestamp = yarp::os::Time::now();
318 }
319 return ReturnValue_ok;
320}
321
322ReturnValue SDLJoypad::getPureAxis(size_t axis_id, double& value)
323{
324 if(axis_id > m_axisCount - 1)
325 {
326 yCError(SDLJOYPAD) << "Axis id out of bound!";
328 }
329 size_t i;
330
331 for(i = 0; i < m_device.size(); ++i)
332 {
333 size_t localCount;
334 localCount = SDL_JoystickNumAxes(m_device[i]);
335 if(axis_id > localCount - 1)
336 {
337 axis_id -= localCount;
338 }
339 else
340 {
341 break;
342 }
343 }
344
345 value = 2 * ((float(SDL_JoystickGetAxis(m_device[i], axis_id)) - (-32.768)) / 0xffff);
346 return ReturnValue_ok;
347}
348
349ReturnValue SDLJoypad::getAxis(size_t axis_id, double& value)
350{
351 if(axis_id > m_axisCount - 1)
352 {
353 yCError(SDLJOYPAD) << "Axis id out of bound!";
355 }
356 //if(!m_axes.at(axis_id)) {yCWarning(SDLJOYPAD) << "Requested axis is part of a stick!";}
357 updateJoypad();
358 return getPureAxis(axis_id, value);
359}
360
361yarp::sig::Vector Vector3(const double& x, const double& y)
362{
363 Vector ret;
364 ret.push_back(x);
365 ret.push_back(y);
366 return ret;
367}
368
370{
371 if (stick_id > m_stickCount - 1)
372 {
373 yCError(SDLJOYPAD) << "Stick id out of bound!";
375 }
376 updateJoypad();
377 stick& stk_config = m_sticks[stick_id];
378 double val;
379
380 if (stk_config.axes_ids.size() > 2)
381 {
382 yCError(SDLJOYPAD) << "Only for 2-dimensional sticks are supported at the moment";
384 }
385
386 for(size_t i = 0; i < stk_config.axes_ids.size(); i++)
387 {
388 if(!getAxis(stk_config.axes_ids[i], val)) {return ReturnValue_error_method_failed;}
389 value.s1=(val * stk_config.direction[i] * (fabs(val) > stk_config.deadZone));
390 value.s2=(val * stk_config.direction[i] * (fabs(val) > stk_config.deadZone));
391 }
392
394 {
395 double v0= value.s1;
396 double v1= value.s2;
397 value.s1 = sqrt(v0 * v0 + v1 * v1);
398 value.s2 = atan2(v0, v1);
399 }
400 return ReturnValue_ok;
401}
402
403ReturnValue SDLJoypad:: getTouch(size_t touch_id, std::vector<yarp::dev::TouchData>& value)
404{
406}
407
408ReturnValue SDLJoypad::getHat(size_t hat_id, unsigned char& value)
409{
410 if(hat_id > m_hatCount - 1)
411 {
412 yCError(SDLJOYPAD) << "Axis id out of bound!";
414 }
415 updateJoypad();
416 size_t i;
417 for(i = 0; i < m_device.size(); ++i)
418 {
419 size_t localCount = SDL_JoystickNumHats(m_device[i]);
420 if(hat_id > localCount - 1)
421 {
422 hat_id -= localCount;
423 }
424 else
425 {
426 break;
427 }
428 }
429
430 value = SDL_JoystickGetHat(m_device[i], hat_id);//TODO: map from their HAT define to our in case of #define changes
431 return ReturnValue_ok;
432}
433
435{
436 if(trackball_id > m_ballCount - 1)
437 {
438 yCError(SDLJOYPAD) << "Trackball id out of bound!";
440 }
441 updateJoypad();
442 int x, y;
443 size_t i;
444 for(i = 0; i < m_device.size(); ++i)
445 {
446 size_t localCount = SDL_JoystickNumBalls(m_device[i]);
447 if(trackball_id > localCount - 1)
448 {
449 trackball_id -= localCount;
450 }
451 else
452 {
453 break;
454 }
455 }
456 if(SDL_JoystickGetBall(m_device[i], trackball_id, &x, &y) == -1)
457 {
458 yCError(SDLJOYPAD) << "SDL_JoystickGetBall returned error";
460 }
461
462 value.x = x;
463 value.y = y;
464 return ReturnValue_ok;
465}
466
467void SDLJoypad::updateJoypad()
468{
470}
471
472// --------------------------------------------------------------
473
475{
476 if (m_actions.find(action_id) != m_actions.end())
477 {
478 yCInfo(SDLJOYPAD) << "executing script" << action_id << ":" << m_actions[action_id];
479 int ret = system(m_actions[action_id].c_str());
480 YARP_UNUSED(ret);
481 }
482 else
483 {
484 yCWarning(SDLJOYPAD) << "no scripts associated to button" << action_id;
485 return false;
486 }
487 return true;
488}
489
491{
492 int dummy = 0;
493 size_t i;
494 int& actCount = count ? *count : dummy;
495 if(!cfg.check(buttActionGroupName))
496 {
497 yCInfo(SDLJOYPAD) << "no actions found in the configuration file (no" << buttActionGroupName << "group found)";
498 actCount = 0;
499 return true;
500 }
502
503 if(!actionsGroup.size())
504 {
505 yCError(SDLJOYPAD) << "no action found under" << buttActionGroupName << "group";
506 actCount = 0;
507 return false;
508 }
509
510 for(i = 1; i < actionsGroup.size(); i++)
511 {
512 if(!actionsGroup.get(i).isList())
513 {
514 yDebug() << "error parsing cfg";
515 return false;
516 }
517
518 Bottle& keyvalue = *actionsGroup.get(i).asList();
519 yDebug() << keyvalue.toString();
520 if(maxButtons==0)
521 {
522 yCError(SDLJOYPAD) << "invalid button count";
523 actCount = 0;
524 return false;
525 }
526 if(!keyvalue.get(0).isInt32() ||
527 keyvalue.get(0).asInt32() < 0 ||
528 (unsigned int) keyvalue.get(0).asInt32() > maxButtons-1 ||
529 !keyvalue.get(1).isString())
530 {
531 yCError(SDLJOYPAD) << "Button's actions parameters must be in the format 'unsigned int string' and the button id must be in range";
532 actCount = 0;
533 return false;
534 }
535 yCInfo(SDLJOYPAD) << "assigning actions" << keyvalue.get(1).asString() << "to button" << keyvalue.get(0).asInt32();
536 m_actions[keyvalue.get(0).asInt32()] = keyvalue.get(1).asString();
537 }
538
539 actCount = i;
540 yCInfo(SDLJOYPAD) << actCount << "action parsed successfully";
541 return true;
542}
543
545{
546 if (m_actions.find(button_id) != m_actions.end())
547 {
548 return true;
549 }
550 return false;
551}
#define yDebug(...)
Definition Log.h:275
#define ReturnValue_ok
Definition ReturnValue.h:82
#define ReturnValue_error_method_failed
Definition ReturnValue.h:87
#define ReturnValue_error_not_implemented_by_device
Definition ReturnValue.h:84
#define ReturnValue_error_input_out_of_bounds
Definition ReturnValue.h:89
yarp::sig::Vector Vector3(const double &x, const double &y)
const std::string buttActionGroupName
Definition SDLJoypad.h:35
std::map< int, std::string > m_actions
Definition SDLJoypad.h:33
bool parseActions(const yarp::os::Searchable &cfg, size_t maxButtons, int *count=nullptr)
bool executeAction(int action_id)
bool actionDetected(int button_id)
SDLJoypad: Device that reads inputs of Joypads compatible with the SDL library.
Definition SDLJoypad.h:64
yarp::dev::ReturnValue getAxis(size_t axis_id, double &value) override
Get the value of an axis if present, return false otherwise.
yarp::dev::ReturnValue getHat(size_t hat_id, unsigned char &value) override
Get the value of an Hat.
yarp::dev::ReturnValue getAxisCount(size_t &axis_count) override
Get number of axes.
yarp::dev::ReturnValue getHatCount(size_t &hat_count) override
Get number of hats.
yarp::dev::ReturnValue getTrackball(size_t trackball_id, yarp::dev::TrackballData &value) override
Get the axes change of a Trackball.
yarp::dev::ReturnValue getTouchSurfaceCount(size_t &touch_count) override
Get the number of touch surface.
yarp::dev::ReturnValue getButton(size_t button_id, double &value) override
Get the value of a button.
yarp::dev::ReturnValue getStick(size_t stick_id, yarp::dev::StickData &value, JoypadCtrl_coordinateMode coordinate_mode) override
Get the value of a stick if present, return false otherwise.
yarp::dev::ReturnValue getTrackballCount(size_t &trackball_count) override
Get number of trackballs.
yarp::dev::ReturnValue getButtonCount(size_t &button_count) override
Get number of buttons.
yarp::dev::ReturnValue getStickCount(size_t &stick_count) override
Get the number of the sticks.
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue getTouch(size_t touch_id, std::vector< yarp::dev::TouchData > &value) override
Get the value of a touch if present, return false otherwise.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
Definition Searchable.h:31
void push_back(const Scalar &elem)
Push a new element in the vector: size is changed.
Definition VectorBase.h:225
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.
Definition dirs.h:16
#define YARP_UNUSED(var)
Definition api.h:162