Fawkes API  Fawkes Development Version
joystick_teleop_thread.cpp
1 
2 /***************************************************************************
3  * joystick_teleop_thread.cpp - Joystick teleop thread
4  *
5  * Created: Sun Nov 13 16:07:40 2011 (as part of the robotino plugin)
6  * Copyright 2011-2014 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "joystick_teleop_thread.h"
23 
24 #include <interfaces/MotorInterface.h>
25 #include <interfaces/JoystickInterface.h>
26 #include <interfaces/Laser360Interface.h>
27 #include <utils/math/angle.h>
28 #include <utils/math/coord.h>
29 
30 
31 #include <cmath>
32 
33 #define CFG_PREFIX "/hardware/joystick/teleop/"
34 #define CFG_AXIS_FORWARD CFG_PREFIX"axis_forward"
35 #define CFG_AXIS_SIDEWARD CFG_PREFIX"axis_sideward"
36 #define CFG_AXIS_ROTATION CFG_PREFIX"axis_rotation"
37 
38 using namespace fawkes;
39 
40 /** @class JoystickTeleOpThread "joystick_teleop_thread.h"
41  * Remotely control a robot using a joystick.
42  * @author Tim Niemueller
43  */
44 
45 /** Constructor. */
47  : Thread("JoystickTeleOpThread", Thread::OPMODE_WAITFORWAKEUP),
48  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SKILL)
49 {
50 }
51 
52 
53 void
55 {
56  cfg_axis_forward_ = config->get_uint(CFG_PREFIX"axis_forward");
57  cfg_axis_sideward_ = config->get_uint(CFG_PREFIX"axis_sideward");
58  cfg_axis_rotation_ = config->get_uint(CFG_PREFIX"axis_rotation");
59  cfg_axis_threshold_ = config->get_float(CFG_PREFIX"axis_threshold");
60 
61  cfg_deadman_use_axis_ = false;
62  try {
63  cfg_deadman_axis_ = config->get_uint(CFG_PREFIX"deadman_axis");
64  cfg_deadman_ax_thresh_ = config->get_float(CFG_PREFIX"deadman_axis_threshold");
65  cfg_deadman_use_axis_ = true;
66  } catch (Exception &e) {
67  logger->log_debug(name(), "No deadman axis configured, ignoring");
68  }
69  cfg_deadman_butmask_ = config->get_uint(CFG_PREFIX"deadman_button_mask");
70 
71  cfg_drive_mode_use_axis_ = false;
72  try {
73  cfg_drive_mode_axis_ = config->get_uint(CFG_PREFIX"drive_mode_axis");
74  cfg_drive_mode_ax_thresh_ = config->get_float(CFG_PREFIX"drive_mode_axis_threshold");
75  cfg_drive_mode_use_axis_ = true;
76  } catch (Exception &e) {
77  logger->log_debug(name(), "No drive_mode axis configured, ignoring");
78  }
79  cfg_drive_mode_butmask_ = config->get_uint(CFG_PREFIX"drive_mode_button_mask");
80 
81  cfg_normal_max_vx_ = config->get_float(CFG_PREFIX"drive_modes/normal/max_vx");
82  cfg_normal_max_vy_ = config->get_float(CFG_PREFIX"drive_modes/normal/max_vy");
83  cfg_normal_max_omega_ = config->get_float(CFG_PREFIX"drive_modes/normal/max_omega");
84 
85  cfg_special_max_vx_ = config->get_float(CFG_PREFIX"drive_modes/special/max_vx");
86  cfg_special_max_vy_ = config->get_float(CFG_PREFIX"drive_modes/special/max_vy");
87  cfg_special_max_omega_ = config->get_float(CFG_PREFIX"drive_modes/special/max_omega");
88 
89  cfg_collision_safety_ = config->get_bool(CFG_PREFIX"collision_safety/enabled");
90  cfg_collision_safety_distance_ = config->get_float(CFG_PREFIX"collision_safety/distance");
91  cfg_collision_safety_angle_ = config->get_uint(CFG_PREFIX"collision_safety/angle");
92 
93  cfg_runstop_enable_buttons_ = config->get_uint(CFG_PREFIX"runstop-enable-buttons");
94  cfg_runstop_disable_buttons_ = config->get_uint(CFG_PREFIX"runstop-disable-buttons");
95 
96  cfg_ifid_motor_ = config->get_string(CFG_PREFIX"motor_interface_id");
97  motor_if_ = blackboard->open_for_reading<MotorInterface>(cfg_ifid_motor_.c_str());
98 
99  cfg_ifid_joystick_ = config->get_string(CFG_PREFIX"joystick_interface_id");
100  joystick_if_ =
101  blackboard->open_for_reading<JoystickInterface>(cfg_ifid_joystick_.c_str());
102 
103  cfg_use_laser_ = false;
104  laser_if_ = NULL;
105  if (cfg_collision_safety_) {
106  try {
107  cfg_ifid_laser_ = config->get_string(CFG_PREFIX"laser_interface_id");
108  laser_if_ =
109  blackboard->open_for_reading<Laser360Interface>(cfg_ifid_laser_.c_str());
110  cfg_use_laser_ = true;
111  } catch (Exception &e) {
112  logger->log_warn(name(), "No laser_interface_id configured, ignoring");
113  }
114 
115  cfg_use_ff_ = false;
116  ff_weak_ = false;
117  ff_strong_ = false;
118  try {
119  cfg_use_ff_ = config->get_bool(CFG_PREFIX"collision_safety/use-force-feedback");
120  } catch (Exception &e) {} // ignore, use default
121  logger->log_debug(name(), "Collision safety force feedback %sabled", cfg_use_ff_ ? "En" : "Dis");
122  if (cfg_use_ff_) {
125 
126  msg->set_strong_magnitude(0xFFFF);
127  msg->set_weak_magnitude(0x8000);
128  msg->set_length(1000);
129 
130  joystick_if_->msgq_enqueue(msg);
131 
132  }
133  } else {
134  logger->log_warn(name(), "Collision safety for joystick is disabled.");
135  }
136 
137  runstop_pressed_ = false;
138  stopped_ = false;
139 }
140 
141 
142 bool
144 {
145  stop();
146  return true;
147 }
148 
149 void
151 {
152  blackboard->close(motor_if_);
153  blackboard->close(joystick_if_);
154  blackboard->close(laser_if_);
155 }
156 
157 void
158 JoystickTeleOpThread::send_transrot(float vx, float vy, float omega)
159 {
160  if (! motor_if_->has_writer()) return;
161 
162  try {
164  new MotorInterface::TransRotMessage(vx, vy, omega);
165  motor_if_->msgq_enqueue(msg);
166  stopped_ = false;
167  } catch (Exception &e) {
168  logger->log_warn(name(), "Failed to send transrot: %s", e.what_no_backtrace());
169  }
170 }
171 
172 
173 void
174 JoystickTeleOpThread::stop()
175 {
176  if (! motor_if_->has_writer()) return;
177 
178  send_transrot(0., 0., 0.);
179  stopped_ = true;
180 }
181 
182 bool
183 JoystickTeleOpThread::is_area_free(float theta)
184 {
185  if (! laser_if_) return true;
186 
187  min_distance_ = FLT_MAX;
188  for (int i = (-1)*cfg_collision_safety_angle_; i <= (int)cfg_collision_safety_angle_; ++i)
189  {
190  int angle = ((int)theta) + i;
191  if (angle < 0)
192  {
193  angle = angle + 359;
194  }
195  if (laser_if_->distances(angle) > 0. && laser_if_->distances(angle) < min_distance_)
196  {
197  min_distance_ = laser_if_->distances(angle);
198  }
199  if (laser_if_->distances(angle) > 0. && laser_if_->distances(angle) < cfg_collision_safety_distance_)
200  {
201  return false;
202  }
203  }
204 
205  return true;
206 }
207 
208 void
210 {
211  joystick_if_->read();
212  if (laser_if_) laser_if_->read();
213  motor_if_->read();
214 
215  if ((! joystick_if_->has_writer() || joystick_if_->num_axes() == 0) && ! stopped_) {
216  logger->log_warn(name(), "Joystick disconnected, stopping");
217  stop();
218  } else if ((cfg_axis_forward_ > joystick_if_->num_axes() ||
219  cfg_axis_sideward_ > joystick_if_->num_axes() ||
220  cfg_axis_rotation_ > joystick_if_->num_axes() ||
221  (cfg_deadman_use_axis_ && cfg_deadman_axis_ > joystick_if_->num_axes()))
222  && ! stopped_)
223  {
224  logger->log_warn(name(), "Axis number out of range, stopping");
225  stop();
226  } else if (joystick_if_->pressed_buttons() == cfg_runstop_enable_buttons_ &&
227  ! runstop_pressed_ &&
228  motor_if_->motor_state() != MotorInterface::MOTOR_DISABLED)
229  {
230  stop();
232  new MotorInterface::SetMotorStateMessage(MotorInterface::MOTOR_DISABLED);
233  motor_if_->msgq_enqueue(msg);
234  logger->log_warn(name(), "Runstop ENABLED");
235  runstop_pressed_ = true;
236  } else if (joystick_if_->pressed_buttons() == cfg_runstop_disable_buttons_ &&
237  ! runstop_pressed_ &&
238  motor_if_->motor_state() == MotorInterface::MOTOR_DISABLED)
239  {
240  stop();
242  new MotorInterface::SetMotorStateMessage(MotorInterface::MOTOR_ENABLED);
243  motor_if_->msgq_enqueue(msg);
244  logger->log_warn(name(), "Runstop DISABLED");
245  runstop_pressed_ = true;
246  } else if ((joystick_if_->pressed_buttons() & cfg_deadman_butmask_) ||
247  (cfg_deadman_use_axis_ &&
248  ((cfg_deadman_ax_thresh_ >= 0 && joystick_if_->axis(cfg_deadman_axis_) > cfg_deadman_ax_thresh_) ||
249  (cfg_deadman_ax_thresh_ < 0 && joystick_if_->axis(cfg_deadman_axis_) < cfg_deadman_ax_thresh_))))
250  {
251  runstop_pressed_ = false;
252  if (fabsf(joystick_if_->axis(cfg_axis_forward_)) < cfg_axis_threshold_ &&
253  fabsf(joystick_if_->axis(cfg_axis_sideward_)) < cfg_axis_threshold_ &&
254  fabsf(joystick_if_->axis(cfg_axis_rotation_)) < cfg_axis_threshold_) {
255  stop();
256  } else {
257  float vx = 0, vy = 0, omega = 0;
258 
259  if ((joystick_if_->pressed_buttons() & cfg_drive_mode_butmask_) ||
260  (cfg_drive_mode_use_axis_ &&
261  ((cfg_drive_mode_ax_thresh_ >= 0 &&
262  joystick_if_->axis(cfg_drive_mode_axis_) > cfg_drive_mode_ax_thresh_) ||
263  (cfg_drive_mode_ax_thresh_ < 0 &&
264  joystick_if_->axis(cfg_drive_mode_axis_) < cfg_drive_mode_ax_thresh_))))
265  {
266  if (fabsf(joystick_if_->axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
267  vx = joystick_if_->axis(cfg_axis_forward_) * cfg_special_max_vx_;
268  }
269  if (fabsf(joystick_if_->axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
270  vy = joystick_if_->axis(cfg_axis_sideward_) * cfg_special_max_vy_;
271  }
272  if (fabsf(joystick_if_->axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
273  omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_special_max_omega_;
274  }
275  } else {
276  if (fabsf(joystick_if_->axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
277  vx = joystick_if_->axis(cfg_axis_forward_) * cfg_normal_max_vx_;
278  }
279  if (fabsf(joystick_if_->axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
280  vy = joystick_if_->axis(cfg_axis_sideward_) * cfg_normal_max_vy_;
281  }
282  if (fabsf(joystick_if_->axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
283  omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_normal_max_omega_;
284  }
285  }
286 
287  float theta, distance;
288  cart2polar2d(vx, vy, &theta, &distance);
289  bool area_free = is_area_free(rad2deg(theta));
290  if (!cfg_use_laser_ || area_free) // if we have no laser or area is free, move
291  {
292  if (laser_if_ && laser_if_->has_writer() && min_distance_ < 2*cfg_collision_safety_distance_)
293  {
294  logger->log_warn(name(),"slow down");
295  vx = vx * min_distance_ / 2 / cfg_collision_safety_distance_;
296  vy = vy * min_distance_ / 2 / cfg_collision_safety_distance_;
297 
298  if (cfg_use_ff_ && ! ff_weak_ && joystick_if_->supported_ff_effects() != 0) {
301 
302  msg->set_strong_magnitude(0xFFFF);
303  msg->set_weak_magnitude(0x8000);
304 
305  joystick_if_->msgq_enqueue(msg);
306  ff_weak_ = true;
307  ff_strong_ = false;
308  }
309  } else if (ff_weak_ || ff_strong_) {
312 
313  joystick_if_->msgq_enqueue(msg);
314  ff_weak_ = false;
315  ff_strong_ = false;
316  }
317  send_transrot(vx, vy, omega);
318  runstop_pressed_ = false;
319  }
320  else if (cfg_use_laser_ && ! area_free)
321  {
322  logger->log_warn(name(),"obstacle reached");
323  send_transrot(0.,0.,omega);
324 
325  if (cfg_use_ff_ && ! ff_weak_ && joystick_if_->supported_ff_effects() != 0) {
328 
329  msg->set_strong_magnitude(0x8000);
330  msg->set_weak_magnitude(0xFFFF);
331 
332  logger->log_debug(name(), "Enabling strong rumble");
333  joystick_if_->msgq_enqueue(msg);
334  ff_strong_ = true;
335  ff_weak_ = false;
336  }
337  }
338  }
339  } else if (! stopped_) {
340  runstop_pressed_ = false;
341  stop();
342  } else if (joystick_if_->pressed_buttons() != cfg_runstop_enable_buttons_ &&
343  joystick_if_->pressed_buttons() != cfg_runstop_enable_buttons_)
344  {
345  runstop_pressed_ = false;
346  }
347 }
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
Laser360Interface Fawkes BlackBoard Interface.
SetMotorStateMessage Fawkes BlackBoard Interface Message.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:62
TransRotMessage Fawkes BlackBoard Interface Message.
JoystickInterface Fawkes BlackBoard Interface.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void init()
Initialize the thread.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void loop()
Code to execute in the thread.
void set_weak_magnitude(const uint16_t new_weak_magnitude)
Set weak_magnitude value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
float * axis() const
Get axis value.
StopRumbleMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
void cart2polar2d(float cart_x, float cart_y, float *polar_phi, float *polar_dist)
Convert a 2D cartesian coordinate to a 2D polar coordinate.
Definition: coord.h:39
uint8_t supported_ff_effects() const
Get supported_ff_effects value.
uint32_t pressed_buttons() const
Get pressed_buttons value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
StartRumbleMessage Fawkes BlackBoard Interface Message.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
uint8_t num_axes() const
Get num_axes value.
MotorInterface Fawkes BlackBoard Interface.
void set_length(const uint16_t new_length)
Set length value.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
float * distances() const
Get distances value.
void set_strong_magnitude(const uint16_t new_strong_magnitude)
Set strong_magnitude value.
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.
uint32_t motor_state() const
Get motor_state value.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void finalize()
Finalize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.