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;
154JoystickTeleOpThread::send_transrot(
float vx,
float vy,
float omega)
169JoystickTeleOpThread::stop()
174 send_transrot(0., 0., 0.);
179JoystickTeleOpThread::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;
virtual void init()
Initialize the thread.
JoystickTeleOpThread()
Constructor.
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.
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.
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.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
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.
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.
const char * name() const
Get name of thread.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
void cart2polar2d(float cart_x, float cart_y, float *polar_phi, float *polar_dist)
Convert a 2D cartesian coordinate to a 2D polar coordinate.
float rad2deg(float rad)
Convert an angle given in radians to degrees.