290 lines
8.4 KiB
Python
290 lines
8.4 KiB
Python
#!/usr/bin/env python
|
|
import ctypes
|
|
import fcntl
|
|
import glob
|
|
import math
|
|
import os
|
|
import struct
|
|
import threading
|
|
import time
|
|
|
|
import rospy
|
|
from sensor_msgs.msg import Joy
|
|
from exomy.msg import RoverCommand
|
|
from locomotion_modes import LocomotionMode
|
|
|
|
# Define locomotion modes
|
|
global locomotion_mode
|
|
global motors_enabled
|
|
global last_start_button_pressed
|
|
global last_locomotion_mode
|
|
|
|
locomotion_mode = LocomotionMode.ACKERMANN.value
|
|
last_locomotion_mode = locomotion_mode
|
|
motors_enabled = True
|
|
_speed_limit = 100
|
|
_speed_limit_last_read = 0.0
|
|
AXIS_DEADZONE = 0.1
|
|
last_start_button_pressed = False
|
|
last_webgui_time = None
|
|
WEBGUI_PRIORITY_TIMEOUT = 2.0
|
|
|
|
VELOCITY_EXPO = 2.0
|
|
CARDINAL_SNAP_THRESHOLD = 0.12
|
|
|
|
DEFAULT_CONTROLLER = "logitech-F710"
|
|
CONTROLLER_FUNCTION_MAPS = {
|
|
"webgui": {
|
|
"x_axis": 0,
|
|
"y_axis": 1,
|
|
"invert_x_axis": True,
|
|
"invert_y_axis": False,
|
|
"X_button": 0,
|
|
"Y_button": 3,
|
|
"A_button": 1,
|
|
"B_button": 2,
|
|
"start_button": 9,
|
|
"select_button": 8,
|
|
"sensitivity": 0.15,
|
|
},
|
|
"logitech-F710": {
|
|
"x_axis": 0,
|
|
"y_axis": 1,
|
|
"invert_x_axis": True,
|
|
"invert_y_axis": True,
|
|
"X_button": 0,
|
|
"Y_button": 3,
|
|
"A_button": 1,
|
|
"B_button": 2,
|
|
"start_button": 9,
|
|
"select_button": 8,
|
|
"sensitivity": 0.10,
|
|
},
|
|
"xbox-one": {
|
|
"x_axis": 0,
|
|
"y_axis": 1,
|
|
"invert_x_axis": False,
|
|
"invert_y_axis": False,
|
|
"X_button": 2,
|
|
"Y_button": 3,
|
|
"A_button": 0,
|
|
"B_button": 1,
|
|
"start_button": 7,
|
|
"select_button": 6,
|
|
"sensitivity": 0.11,
|
|
},
|
|
}
|
|
|
|
|
|
def get_speed_limit():
|
|
global _speed_limit, _speed_limit_last_read
|
|
now = time.time()
|
|
if now - _speed_limit_last_read > 2.0:
|
|
try:
|
|
_speed_limit = int(rospy.get_param('/speed_limit_percent', 100))
|
|
except Exception:
|
|
pass
|
|
_speed_limit_last_read = now
|
|
return _speed_limit
|
|
|
|
|
|
def _rumble_thread(duration_ms):
|
|
EV_FF = 0x15
|
|
FF_RUMBLE = 0x50
|
|
EVIOCSFF = (0x40000000 | (48 << 16) | (ord('E') << 8) | 0x80)
|
|
|
|
device_path = None
|
|
for path in sorted(glob.glob('/dev/input/event*')):
|
|
try:
|
|
sys_name = '/sys/class/input/{0}/device/name'.format(os.path.basename(path))
|
|
with open(sys_name) as f:
|
|
name = f.read().strip()
|
|
if any(k in name for k in ('Gamepad', 'F710', 'GAMEPAD')):
|
|
device_path = path
|
|
break
|
|
except OSError:
|
|
pass
|
|
|
|
if not device_path:
|
|
return
|
|
|
|
try:
|
|
fd = os.open(device_path, os.O_RDWR)
|
|
try:
|
|
buf = ctypes.create_string_buffer(48)
|
|
struct.pack_into('<H', buf, 0, FF_RUMBLE)
|
|
struct.pack_into('<h', buf, 2, -1)
|
|
struct.pack_into('<H', buf, 10, duration_ms)
|
|
struct.pack_into('<H', buf, 16, 0xFFFF)
|
|
struct.pack_into('<H', buf, 18, 0x8000)
|
|
fcntl.ioctl(fd, EVIOCSFF, buf)
|
|
effect_id = struct.unpack_from('<h', buf, 2)[0]
|
|
import time as _t
|
|
os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 1))
|
|
_t.sleep(duration_ms / 1000.0 + 0.05)
|
|
os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 0))
|
|
finally:
|
|
os.close(fd)
|
|
except Exception as exc:
|
|
rospy.logwarn('rumble fehler: %s', exc)
|
|
|
|
|
|
def trigger_rumble(duration_ms=150):
|
|
threading.Thread(target=_rumble_thread, args=(duration_ms,)).start()
|
|
|
|
|
|
def apply_deadzone(value, deadzone):
|
|
if abs(value) <= deadzone:
|
|
return 0.0
|
|
|
|
scaled = (abs(value) - deadzone) / (1.0 - deadzone)
|
|
return math.copysign(scaled, value)
|
|
|
|
|
|
def get_controller_function_map(data):
|
|
if data.header.frame_id == "webgui":
|
|
return CONTROLLER_FUNCTION_MAPS["webgui"]
|
|
|
|
controller_name = rospy.get_param("controller", DEFAULT_CONTROLLER)
|
|
if controller_name in CONTROLLER_FUNCTION_MAPS:
|
|
return CONTROLLER_FUNCTION_MAPS[controller_name]
|
|
|
|
rospy.logwarn("Unbekannter Controller '%s', nutze Fallback '%s'.",
|
|
controller_name, DEFAULT_CONTROLLER)
|
|
return CONTROLLER_FUNCTION_MAPS[DEFAULT_CONTROLLER]
|
|
|
|
|
|
def get_axis_value(axes, axis_index):
|
|
if axis_index < len(axes):
|
|
return axes[axis_index]
|
|
return 0.0
|
|
|
|
|
|
def is_button_pressed(buttons, button_index):
|
|
return button_index < len(buttons) and buttons[button_index] == 1
|
|
|
|
|
|
def snap_axes_to_cardinal(x, y, threshold):
|
|
if abs(x) <= threshold and abs(y) > 0.0:
|
|
return 0.0, math.copysign(abs(y), y)
|
|
if abs(y) <= threshold and abs(x) > 0.0:
|
|
return math.copysign(abs(x), x), 0.0
|
|
return x, y
|
|
|
|
|
|
def callback(data):
|
|
|
|
global locomotion_mode
|
|
global motors_enabled
|
|
global last_start_button_pressed
|
|
global last_webgui_time
|
|
global last_locomotion_mode
|
|
|
|
is_webgui = data.header.frame_id == "webgui"
|
|
now = rospy.Time.now()
|
|
|
|
if is_webgui:
|
|
last_webgui_time = now
|
|
elif last_webgui_time is not None and (now - last_webgui_time).to_sec() < WEBGUI_PRIORITY_TIMEOUT:
|
|
return
|
|
|
|
rover_cmd = RoverCommand()
|
|
|
|
# Function map for the Logitech F710 joystick
|
|
# Button on pad | function
|
|
# --------------|----------------------
|
|
# A | Ackermann mode
|
|
# X | Point turn mode
|
|
# Y | Crabbing mode
|
|
# Left Stick | Control speed and direction
|
|
# START Button | Enable and disable motors
|
|
|
|
controller_function_map = get_controller_function_map(data)
|
|
|
|
# Reading out joystick data
|
|
y = apply_deadzone(
|
|
get_axis_value(data.axes, controller_function_map["y_axis"]),
|
|
controller_function_map["sensitivity"])
|
|
x = apply_deadzone(
|
|
get_axis_value(data.axes, controller_function_map["x_axis"]),
|
|
controller_function_map["sensitivity"])
|
|
|
|
if controller_function_map["invert_x_axis"]:
|
|
x *= -1
|
|
if controller_function_map["invert_y_axis"]:
|
|
y *= -1
|
|
|
|
# Kleine Queranteile sollen beim Geradeausfahren nicht zu einem
|
|
# unbeabsichtigten Lenkwinkel fuehren.
|
|
snap_threshold = max(
|
|
controller_function_map["sensitivity"],
|
|
CARDINAL_SNAP_THRESHOLD)
|
|
x, y = snap_axes_to_cardinal(x, y, snap_threshold)
|
|
|
|
# Reading out button data to set locomotion mode
|
|
# X Button
|
|
if is_button_pressed(data.buttons, controller_function_map["X_button"]):
|
|
locomotion_mode = LocomotionMode.POINT_TURN.value
|
|
# A Button
|
|
if is_button_pressed(data.buttons, controller_function_map["A_button"]):
|
|
locomotion_mode = LocomotionMode.ACKERMANN.value
|
|
# B Button
|
|
if is_button_pressed(data.buttons, controller_function_map["B_button"]):
|
|
pass
|
|
# Y Button
|
|
if is_button_pressed(data.buttons, controller_function_map["Y_button"]):
|
|
locomotion_mode = LocomotionMode.CRABBING.value
|
|
|
|
if not is_webgui and locomotion_mode != last_locomotion_mode:
|
|
trigger_rumble(150)
|
|
last_locomotion_mode = locomotion_mode
|
|
|
|
rover_cmd.locomotion_mode = locomotion_mode
|
|
|
|
# Enable and disable motors
|
|
# START Button
|
|
start_button_pressed = is_button_pressed(
|
|
data.buttons, controller_function_map["start_button"])
|
|
if start_button_pressed and not last_start_button_pressed:
|
|
if motors_enabled is True:
|
|
motors_enabled = False
|
|
rospy.loginfo("Motors disabled!")
|
|
elif motors_enabled is False:
|
|
motors_enabled = True
|
|
rospy.loginfo("Motors enabled!")
|
|
else:
|
|
rospy.logerr(
|
|
"Exceptional value for [motors_enabled] = {}".format(motors_enabled))
|
|
motors_enabled = False
|
|
last_start_button_pressed = start_button_pressed
|
|
|
|
rover_cmd.motors_enabled = motors_enabled
|
|
|
|
# The velocity is decoded as value between 0...100, capped by speed limit
|
|
stick_length = min(math.sqrt(x*x + y*y), 1.0)
|
|
rover_cmd.vel = int(100.0 * math.pow(get_speed_limit() / 100.0 * stick_length, VELOCITY_EXPO))
|
|
|
|
# The steering is described as an angle between -180...180
|
|
# Which describe the joystick position as follows:
|
|
# +90
|
|
# 0 +-180
|
|
# -90
|
|
#
|
|
rover_cmd.steering = int(math.atan2(y, x)*180.0/math.pi)
|
|
|
|
rover_cmd.connected = True
|
|
|
|
pub.publish(rover_cmd)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
global pub
|
|
|
|
rospy.init_node('joystick_parser_node')
|
|
rospy.loginfo('joystick_parser_node started')
|
|
|
|
sub = rospy.Subscriber("/joy_delayed", Joy, callback, queue_size=1)
|
|
pub = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)
|
|
|
|
rospy.spin()
|