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/JoystickInterface.h>
25 #include <interfaces/Laser360Interface.h>
26 #include <interfaces/MotorInterface.h>
27 #include <utils/math/angle.h>
28 #include <utils/math/coord.h>
29 
30 #include <cmath>
31 
32 #define CFG_PREFIX "/hardware/joystick/teleop/"
33 #define CFG_AXIS_FORWARD CFG_PREFIX "axis_forward"
34 #define CFG_AXIS_SIDEWARD CFG_PREFIX "axis_sideward"
35 #define CFG_AXIS_ROTATION CFG_PREFIX "axis_rotation"
36 
37 using namespace fawkes;
38 
39 /** @class JoystickTeleOpThread "joystick_teleop_thread.h"
40  * Remotely control a robot using a joystick.
41  * @author Tim Niemueller
42  */
43 
44 /** Constructor. */
46 : Thread("JoystickTeleOpThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SKILL)
48 {
49 }
50 
51 void
53 {
54  cfg_axis_forward_ = config->get_uint(CFG_PREFIX "axis_forward");
55  cfg_axis_sideward_ = config->get_uint(CFG_PREFIX "axis_sideward");
56  cfg_axis_rotation_ = config->get_uint(CFG_PREFIX "axis_rotation");
57  cfg_axis_threshold_ = config->get_float(CFG_PREFIX "axis_threshold");
58 
59  cfg_deadman_use_axis_ = false;
60  try {
61  cfg_deadman_axis_ = config->get_uint(CFG_PREFIX "deadman_axis");
62  cfg_deadman_ax_thresh_ = config->get_float(CFG_PREFIX "deadman_axis_threshold");
63  cfg_deadman_use_axis_ = true;
64  } catch (Exception &e) {
65  logger->log_debug(name(), "No deadman axis configured, ignoring");
66  }
67  cfg_deadman_butmask_ = config->get_uint(CFG_PREFIX "deadman_button_mask");
68 
69  cfg_drive_mode_use_axis_ = false;
70  try {
71  cfg_drive_mode_axis_ = config->get_uint(CFG_PREFIX "drive_mode_axis");
72  cfg_drive_mode_ax_thresh_ = config->get_float(CFG_PREFIX "drive_mode_axis_threshold");
73  cfg_drive_mode_use_axis_ = true;
74  } catch (Exception &e) {
75  logger->log_debug(name(), "No drive_mode axis configured, ignoring");
76  }
77  cfg_drive_mode_butmask_ = config->get_uint(CFG_PREFIX "drive_mode_button_mask");
78 
79  cfg_normal_max_vx_ = config->get_float(CFG_PREFIX "drive_modes/normal/max_vx");
80  cfg_normal_max_vy_ = config->get_float(CFG_PREFIX "drive_modes/normal/max_vy");
81  cfg_normal_max_omega_ = config->get_float(CFG_PREFIX "drive_modes/normal/max_omega");
82 
83  cfg_special_max_vx_ = config->get_float(CFG_PREFIX "drive_modes/special/max_vx");
84  cfg_special_max_vy_ = config->get_float(CFG_PREFIX "drive_modes/special/max_vy");
85  cfg_special_max_omega_ = config->get_float(CFG_PREFIX "drive_modes/special/max_omega");
86 
87  cfg_collision_safety_ = config->get_bool(CFG_PREFIX "collision_safety/enabled");
88  cfg_collision_safety_distance_ = config->get_float(CFG_PREFIX "collision_safety/distance");
89  cfg_collision_safety_angle_ = config->get_uint(CFG_PREFIX "collision_safety/angle");
90 
91  cfg_runstop_enable_buttons_ = config->get_uint(CFG_PREFIX "runstop-enable-buttons");
92  cfg_runstop_disable_buttons_ = config->get_uint(CFG_PREFIX "runstop-disable-buttons");
93 
94  cfg_ifid_motor_ = config->get_string(CFG_PREFIX "motor_interface_id");
95  motor_if_ = blackboard->open_for_reading<MotorInterface>(cfg_ifid_motor_.c_str());
96 
97  cfg_ifid_joystick_ = config->get_string(CFG_PREFIX "joystick_interface_id");
98  joystick_if_ = blackboard->open_for_reading<JoystickInterface>(cfg_ifid_joystick_.c_str());
99 
100  cfg_use_laser_ = false;
101  laser_if_ = NULL;
102  if (cfg_collision_safety_) {
103  try {
104  cfg_ifid_laser_ = config->get_string(CFG_PREFIX "laser_interface_id");
105  laser_if_ = blackboard->open_for_reading<Laser360Interface>(cfg_ifid_laser_.c_str());
106  cfg_use_laser_ = true;
107  } catch (Exception &e) {
108  logger->log_warn(name(), "No laser_interface_id configured, ignoring");
109  }
110 
111  cfg_use_ff_ = false;
112  ff_weak_ = false;
113  ff_strong_ = false;
114  try {
115  cfg_use_ff_ = config->get_bool(CFG_PREFIX "collision_safety/use-force-feedback");
116  } catch (Exception &e) {
117  } // ignore, use default
118  logger->log_debug(name(),
119  "Collision safety force feedback %sabled",
120  cfg_use_ff_ ? "En" : "Dis");
121  if (cfg_use_ff_) {
123 
124  msg->set_strong_magnitude(0xFFFF);
125  msg->set_weak_magnitude(0x8000);
126  msg->set_length(1000);
127 
128  joystick_if_->msgq_enqueue(msg);
129  }
130  } else {
131  logger->log_warn(name(), "Collision safety for joystick is disabled.");
132  }
133 
134  runstop_pressed_ = false;
135  stopped_ = false;
136 }
137 
138 bool
140 {
141  stop();
142  return true;
143 }
144 
145 void
147 {
148  blackboard->close(motor_if_);
149  blackboard->close(joystick_if_);
150  blackboard->close(laser_if_);
151 }
152 
153 void
154 JoystickTeleOpThread::send_transrot(float vx, float vy, float omega)
155 {
156  if (!motor_if_->has_writer())
157  return;
158 
159  try {
161  motor_if_->msgq_enqueue(msg);
162  stopped_ = false;
163  } catch (Exception &e) {
164  logger->log_warn(name(), "Failed to send transrot: %s", e.what_no_backtrace());
165  }
166 }
167 
168 void
169 JoystickTeleOpThread::stop()
170 {
171  if (!motor_if_->has_writer())
172  return;
173 
174  send_transrot(0., 0., 0.);
175  stopped_ = true;
176 }
177 
178 bool
179 JoystickTeleOpThread::is_area_free(float theta)
180 {
181  if (!laser_if_)
182  return true;
183 
184  min_distance_ = FLT_MAX;
185  for (int i = (-1) * (int)cfg_collision_safety_angle_; i <= (int)cfg_collision_safety_angle_;
186  ++i) {
187  int angle = ((int)theta) + i;
188  if (angle < 0) {
189  angle = angle + 359;
190  }
191  if (laser_if_->distances(angle) > 0. && laser_if_->distances(angle) < min_distance_) {
192  min_distance_ = laser_if_->distances(angle);
193  }
194  if (laser_if_->distances(angle) > 0.
195  && laser_if_->distances(angle) < cfg_collision_safety_distance_) {
196  return false;
197  }
198  }
199 
200  return true;
201 }
202 
203 void
205 {
206  joystick_if_->read();
207  if (laser_if_)
208  laser_if_->read();
209  motor_if_->read();
210 
211  if ((!joystick_if_->has_writer() || joystick_if_->num_axes() == 0) && !stopped_) {
212  logger->log_warn(name(), "Joystick disconnected, stopping");
213  stop();
214  } else if ((cfg_axis_forward_ > joystick_if_->num_axes()
215  || cfg_axis_sideward_ > joystick_if_->num_axes()
216  || cfg_axis_rotation_ > joystick_if_->num_axes()
217  || (cfg_deadman_use_axis_ && cfg_deadman_axis_ > joystick_if_->num_axes()))
218  && !stopped_) {
219  logger->log_warn(name(), "Axis number out of range, stopping");
220  stop();
221  } else if (joystick_if_->pressed_buttons() == cfg_runstop_enable_buttons_ && !runstop_pressed_
222  && motor_if_->motor_state() != MotorInterface::MOTOR_DISABLED) {
223  try {
224  stop();
226  new MotorInterface::SetMotorStateMessage(MotorInterface::MOTOR_DISABLED);
227  motor_if_->msgq_enqueue(msg);
228  runstop_pressed_ = true;
229  logger->log_warn(name(), "Runstop ENABLED");
230  } catch (Exception &e) {
231  logger->log_error(name(), "FAILED to enable runstop: %s", e.what_no_backtrace());
232  }
233  } else if (joystick_if_->pressed_buttons() == cfg_runstop_disable_buttons_ && !runstop_pressed_
234  && motor_if_->motor_state() == MotorInterface::MOTOR_DISABLED) {
235  try {
236  stop();
238  new MotorInterface::SetMotorStateMessage(MotorInterface::MOTOR_ENABLED);
239  motor_if_->msgq_enqueue(msg);
240  logger->log_warn(name(), "Runstop DISABLED");
241  runstop_pressed_ = true;
242  } catch (Exception &e) {
243  logger->log_error(name(), "FAILED to enable runstop: %s", e.what_no_backtrace());
244  }
245  } else if ((joystick_if_->pressed_buttons() & cfg_deadman_butmask_)
246  || (cfg_deadman_use_axis_
247  && ((cfg_deadman_ax_thresh_ >= 0
248  && joystick_if_->axis(cfg_deadman_axis_) > cfg_deadman_ax_thresh_)
249  || (cfg_deadman_ax_thresh_ < 0
250  && joystick_if_->axis(cfg_deadman_axis_) < cfg_deadman_ax_thresh_)))) {
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  if (fabsf(joystick_if_->axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
266  vx = joystick_if_->axis(cfg_axis_forward_) * cfg_special_max_vx_;
267  }
268  if (fabsf(joystick_if_->axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
269  vy = joystick_if_->axis(cfg_axis_sideward_) * cfg_special_max_vy_;
270  }
271  if (fabsf(joystick_if_->axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
272  omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_special_max_omega_;
273  }
274  } else {
275  if (fabsf(joystick_if_->axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
276  vx = joystick_if_->axis(cfg_axis_forward_) * cfg_normal_max_vx_;
277  }
278  if (fabsf(joystick_if_->axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
279  vy = joystick_if_->axis(cfg_axis_sideward_) * cfg_normal_max_vy_;
280  }
281  if (fabsf(joystick_if_->axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
282  omega = joystick_if_->axis(cfg_axis_rotation_) * cfg_normal_max_omega_;
283  }
284  }
285 
286  float theta, distance;
287  cart2polar2d(vx, vy, &theta, &distance);
288  bool area_free = is_area_free(rad2deg(theta));
289  if (!cfg_use_laser_ || area_free) // if we have no laser or area is free, move
290  {
291  if (laser_if_ && laser_if_->has_writer()
292  && min_distance_ < 2 * cfg_collision_safety_distance_) {
293  logger->log_warn(name(), "slow down");
294  vx = vx * min_distance_ / 2 / cfg_collision_safety_distance_;
295  vy = vy * min_distance_ / 2 / cfg_collision_safety_distance_;
296 
297  if (cfg_use_ff_ && !ff_weak_ && joystick_if_->supported_ff_effects() != 0) {
300 
301  msg->set_strong_magnitude(0xFFFF);
302  msg->set_weak_magnitude(0x8000);
303 
304  joystick_if_->msgq_enqueue(msg);
305  ff_weak_ = true;
306  ff_strong_ = false;
307  }
308  } else if (ff_weak_ || ff_strong_) {
310 
311  joystick_if_->msgq_enqueue(msg);
312  ff_weak_ = false;
313  ff_strong_ = false;
314  }
315  send_transrot(vx, vy, omega);
316  runstop_pressed_ = false;
317  } else if (cfg_use_laser_ && !area_free) {
318  logger->log_warn(name(), "obstacle reached");
319  send_transrot(0., 0., omega);
320 
321  if (cfg_use_ff_ && !ff_weak_ && joystick_if_->supported_ff_effects() != 0) {
323 
324  msg->set_strong_magnitude(0x8000);
325  msg->set_weak_magnitude(0xFFFF);
326 
327  logger->log_debug(name(), "Enabling strong rumble");
328  joystick_if_->msgq_enqueue(msg);
329  ff_strong_ = true;
330  ff_weak_ = false;
331  }
332  }
333  }
334  } else if (!stopped_) {
335  runstop_pressed_ = false;
336  stop();
337  } else if (joystick_if_->pressed_buttons() != cfg_runstop_enable_buttons_
338  && joystick_if_->pressed_buttons() != cfg_runstop_disable_buttons_) {
339  runstop_pressed_ = false;
340  }
341 }
fawkes::cart2polar2d
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:42
fawkes::MotorInterface::SetMotorStateMessage
Definition: MotorInterface.h:123
fawkes::MotorInterface::motor_state
uint32_t motor_state() const
Get motor_state value.
Definition: MotorInterface.cpp:118
fawkes::JoystickInterface::StartRumbleMessage::set_weak_magnitude
void set_weak_magnitude(const uint16_t new_weak_magnitude)
Set weak_magnitude value.
Definition: JoystickInterface.cpp:682
fawkes::JoystickInterface::StartRumbleMessage
Definition: JoystickInterface.h:130
JoystickTeleOpThread::init
virtual void init()
Initialize the thread.
Definition: joystick_teleop_thread.cpp:51
fawkes::MotorInterface::TransRotMessage
Definition: MotorInterface.h:363
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:475
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
JoystickTeleOpThread::loop
virtual void loop()
Code to execute in the thread.
Definition: joystick_teleop_thread.cpp:203
fawkes::JoystickInterface::supported_ff_effects
uint8_t supported_ff_effects() const
Get supported_ff_effects value.
Definition: JoystickInterface.cpp:254
fawkes::JoystickInterface::pressed_buttons
uint32_t pressed_buttons() const
Get pressed_buttons value.
Definition: JoystickInterface.cpp:291
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
fawkes::Laser360Interface::distances
float * distances() const
Get distances value.
Definition: Laser360Interface.cpp:117
fawkes::MotorInterface
Definition: MotorInterface.h:37
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
fawkes::JoystickInterface::axis
float * axis() const
Get axis value.
Definition: JoystickInterface.cpp:326
fawkes::rad2deg
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:50
fawkes::JoystickInterface::StartRumbleMessage::set_length
void set_length(const uint16_t new_length)
Set length value.
Definition: JoystickInterface.cpp:562
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:50
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes
fawkes::distance
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:63
fawkes::JoystickInterface::StopRumbleMessage
Definition: JoystickInterface.h:174
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:817
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:50
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:662
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::Thread
Definition: thread.h:44
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:47
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
JoystickTeleOpThread::prepare_finalize_user
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
Definition: joystick_teleop_thread.cpp:138
fawkes::JoystickInterface::num_axes
uint8_t num_axes() const
Get num_axes value.
Definition: JoystickInterface.cpp:184
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:882
fawkes::Laser360Interface
Definition: Laser360Interface.h:37
fawkes::JoystickInterface::StartRumbleMessage::set_strong_magnitude
void set_strong_magnitude(const uint16_t new_strong_magnitude)
Set strong_magnitude value.
Definition: JoystickInterface.cpp:652
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
JoystickTeleOpThread::JoystickTeleOpThread
JoystickTeleOpThread()
Constructor.
Definition: joystick_teleop_thread.cpp:44
fawkes::JoystickInterface
Definition: JoystickInterface.h:37
JoystickTeleOpThread::finalize
virtual void finalize()
Finalize the thread.
Definition: joystick_teleop_thread.cpp:145
fawkes::Exception
Definition: exception.h:39