Fawkes API Fawkes Development Version
acquisition_thread.cpp
1
2/***************************************************************************
3 * acqusition_thread.cpp - Thread that retrieves the joystick data
4 *
5 * Created: Sat Nov 22 18:14:55 2008
6 * Copyright 2008 Tim Niemueller [www.niemueller.de]
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "acquisition_thread.h"
24
25#include "force_feedback.h"
26
27#include <core/exceptions/system.h>
28#include <core/threading/mutex.h>
29#include <linux/joystick.h>
30#include <sys/stat.h>
31#include <sys/types.h>
32#include <utils/time/time.h>
33
34#include <algorithm>
35#include <cerrno>
36#include <cstdlib>
37#include <cstring>
38#include <fcntl.h>
39#include <unistd.h>
40
41using namespace fawkes;
42
43#define COMBO_IDX_UP 0
44#define COMBO_IDX_DOWN 1
45#define COMBO_IDX_LEFT 2
46#define COMBO_IDX_RIGHT 3
47#define COMBO_IDX_RELEASE 4
48
49/** @class JoystickAcquisitionThread "acquisition_thread.h"
50 * Joystick acqusition thread for Linux joystick API.
51 * @see Linux Kernel Documentation (joystick-api.txt)
52 * @author Tim Niemueller
53 */
54
55/** Constructor. */
57: Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS)
58{
60 data_mutex_ = NULL;
61 axis_values_ = NULL;
62 bbhandler_ = NULL;
63 ff_ = NULL;
64 logger = NULL;
65}
66
67/** Alternative constructor.
68 * This constructor is meant to be used to create an instance that is used
69 * outside of Fawkes.
70 * @param device_file joystick device file
71 * @param handler BlackBoard handler that will post data to the BlackBoard
72 * @param logger logging instance
73 */
76 Logger * logger)
77: Thread("JoystickAcquisitionThread", Thread::OPMODE_CONTINUOUS)
78{
80 data_mutex_ = NULL;
81 axis_values_ = NULL;
82 ff_ = NULL;
83 bbhandler_ = handler;
84 this->logger = logger;
85 safety_lockout_ = true;
86 init(device_file);
87}
88
89void
91{
92 try {
93 cfg_device_file_ = config->get_string("/hardware/joystick/device_file");
94 cfg_retry_interval_ = config->get_float("/hardware/joystick/retry_interval");
95 } catch (Exception &e) {
96 e.append("Could not read all required config values for %s", name());
97 throw;
98 }
99
100 safety_lockout_ = true;
101 try {
102 safety_lockout_ = config->get_bool("/hardware/joystick/safety_lockout/enable");
103 } catch (Exception &e) {
104 } // ignore, use default
105 if (safety_lockout_) {
106 cfg_safety_lockout_timeout_ = config->get_float("/hardware/joystick/safety_lockout/timeout");
107 cfg_safety_button_mask_ = config->get_uint("/hardware/joystick/safety_lockout/button-mask");
108 cfg_safety_bypass_button_mask_ = 0;
109 try {
110 cfg_safety_bypass_button_mask_ =
111 config->get_uint("/hardware/joystick/safety_lockout/bypass-button-mask");
112 } catch (Exception &e) {
113 } // ignore, use default
114 }
115 for (int i = 0; i < 5; ++i)
116 safety_combo_[i] = false;
117
118 cfg_lazy_init_ = false;
119 try {
120 cfg_lazy_init_ = config->get_bool("/hardware/joystick/allow_deferred_initialization");
121 } catch (Exception &e) {
122 } // ignore, use default
123
124 try {
125 init(cfg_device_file_, cfg_lazy_init_);
126 } catch (Exception &e) {
127 if (!cfg_lazy_init_) {
128 e.append("Deferred initialization of joystick device disabled");
129 }
130 throw;
131 }
132
133 if (!connected_ && cfg_lazy_init_) {
134 logger->log_info(name(), "Cannot open joystick, deferred initialization enabled");
135 }
136
137 if (safety_lockout_) {
139 "To enable joystick, move primary cross all the way in all "
140 "directions while holding first button. Then let go of button.");
141 }
142}
143
144void
145JoystickAcquisitionThread::open_joystick()
146{
147 fd_ = open(cfg_device_file_.c_str(), O_RDONLY);
148 if (fd_ == -1) {
149 throw CouldNotOpenFileException(cfg_device_file_.c_str(),
150 errno,
151 "Opening the joystick device file failed");
152 }
153
154 if (ioctl(fd_, JSIOCGNAME(sizeof(joystick_name_)), joystick_name_) < 0) {
155 throw Exception(errno, "Failed to get name of joystick");
156 }
157 if (ioctl(fd_, JSIOCGAXES, &num_axes_) < 0) {
158 throw Exception(errno, "Failed to get number of axes for joystick");
159 }
160 if (ioctl(fd_, JSIOCGBUTTONS, &num_buttons_) < 0) {
161 throw Exception(errno, "Failed to get number of buttons for joystick");
162 }
163
164 if (axis_values_ == NULL) {
165 // memory had not been allocated
166 // minimum of 8 because there are 8 axes in the interface
167 axis_array_size_ = std::max((int)num_axes_, 8);
168 axis_values_ = (float *)malloc(sizeof(float) * axis_array_size_);
169 } else if (num_axes_ > std::max((int)axis_array_size_, 8)) {
170 // We loose axes as we cannot increase BB interface on-the-fly
171 num_axes_ = axis_array_size_;
172 }
173
174 logger->log_debug(name(), "Joystick device: %s", cfg_device_file_.c_str());
175 logger->log_debug(name(), "Joystick name: %s", joystick_name_);
176 logger->log_debug(name(), "Number of Axes: %i", num_axes_);
177 logger->log_debug(name(), "Number of Buttons: %i", num_buttons_);
178 logger->log_debug(name(), "Axis Array Size: %u", axis_array_size_);
179
180 memset(axis_values_, 0, sizeof(float) * axis_array_size_);
181 pressed_buttons_ = 0;
182
183 if (bbhandler_) {
184 bbhandler_->joystick_plugged(num_axes_, num_buttons_);
185 }
186 connected_ = true;
187 just_connected_ = true;
188}
189
190void
191JoystickAcquisitionThread::open_forcefeedback()
192{
193 ff_ = new JoystickForceFeedback(joystick_name_);
194 logger->log_debug(name(), "Force Feedback: %s", (ff_) ? "Yes" : "No");
195 logger->log_debug(name(), "Supported effects:");
196
197 if (ff_->can_rumble())
198 logger->log_debug(name(), " rumble");
199 if (ff_->can_periodic())
200 logger->log_debug(name(), " periodic");
201 if (ff_->can_constant())
202 logger->log_debug(name(), " constant");
203 if (ff_->can_spring())
204 logger->log_debug(name(), " spring");
205 if (ff_->can_friction())
206 logger->log_debug(name(), " friction");
207 if (ff_->can_damper())
208 logger->log_debug(name(), " damper");
209 if (ff_->can_inertia())
210 logger->log_debug(name(), " inertia");
211 if (ff_->can_ramp())
212 logger->log_debug(name(), " ramp");
213 if (ff_->can_square())
214 logger->log_debug(name(), " square");
215 if (ff_->can_triangle())
216 logger->log_debug(name(), " triangle");
217 if (ff_->can_sine())
218 logger->log_debug(name(), " sine");
219 if (ff_->can_saw_up())
220 logger->log_debug(name(), " saw up");
221 if (ff_->can_saw_down())
222 logger->log_debug(name(), " saw down");
223 if (ff_->can_custom())
224 logger->log_debug(name(), " custom");
225}
226
227void
228JoystickAcquisitionThread::init(const std::string &device_file, bool allow_open_fail)
229{
230 fd_ = -1;
231 connected_ = false;
232 just_connected_ = false;
233 new_data_ = false;
234
235 cfg_device_file_ = device_file;
236 try {
237 open_joystick();
238 try {
239 open_forcefeedback();
240 } catch (Exception &e) {
241 logger->log_warn(name(), "Initializing force feedback failed, disabling");
242 logger->log_warn(name(), e);
243 }
244 } catch (Exception &e) {
245 if (!allow_open_fail)
246 throw;
247 }
248 data_mutex_ = new Mutex();
249}
250
251void
253{
254 if (fd_ >= 0)
255 close(fd_);
256 if (axis_values_)
257 free(axis_values_);
258 delete data_mutex_;
259}
260
261void
263{
264 if (connected_) {
265 struct js_event e;
266
267 long int timeout_sec = (long int)truncf(cfg_safety_lockout_timeout_);
268 long int timeout_usec = (cfg_safety_lockout_timeout_ - timeout_sec) * 10000000;
269 timeval timeout = {timeout_sec, timeout_usec};
270
271 fd_set read_fds;
272 FD_ZERO(&read_fds);
273 FD_SET(fd_, &read_fds);
274
275 int rv = 0;
276 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
277
278 if (rv == 0) {
279 if (!safety_lockout_) {
281 "No action for %.2f seconds, re-enabling safety lockout",
282 cfg_safety_lockout_timeout_);
283 safety_lockout_ = true;
284 for (int i = 0; i < 5; ++i)
285 safety_combo_[i] = false;
286 }
287 new_data_ = false;
288 return;
289 }
290
291 if (rv == -1 || read(fd_, &e, sizeof(struct js_event)) < (int)sizeof(struct js_event)) {
292 logger->log_warn(name(), "Joystick removed, will try to reconnect.");
293 close(fd_);
294 fd_ = -1;
295 connected_ = false;
296 just_connected_ = false;
297 safety_lockout_ = true;
298 new_data_ = false;
299 if (bbhandler_) {
300 bbhandler_->joystick_unplugged();
301 }
302 return;
303 }
304
305 data_mutex_->lock();
306
307 new_data_ = !safety_lockout_;
308 unsigned int last_pressed_buttons = pressed_buttons_;
309
310 if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
311 //logger->log_debug(name(), "Button %u button event: %f", e.number, e.value);
312 if (e.number <= 32) {
313 if (e.value) {
314 pressed_buttons_ |= (1 << e.number);
315 } else {
316 pressed_buttons_ &= ~(1 << e.number);
317 }
318 } else {
319 logger->log_warn(name(), "Button value for button > 32, ignoring");
320 }
321 } else if ((e.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) {
322 if (e.number >= axis_array_size_) {
324 "Got value for axis %u, but only %u axes registered. "
325 "Plugged in a different joystick? Ignoring.",
326 e.number + 1 /* natural numbering */,
327 axis_array_size_);
328 } else {
329 // Joystick axes usually go positive right, down, twist right, min speed,
330 // hat right, and hat down. In the Fawkes coordinate system we actually
331 // want opposite directions, hence multiply each value by -1
332 axis_values_[e.number] = (e.value == 0) ? 0. : (e.value / -32767.f);
333
334 //logger->log_debug(name(), "Axis %u new X: %f",
335 // axis_index, axis_values_[e.number]);
336 }
337 }
338
339 // As a special case, allow a specific button combination to be
340 // written even during safety lockout. Can be used to implement
341 // an emergency stop, for example.
342 if (safety_lockout_
343 && ((cfg_safety_bypass_button_mask_ & pressed_buttons_)
344 || ((cfg_safety_bypass_button_mask_ & last_pressed_buttons)
345 && pressed_buttons_ == 0))) {
346 new_data_ = true;
347 }
348
349 data_mutex_->unlock();
350
351 if (safety_lockout_) {
352 // the actual axis directions don't matter, we are just interested
353 // that they take both extremes once.
354 if (num_axes_ < 2 || num_buttons_ == 0) {
355 safety_combo_[COMBO_IDX_UP] = true;
356 safety_combo_[COMBO_IDX_DOWN] = true;
357 safety_combo_[COMBO_IDX_RIGHT] = true;
358 safety_combo_[COMBO_IDX_LEFT] = true;
359 safety_combo_[COMBO_IDX_RELEASE] = true;
360 } else {
361 if (pressed_buttons_ & cfg_safety_button_mask_) {
362 if (axis_values_[0] > 0.9)
363 safety_combo_[COMBO_IDX_UP] = true;
364 if (axis_values_[0] < -0.9)
365 safety_combo_[COMBO_IDX_DOWN] = true;
366 if (axis_values_[1] > 0.9)
367 safety_combo_[COMBO_IDX_RIGHT] = true;
368 if (axis_values_[1] < -0.9)
369 safety_combo_[COMBO_IDX_LEFT] = true;
370 }
371 if (safety_combo_[COMBO_IDX_UP] && safety_combo_[COMBO_IDX_DOWN]
372 && safety_combo_[COMBO_IDX_LEFT] && safety_combo_[COMBO_IDX_RIGHT]
373 && pressed_buttons_ == 0) {
374 safety_combo_[COMBO_IDX_RELEASE] = true;
375 }
376 }
377
378 if (safety_combo_[COMBO_IDX_UP] && safety_combo_[COMBO_IDX_DOWN]
379 && safety_combo_[COMBO_IDX_LEFT] && safety_combo_[COMBO_IDX_RIGHT]
380 && safety_combo_[COMBO_IDX_RELEASE]) {
381 logger->log_warn(name(), "Joystick safety lockout DISABLED (combo received)");
382 safety_lockout_ = false;
383 }
384 } else {
385 if (bbhandler_) {
386 bbhandler_->joystick_changed(pressed_buttons_, axis_values_);
387 }
388 }
389 } else {
390 // Connection to joystick has been lost
391 try {
392 open_joystick();
393 logger->log_warn(name(), "Joystick plugged in. Delivering data again.");
394 try {
395 open_forcefeedback();
396 } catch (Exception &e) {
397 logger->log_warn(name(), "Initializing force feedback failed, disabling");
398 }
399 } catch (Exception &e) {
400 Time duration(cfg_retry_interval_);
401 duration.wait_systime();
402 }
403 }
404}
405
406/** Lock data if fresh.
407 * If new data has been received since get_distance_data() or get_echo_data()
408 * was called last the data is locked, no new data can arrive until you call
409 * unlock(), otherwise the lock is immediately released after checking.
410 * @return true if the lock was acquired and there is new data, false otherwise
411 */
412bool
414{
415 data_mutex_->lock();
416 if (new_data_ || just_connected_) {
417 just_connected_ = false;
418 return true;
419 } else {
420 data_mutex_->unlock();
421 return false;
422 }
423}
424
425/** Unlock data. */
426void
428{
429 new_data_ = false;
430 data_mutex_->unlock();
431}
432
433/** Get number of axes.
434 * @return number of axes.
435 */
436char
438{
439 return num_axes_;
440}
441
442/** Get number of buttons.
443 * @return number of buttons.
444 */
445char
447{
448 return num_buttons_;
449}
450
451/** Get joystick name.
452 * @return joystick name
453 */
454const char *
456{
457 return joystick_name_;
458}
459
460/** Pressed buttons.
461 * @return bit field where each set bit represents a pressed button.
462 */
463unsigned int
465{
466 if (!safety_lockout_) {
467 return pressed_buttons_;
468 } else if (pressed_buttons_ & cfg_safety_bypass_button_mask_) {
469 return pressed_buttons_ & cfg_safety_bypass_button_mask_;
470 } else {
471 return 0;
472 }
473}
474
475/** Get values for the axes.
476 * @return array of axis values.
477 */
478float *
480{
481 if (safety_lockout_) {
482 memset(axis_values_, 0, axis_array_size_ * sizeof(float));
483 }
484 return axis_values_;
485}
virtual void finalize()
Finalize the thread.
bool lock_if_new_data()
Lock data if fresh.
virtual void init()
Initialize the thread.
const char * joystick_name() const
Get joystick name.
unsigned int pressed_buttons() const
Pressed buttons.
char num_buttons() const
Get number of buttons.
virtual void loop()
Code to execute in the thread.
char num_axes() const
Get number of axes.
float * axis_values()
Get values for the axes.
Handler class for joystick data.
Definition: bb_handler.h:27
virtual void joystick_changed(unsigned int pressed_buttons, float *axis_values)=0
Joystick data changed.
virtual void joystick_unplugged()=0
The joystick has been unplugged and is no longer available.
virtual void joystick_plugged(char num_axes, char num_buttons)=0
A (new) joystick has been plugged in.
Cause force feedback on a joystick.
bool can_spring()
Check if spring effect is supported.
bool can_inertia()
Check if inertia effect is supported.
bool can_ramp()
Check if ramp effect is supported.
bool can_friction()
Check if friction effect is supported.
bool can_square()
Check if square effect is supported.
bool can_saw_down()
Check if downward saw effect is supported.
bool can_triangle()
Check if triangle effect is supported.
bool can_rumble()
Check if rumbling effect is supported.
bool can_periodic()
Check if periodic effect is supported.
bool can_sine()
Check if sine effect is supported.
bool can_custom()
Check if custom effect is supported.
bool can_damper()
Check if damper effect is supported.
bool can_saw_up()
Check if upward saw effect is supported.
bool can_constant()
Check if constant effect is supported.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
File could not be opened.
Definition: system.h:53
Base class for exceptions in Fawkes.
Definition: exception.h:36
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex mutual exclusion lock.
Definition: mutex.h:33
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
Thread class encapsulation of pthreads.
Definition: thread.h:46
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Definition: thread.cpp:716
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
void wait_systime()
Wait (sleep) for this system time.
Definition: time.cpp:760
Fawkes library namespace.