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 
41 using 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 
89 void
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_) {
138  logger->log_info(name(),
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 
144 void
145 JoystickAcquisitionThread::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 
190 void
191 JoystickAcquisitionThread::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 
227 void
228 JoystickAcquisitionThread::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 
251 void
253 {
254  if (fd_ >= 0)
255  close(fd_);
256  if (axis_values_)
257  free(axis_values_);
258  delete data_mutex_;
259 }
260 
261 void
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_) {
280  logger->log_warn(name(),
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_) {
323  logger->log_warn(name(),
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  */
412 bool
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. */
426 void
428 {
429  new_data_ = false;
430  data_mutex_->unlock();
431 }
432 
433 /** Get number of axes.
434  * @return number of axes.
435  */
436 char
438 {
439  return num_axes_;
440 }
441 
442 /** Get number of buttons.
443  * @return number of buttons.
444  */
445 char
447 {
448  return num_buttons_;
449 }
450 
451 /** Get joystick name.
452  * @return joystick name
453  */
454 const char *
456 {
457  return joystick_name_;
458 }
459 
460 /** Pressed buttons.
461  * @return bit field where each set bit represents a pressed button.
462  */
463 unsigned 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  */
478 float *
480 {
481  if (safety_lockout_) {
482  memset(axis_values_, 0, axis_array_size_ * sizeof(float));
483  }
484  return axis_values_;
485 }
fawkes::Mutex::lock
void lock()
Lock this mutex.
Definition: mutex.cpp:87
JoystickAcquisitionThread::joystick_name
const char * joystick_name() const
Get joystick name.
Definition: acquisition_thread.cpp:455
JoystickForceFeedback::can_spring
bool can_spring()
Check if spring effect is supported.
Definition: force_feedback.h:74
fawkes::Thread::set_prepfin_conc_loop
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Definition: thread.cpp:716
JoystickBlackBoardHandler::joystick_plugged
virtual void joystick_plugged(char num_axes, char num_buttons)=0
A (new) joystick has been plugged in.
JoystickAcquisitionThread::num_buttons
char num_buttons() const
Get number of buttons.
Definition: acquisition_thread.cpp:446
fawkes::Mutex
Mutex mutual exclusion lock.
Definition: mutex.h:33
JoystickForceFeedback::can_ramp
bool can_ramp()
Check if ramp effect is supported.
Definition: force_feedback.h:94
JoystickForceFeedback::can_damper
bool can_damper()
Check if damper effect is supported.
Definition: force_feedback.h:84
JoystickAcquisitionThread::finalize
virtual void finalize()
Finalize the thread.
Definition: acquisition_thread.cpp:252
JoystickForceFeedback::can_saw_down
bool can_saw_down()
Check if downward saw effect is supported.
Definition: force_feedback.h:119
JoystickForceFeedback::can_triangle
bool can_triangle()
Check if triangle effect is supported.
Definition: force_feedback.h:104
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
JoystickForceFeedback::can_rumble
bool can_rumble()
Check if rumbling effect is supported.
Definition: force_feedback.h:59
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
JoystickBlackBoardHandler::joystick_changed
virtual void joystick_changed(unsigned int pressed_buttons, float *axis_values)=0
Joystick data changed.
JoystickForceFeedback
Cause force feedback on a joystick.
Definition: force_feedback.h:31
JoystickAcquisitionThread::loop
virtual void loop()
Code to execute in the thread.
Definition: acquisition_thread.cpp:262
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
JoystickForceFeedback::can_periodic
bool can_periodic()
Check if periodic effect is supported.
Definition: force_feedback.h:64
fawkes::Mutex::unlock
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
JoystickAcquisitionThread::init
virtual void init()
Initialize the thread.
Definition: acquisition_thread.cpp:90
JoystickAcquisitionThread::unlock
void unlock()
Unlock data.
Definition: acquisition_thread.cpp:427
JoystickForceFeedback::can_friction
bool can_friction()
Check if friction effect is supported.
Definition: force_feedback.h:79
fawkes::Exception::append
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
fawkes::CouldNotOpenFileException
File could not be opened.
Definition: system.h:53
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::Logger
Interface for logging.
Definition: logger.h:42
fawkes
Fawkes library namespace.
JoystickForceFeedback::can_saw_up
bool can_saw_up()
Check if upward saw effect is supported.
Definition: force_feedback.h:114
JoystickForceFeedback::can_sine
bool can_sine()
Check if sine effect is supported.
Definition: force_feedback.h:109
fawkes::Time::wait_systime
void wait_systime()
Wait (sleep) for this system time.
Definition: time.cpp:760
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
JoystickAcquisitionThread::num_axes
char num_axes() const
Get number of axes.
Definition: acquisition_thread.cpp:437
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
JoystickAcquisitionThread::lock_if_new_data
bool lock_if_new_data()
Lock data if fresh.
Definition: acquisition_thread.cpp:413
fawkes::Time
A class for handling time.
Definition: time.h:93
JoystickForceFeedback::can_square
bool can_square()
Check if square effect is supported.
Definition: force_feedback.h:99
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
JoystickAcquisitionThread::axis_values
float * axis_values()
Get values for the axes.
Definition: acquisition_thread.cpp:479
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
JoystickForceFeedback::can_constant
bool can_constant()
Check if constant effect is supported.
Definition: force_feedback.h:69
JoystickForceFeedback::can_custom
bool can_custom()
Check if custom effect is supported.
Definition: force_feedback.h:124
JoystickAcquisitionThread::JoystickAcquisitionThread
JoystickAcquisitionThread()
Constructor.
Definition: acquisition_thread.cpp:56
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
JoystickForceFeedback::can_inertia
bool can_inertia()
Check if inertia effect is supported.
Definition: force_feedback.h:89
JoystickBlackBoardHandler::joystick_unplugged
virtual void joystick_unplugged()=0
The joystick has been unplugged and is no longer available.
JoystickAcquisitionThread::pressed_buttons
unsigned int pressed_buttons() const
Pressed buttons.
Definition: acquisition_thread.cpp:464
JoystickBlackBoardHandler
Handler class for joystick data.
Definition: bb_handler.h:27
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36