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
37using 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),
48{
49}
50
51void
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
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
138bool
140{
141 stop();
142 return true;
143}
144
145void
147{
148 blackboard->close(motor_if_);
149 blackboard->close(joystick_if_);
150 blackboard->close(laser_if_);
151}
152
153void
154JoystickTeleOpThread::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
168void
169JoystickTeleOpThread::stop()
170{
171 if (!motor_if_->has_writer())
172 return;
173
174 send_transrot(0., 0., 0.);
175 stopped_ = true;
176}
177
178bool
179JoystickTeleOpThread::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
203void
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}
virtual void init()
Initialize the thread.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
virtual void loop()
Code to execute in the thread.
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 Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
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.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
StartRumbleMessage Fawkes BlackBoard Interface Message.
void set_strong_magnitude(const uint16_t new_strong_magnitude)
Set strong_magnitude value.
void set_length(const uint16_t new_length)
Set length value.
void set_weak_magnitude(const uint16_t new_weak_magnitude)
Set weak_magnitude value.
StopRumbleMessage Fawkes BlackBoard Interface Message.
JoystickInterface Fawkes BlackBoard Interface.
uint32_t pressed_buttons() const
Get pressed_buttons value.
uint8_t supported_ff_effects() const
Get supported_ff_effects value.
uint8_t num_axes() const
Get num_axes value.
float * axis() const
Get axis value.
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
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_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
SetMotorStateMessage Fawkes BlackBoard Interface Message.
TransRotMessage Fawkes BlackBoard Interface Message.
MotorInterface Fawkes BlackBoard Interface.
uint32_t motor_state() const
Get motor_state value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59
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:38
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46