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