100 lines
2.8 KiB
Python
100 lines
2.8 KiB
Python
#!/usr/bin/env python
|
|
import errno
|
|
import os
|
|
import select
|
|
import struct
|
|
import time
|
|
|
|
import rospy
|
|
from sensor_msgs.msg import Joy
|
|
|
|
|
|
DEVICE_PATH = '/dev/input/js0'
|
|
AXIS_COUNT = 8
|
|
BUTTON_COUNT = 12
|
|
PUBLISH_RATE_HZ = 20.0
|
|
|
|
JS_EVENT_BUTTON = 0x01
|
|
JS_EVENT_AXIS = 0x02
|
|
JS_EVENT_INIT = 0x80
|
|
EVENT_SIZE = struct.calcsize('IhBB')
|
|
|
|
|
|
def open_device():
|
|
while not rospy.is_shutdown():
|
|
try:
|
|
fd = os.open(DEVICE_PATH, os.O_RDONLY | os.O_NONBLOCK)
|
|
rospy.loginfo('Joystick verbunden: %s', DEVICE_PATH)
|
|
return fd
|
|
except OSError as exc:
|
|
if exc.errno != errno.ENOENT:
|
|
rospy.logwarn('Joystick kann nicht geoeffnet werden: %s', exc)
|
|
rospy.sleep(1.0)
|
|
return None
|
|
|
|
|
|
def close_device(fd):
|
|
if fd is None:
|
|
return
|
|
try:
|
|
os.close(fd)
|
|
except OSError:
|
|
pass
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('f710_joy_node')
|
|
|
|
publisher = rospy.Publisher('/joy', Joy, queue_size=5)
|
|
rate = rospy.Rate(PUBLISH_RATE_HZ)
|
|
|
|
axes = [0.0] * AXIS_COUNT
|
|
buttons = [0] * BUTTON_COUNT
|
|
joystick_fd = None
|
|
|
|
while not rospy.is_shutdown():
|
|
if joystick_fd is None:
|
|
joystick_fd = open_device()
|
|
axes = [0.0] * AXIS_COUNT
|
|
buttons = [0] * BUTTON_COUNT
|
|
if joystick_fd is None:
|
|
break
|
|
|
|
try:
|
|
readable, _, _ = select.select([joystick_fd], [], [], 0.0)
|
|
if readable:
|
|
event = os.read(joystick_fd, EVENT_SIZE)
|
|
while event and len(event) == EVENT_SIZE:
|
|
_, value, event_type, number = struct.unpack('IhBB', event)
|
|
event_type &= ~JS_EVENT_INIT
|
|
|
|
if event_type == JS_EVENT_AXIS and number < len(axes):
|
|
axes[number] = max(-1.0, min(1.0, value / 32767.0))
|
|
elif event_type == JS_EVENT_BUTTON and number < len(buttons):
|
|
buttons[number] = 1 if value else 0
|
|
|
|
try:
|
|
event = os.read(joystick_fd, EVENT_SIZE)
|
|
except OSError as exc:
|
|
if exc.errno in (errno.EAGAIN, errno.EWOULDBLOCK):
|
|
break
|
|
raise
|
|
|
|
message = Joy()
|
|
message.header.stamp = rospy.Time.now()
|
|
message.axes = list(axes)
|
|
message.buttons = list(buttons)
|
|
publisher.publish(message)
|
|
rate.sleep()
|
|
|
|
except OSError as exc:
|
|
if exc.errno in (errno.ENODEV, errno.EIO, errno.ENXIO, errno.EBADF):
|
|
rospy.logwarn('Joystick getrennt, warte auf Neuverbindung.')
|
|
else:
|
|
rospy.logwarn('Joystick-Lesefehler: %s', exc)
|
|
close_device(joystick_fd)
|
|
joystick_fd = None
|
|
time.sleep(1.0)
|
|
|
|
close_device(joystick_fd)
|