22 #include "joystick_teleop_thread.h"
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>
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"
46 :
Thread(
"JoystickTeleOpThread",
Thread::OPMODE_WAITFORWAKEUP),
59 cfg_deadman_use_axis_ =
false;
62 cfg_deadman_ax_thresh_ =
config->
get_float(CFG_PREFIX
"deadman_axis_threshold");
63 cfg_deadman_use_axis_ =
true;
67 cfg_deadman_butmask_ =
config->
get_uint(CFG_PREFIX
"deadman_button_mask");
69 cfg_drive_mode_use_axis_ =
false;
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;
77 cfg_drive_mode_butmask_ =
config->
get_uint(CFG_PREFIX
"drive_mode_button_mask");
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");
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");
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");
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");
100 cfg_use_laser_ =
false;
102 if (cfg_collision_safety_) {
106 cfg_use_laser_ =
true;
115 cfg_use_ff_ =
config->
get_bool(CFG_PREFIX
"collision_safety/use-force-feedback");
119 "Collision safety force feedback %sabled",
120 cfg_use_ff_ ?
"En" :
"Dis");
134 runstop_pressed_ =
false;
154 JoystickTeleOpThread::send_transrot(
float vx,
float vy,
float omega)
169 JoystickTeleOpThread::stop()
174 send_transrot(0., 0., 0.);
179 JoystickTeleOpThread::is_area_free(
float theta)
184 min_distance_ = FLT_MAX;
185 for (
int i = (-1) * (
int)cfg_collision_safety_angle_; i <= (int)cfg_collision_safety_angle_;
187 int angle = ((int)theta) + i;
191 if (laser_if_->
distances(angle) > 0. && laser_if_->
distances(angle) < min_distance_) {
192 min_distance_ = laser_if_->
distances(angle);
195 && laser_if_->
distances(angle) < cfg_collision_safety_distance_) {
206 joystick_if_->
read();
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()))
221 }
else if (joystick_if_->
pressed_buttons() == cfg_runstop_enable_buttons_ && !runstop_pressed_
222 && motor_if_->
motor_state() != MotorInterface::MOTOR_DISABLED) {
228 runstop_pressed_ =
true;
233 }
else if (joystick_if_->
pressed_buttons() == cfg_runstop_disable_buttons_ && !runstop_pressed_
234 && motor_if_->
motor_state() == MotorInterface::MOTOR_DISABLED) {
241 runstop_pressed_ =
true;
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_) {
257 float vx = 0, vy = 0, omega = 0;
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_;
268 if (fabsf(joystick_if_->
axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
269 vy = joystick_if_->
axis(cfg_axis_sideward_) * cfg_special_max_vy_;
271 if (fabsf(joystick_if_->
axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
272 omega = joystick_if_->
axis(cfg_axis_rotation_) * cfg_special_max_omega_;
275 if (fabsf(joystick_if_->
axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
276 vx = joystick_if_->
axis(cfg_axis_forward_) * cfg_normal_max_vx_;
278 if (fabsf(joystick_if_->
axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
279 vy = joystick_if_->
axis(cfg_axis_sideward_) * cfg_normal_max_vy_;
281 if (fabsf(joystick_if_->
axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
282 omega = joystick_if_->
axis(cfg_axis_rotation_) * cfg_normal_max_omega_;
288 bool area_free = is_area_free(
rad2deg(theta));
289 if (!cfg_use_laser_ || area_free)
292 && min_distance_ < 2 * cfg_collision_safety_distance_) {
294 vx = vx * min_distance_ / 2 / cfg_collision_safety_distance_;
295 vy = vy * min_distance_ / 2 / cfg_collision_safety_distance_;
308 }
else if (ff_weak_ || ff_strong_) {
315 send_transrot(vx, vy, omega);
316 runstop_pressed_ =
false;
317 }
else if (cfg_use_laser_ && !area_free) {
319 send_transrot(0., 0., omega);
334 }
else if (!stopped_) {
335 runstop_pressed_ =
false;
337 }
else if (joystick_if_->
pressed_buttons() != cfg_runstop_enable_buttons_
339 runstop_pressed_ =
false;