22 #include "joystick_teleop_thread.h" 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> 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" 47 :
Thread(
"JoystickTeleOpThread",
Thread::OPMODE_WAITFORWAKEUP),
61 cfg_deadman_use_axis_ =
false;
64 cfg_deadman_ax_thresh_ =
config->
get_float(CFG_PREFIX
"deadman_axis_threshold");
65 cfg_deadman_use_axis_ =
true;
69 cfg_deadman_butmask_ =
config->
get_uint(CFG_PREFIX
"deadman_button_mask");
71 cfg_drive_mode_use_axis_ =
false;
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;
79 cfg_drive_mode_butmask_ =
config->
get_uint(CFG_PREFIX
"drive_mode_button_mask");
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");
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");
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");
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");
103 cfg_use_laser_ =
false;
105 if (cfg_collision_safety_) {
110 cfg_use_laser_ =
true;
119 cfg_use_ff_ =
config->
get_bool(CFG_PREFIX
"collision_safety/use-force-feedback");
121 logger->
log_debug(
name(),
"Collision safety force feedback %sabled", cfg_use_ff_ ?
"En" :
"Dis");
137 runstop_pressed_ =
false;
158 JoystickTeleOpThread::send_transrot(
float vx,
float vy,
float omega)
174 JoystickTeleOpThread::stop()
178 send_transrot(0., 0., 0.);
183 JoystickTeleOpThread::is_area_free(
float theta)
185 if (! laser_if_)
return true;
187 min_distance_ = FLT_MAX;
188 for (
int i = (-1)*cfg_collision_safety_angle_; i <= (int)cfg_collision_safety_angle_; ++i)
190 int angle = ((int)theta) + i;
197 min_distance_ = laser_if_->
distances(angle);
199 if (laser_if_->
distances(angle) > 0. && laser_if_->
distances(angle) < cfg_collision_safety_distance_)
211 joystick_if_->
read();
212 if (laser_if_) laser_if_->
read();
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()))
226 }
else if (joystick_if_->
pressed_buttons() == cfg_runstop_enable_buttons_ &&
227 ! runstop_pressed_ &&
228 motor_if_->
motor_state() != MotorInterface::MOTOR_DISABLED)
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)
245 runstop_pressed_ =
true;
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_))))
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_))))
266 if (fabsf(joystick_if_->
axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
267 vx = joystick_if_->
axis(cfg_axis_forward_) * cfg_special_max_vx_;
269 if (fabsf(joystick_if_->
axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
270 vy = joystick_if_->
axis(cfg_axis_sideward_) * cfg_special_max_vy_;
272 if (fabsf(joystick_if_->
axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
273 omega = joystick_if_->
axis(cfg_axis_rotation_) * cfg_special_max_omega_;
276 if (fabsf(joystick_if_->
axis(cfg_axis_forward_)) > cfg_axis_threshold_) {
277 vx = joystick_if_->
axis(cfg_axis_forward_) * cfg_normal_max_vx_;
279 if (fabsf(joystick_if_->
axis(cfg_axis_sideward_)) > cfg_axis_threshold_) {
280 vy = joystick_if_->
axis(cfg_axis_sideward_) * cfg_normal_max_vy_;
282 if (fabsf(joystick_if_->
axis(cfg_axis_rotation_)) > cfg_axis_threshold_) {
283 omega = joystick_if_->
axis(cfg_axis_rotation_) * cfg_normal_max_omega_;
289 bool area_free = is_area_free(
rad2deg(theta));
290 if (!cfg_use_laser_ || area_free)
292 if (laser_if_ && laser_if_->
has_writer() && min_distance_ < 2*cfg_collision_safety_distance_)
295 vx = vx * min_distance_ / 2 / cfg_collision_safety_distance_;
296 vy = vy * min_distance_ / 2 / cfg_collision_safety_distance_;
309 }
else if (ff_weak_ || ff_strong_) {
317 send_transrot(vx, vy, omega);
318 runstop_pressed_ =
false;
320 else if (cfg_use_laser_ && ! area_free)
323 send_transrot(0.,0.,omega);
339 }
else if (! stopped_) {
340 runstop_pressed_ =
false;
342 }
else if (joystick_if_->
pressed_buttons() != cfg_runstop_enable_buttons_ &&
345 runstop_pressed_ =
false;
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.
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.
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.
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.
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.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
const char * name() const
Get name of thread.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
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.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
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.
JoystickTeleOpThread()
Constructor.
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.
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.
virtual void close(Interface *interface)=0
Close interface.