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;
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.
virtual void log_error(const char *component, const char *format,...)=0
Log error 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.