def test_offset_rotate():
_log = Logger("offset-rotate-test", Level.INFO)
_count = 0
for offset in arange(0.0, 360.0, 90.0):
_count += 1
_log.heading('Offset Rotate Test', 'offset={:<5.2f};'.format(offset),
'{:d}/4'.format(_count))
for angle in arange(0.0, 360.0, 30.0):
_alt = Convert.offset_in_degrees(angle, offset)
_log.info(Fore.GREEN +
'from {:>5.2f}°\tto {:>5.2f}°;'.format(angle, _alt) +
Fore.BLACK + '\toffset={:<5.2f};'.format(offset))
time.sleep(0.01)
_log.info('complete.')
def test_battery_check():
_log = Logger("batcheck test", Level.INFO)
_log.heading('battery check', 'Starting test...', '[1/3]')
_log.info(Fore.RED + 'Press Ctrl+C to exit.')
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_log.heading('battery check', 'Creating objects...', '[2/3]')
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message queue...')
_queue = MessageQueue(_message_factory, Level.INFO)
# _consumer = MockConsumer()
# _queue.add_consumer(_consumer)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.INFO)
_log.info('creating clock...')
_clock = Clock(_config, _message_bus, _message_factory, Level.INFO)
_log.info('creating battery check...')
_battery_check = BatteryCheck(_config, _clock, _queue, _message_factory,
Level.INFO)
_battery_check.enable()
# _battery_check.set_enable_messaging(True)
_log.heading('battery check', 'Enabling battery check...', '[3/3]')
_clock.enable()
while _battery_check.count < 40:
time.sleep(0.5)
_battery_check.close()
# _log.info('consumer received {:d} messages.'.format(_consumer.count))
_log.info('complete.')
init()
from lib.logger import Logger, Level
from lib.gamepad_demo import GamepadDemo
_log = Logger('gamepad-test', Level.INFO)
_motors = None
_gamepad_demo = None
_exit_code = 0
try:
_main_thread = threading.main_thread()
_main_thread.name = 'main'
_log.heading('test', 'starting gamepad demo...', None)
_gamepad_demo = GamepadDemo(Level.INFO)
_gamepad_demo.enable()
_motors = _gamepad_demo.get_motors()
while _gamepad_demo.enabled:
time.sleep(1.0)
_log.info('exited loop.')
time.sleep(1.0)
except KeyboardInterrupt:
_log.info('caught Ctrl-C; exiting...')
except OSError:
_exit_code = 1
_log.error('unable to connect to gamepad')
except Exception:
_exit_code = 1
class GamepadDemo():
def __init__(self, level):
super().__init__()
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
self._log = Logger("gamepad-demo", level)
self._log.heading('gamepad-demo', 'Configuring Gamepad...', None)
self._config = _config['ros'].get('gamepad_demo')
self._enable_ifs = self._config.get('enable_ifs')
self._enable_compass = self._config.get('enable_compass')
self._enable_indicator = self._config.get('enable_indicator')
self._message_factory = MessageFactory(level)
self._motors = Motors(_config, None, Level.INFO)
# self._motor_controller = SimpleMotorController(self._motors, Level.INFO)
self._pid_motor_ctrl = PIDMotorController(_config, self._motors,
Level.INFO)
# i2c scanner, let's us know if certain devices are available
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.get_int_addresses()
ltr559_available = (0x23 in _addresses)
'''
Availability of displays:
The 5x5 RGB Matrix is at 0x74 for port, 0x77 for starboard.
The 11x7 LED matrix is at 0x75 for starboard, 0x77 for port. The latter
conflicts with the RGB LED matrix, so both cannot be used simultaneously.
We check for either the 0x74 address to see if RGB Matrix displays are
used, OR for 0x75 to assume a pair of 11x7 Matrix displays are being used.
'''
# rgbmatrix5x5_stbd_available = ( 0x74 in _addresses ) # not used yet
# matrix11x7_stbd_available = ( 0x75 in _addresses ) # used as camera lighting
matrix11x7_stbd_available = False
# self._blob = BlobSensor(_config, self._motors, Level.INFO)
self._blob = None
self._lux = Lux(Level.INFO) if ltr559_available else None
self._video = None
# self._video = Video(_config, self._lux, matrix11x7_stbd_available, Level.INFO)
self._message_bus = MessageBus(Level.INFO)
# in this application the gamepad controller is the message queue
# self._queue = MessageQueue(self._message_factory, Level.INFO)
self._clock = Clock(_config, self._message_bus, self._message_factory,
Level.INFO)
# attempt to find the gamepad
self._gamepad = Gamepad(_config, self._message_bus,
self._message_factory, Level.INFO)
# if self._enable_indicator:
# self._indicator = Indicator(Level.INFO)
# if self._enable_compass:
# self._compass = Compass(_config, self._queue, self._indicator, Level.INFO)
# self._video.set_compass(self._compass)
_enable_battery_check = False
if _enable_battery_check:
self._log.info('starting battery check thread...')
self._battery_check = BatteryCheck(_config, self._queue,
self._message_factory,
Level.INFO)
else:
self._battery_check = None
if self._enable_ifs:
self._log.info('integrated front sensor enabled.')
self._ifs = IntegratedFrontSensor(_config, self._clock,
self._message_bus,
self._message_factory,
Level.INFO)
# add indicator as message consumer
if self._enable_indicator:
self._queue.add_consumer(self._indicator)
else:
self._ifs = None
self._log.info('integrated front sensor disabled.')
# self._ctrl = GamepadController(_config, self._queue, self._pid_motor_ctrl, self._ifs, self._video, self._blob, matrix11x7_stbd_available, Level.INFO, self._close_demo_callback)
self._ctrl = GamepadController(_config, self._message_bus,
self._pid_motor_ctrl, self._ifs,
self._video, self._blob,
matrix11x7_stbd_available, Level.INFO,
self._close_demo_callback)
self._message_bus.add_handler(Message, self._ctrl.handle_message)
self._enabled = False
self._log.info('connecting gamepad...')
self._gamepad.connect()
self._log.info('ready.')
# ..........................................................................
def get_motors(self):
return self._motors
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
if self._enabled:
self._log.warning('already enabled.')
return
self._log.info('enabling...')
self._gamepad.enable()
self._clock.enable()
# if self._enable_compass:
# self._compass.enable()
if self._battery_check:
self._battery_check.enable()
if self._enable_ifs:
self._ifs.enable()
self._ctrl.enable()
self._enabled = True
self._log.info('enabled.')
# ..........................................................................
def get_thread_position(self, thread):
frame = sys._current_frames().get(thread.ident, None)
if frame:
return frame.f_code.co_filename, frame.f_code.co_name, frame.f_code.co_firstlineno
# ..........................................................................
def disable(self):
if not self._enabled:
self._log.warning('already disabled.')
return
self._log.info('disabling...')
self._enabled = False
self._clock.disable()
if self._battery_check:
self._battery_check.disable()
# if self._enable_compass:
# self._compass.disable()
if self._enable_ifs:
self._ifs.disable()
self._pid_motor_ctrl.disable()
self._gamepad.disable()
_show_thread_info = False
if _show_thread_info:
for thread in threading.enumerate():
self._log.info(
Fore.GREEN +
'thread "{}" is alive? {}; is daemon? {}\t😡'.format(
thread.name, thread.is_alive(), thread.isDaemon()))
if thread is not None:
_position = self.get_thread_position(thread)
if _position:
self._log.info(
Fore.GREEN +
' thread "{}" filename: {}; co_name: {}; first_lineno: {}'
.format(thread.name, _position[0], _position[1],
_position[2]))
else:
self._log.info(Fore.GREEN +
' thread "{}" position null.'.format(
thread.name))
else:
self._log.info(Fore.GREEN + ' null thread.')
self._log.info('disabled.')
# ..........................................................................
def _close_demo_callback(self):
self._log.info(Fore.MAGENTA + 'close demo callback...')
# self._queue.disable()
self.disable()
self.close()
# ..........................................................................
def close(self):
if self._enabled:
self.disable()
self._log.info('closing...')
if self._enable_ifs:
self._ifs.close()
self._pid_motor_ctrl.close()
self._gamepad.close()
self._log.info('closed.')
]
for feature in _features:
count += 1
_log.info(Fore.YELLOW + '{}; enabling feature: {}'.format(count, feature))
bno.enable_feature(feature)
time.sleep(0.02)
_log.info(Fore.YELLOW + 'ready: enabled {:d} features.'.format(count))
except Exception as e:
_log.error('error enabling feature: {}'.format(e))
sys.exit(1)
_enabled = 0
_log.heading('features enabled', 'starting loop...', '[{:d}/14]'.format(_enabled) )
while True:
time.sleep(0.1)
try:
if ENABLE_ACCELEROMETER:
accel_x, accel_y, accel_z = bno.acceleration # pylint:disable=no-member
_log.heading('acceleration', \
'X: {:0.6f} Y: {:0.6f} Z: {:0.6f} m/s^2'.format(accel_x, accel_y, accel_z), \
'[{:d}/14]'.format(_enabled))
print("")
if ENABLE_GYROSCOPE:
class Gamepad():
_NOT_AVAILABLE_ERROR = 'gamepad device not found (not configured, paired, powered or otherwise available)'
def __init__(self, config, message_bus, message_factory, level):
'''
Parameters:
config: the YAML-based application configuration
message_bus: the message bus to receive messages from this task
message_factory: the factory for creating messages
mutex: vs godzilla
'''
if config is None:
raise ValueError('no configuration provided.')
self._level = level
self._log = Logger("gamepad", level)
self._log.info('initialising...')
self._config = config
_config = config['ros'].get('gamepad')
# config
_loop_freq_hz = _config.get('loop_freq_hz')
self._rate = Rate(_loop_freq_hz)
self._device_path = _config.get('device_path')
self._message_bus = message_bus
self._message_factory = message_factory
self._gamepad_closed = False
self._closed = False
self._enabled = False
self._thread = None
self._gamepad = None
# ..........................................................................
def connect(self):
'''
Scan for likely gamepad device, and if found, connect.
Otherwise we raise an OSError.
'''
_scan = GamepadScan(self._config, self._level)
_matches = _scan.check_gamepad_device()
if not _matches:
self._log.warning(
'no connection attempted: gamepad is not the most recent device (configured at: {}).'
.format(self._device_path))
raise OSError('no gamepad available.')
else:
self._connect()
# ..........................................................................
def has_connection(self):
return self._gamepad != None
# ..........................................................................
def _connect(self):
self._log.heading('gamepad', 'Connecting Gamepad...', None)
try:
self._gamepad = InputDevice(self._device_path)
# display device info
self._log.info(Fore.GREEN + "gamepad: {}".format(self._gamepad))
self._log.info('connected.')
except Exception as e:
self._enabled = False
self._gamepad = None
raise GamepadConnectException(
'unable to connect to input device path {}: {}'.format(
self._device_path, e))
# ..........................................................................
def enable(self):
if not self._closed:
self._log.info('enabled gamepad.')
if not self.in_loop():
self._enabled = True
self._start_gamepad_loop()
else:
self._log.error('cannot start gamepad.')
self._enabled = False
else:
self._log.warning('cannot enable gamepad: already closed.')
self._enabled = False
# ..........................................................................
def in_loop(self):
'''
Returns true if the main loop is active (the thread is alive).
'''
return self._thread != None and self._thread.is_alive()
# ......................................................
@staticmethod
def convert_range(value):
return ((value - 127.0) / 255.0) * -2.0
# ..........................................................................
def _gamepad_loop(self, f_is_enabled):
self._log.info('starting event loop...')
__enabled = True
while __enabled and f_is_enabled():
try:
if self._gamepad is None:
raise Exception(Gamepad._NOT_AVAILABLE_ERROR +
' [gamepad no longer available]')
# loop and filter by event code and print the mapped label
for event in self._gamepad.read_loop():
self._handleEvent(event)
if not f_is_enabled():
self._log.info(Fore.BLACK +
'breaking from event loop.')
break
except Exception as e:
self._log.error('gamepad device error: {}'.format(e))
except OSError as e:
self._log.error(Gamepad._NOT_AVAILABLE_ERROR +
' [lost connection to gamepad]')
finally:
'''
Note that closing the InputDevice is a bit tricky, and we're currently
masking an exception that's always thrown. As there is no data loss on
a gamepad event loop being closed suddenly this is not an issue.
'''
try:
self._log.info('closing gamepad device...')
self._gamepad.close()
self._log.info(Fore.YELLOW + 'gamepad device closed.')
except Exception as e:
self._log.debug(
'error closing gamepad device: {}'.format(e))
finally:
__enabled = False
self._gamepad_closed = True
self._rate.wait()
self._log.info('exited event loop.')
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def _start_gamepad_loop(self):
'''
This is the method to call to actually start the loop.
'''
if not self._enabled:
self._log.error(
'attempt to start gamepad event loop while disabled.')
elif self._gamepad is None:
self._log.error(Gamepad._NOT_AVAILABLE_ERROR +
' [no gamepad found]')
sys.exit(3)
elif not self._closed:
if self._thread is None:
self._enabled = True
self._thread = Thread(name='gamepad',
target=Gamepad._gamepad_loop,
args=[self, lambda: self._enabled],
daemon=True)
# self._thread.setDaemon(False)
self._thread.start()
self._log.info('started.')
else:
self._log.warning('cannot enable: process already running.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
def disable(self):
if self._closed:
self._log.warning('can\'t disable: already closed.')
elif not self._enabled:
self._log.debug('already disabled.')
else:
self._enabled = False
# we'll wait a bit for the gamepad device to close...
time.sleep(2.0)
# _i = 0
# while not self._gamepad_closed and _i < 20:
# _i += 1
# self._log.debug('_i: {:d}'.format(_i))
# time.sleep(0.1)
self._log.info('disabled.')
# ..........................................................................
def close(self):
'''
Permanently close and disable the gamepad.
'''
if self._enabled:
self.disable()
if not self._closed:
self._closed = True
self._log.info('closed.')
else:
self._log.debug('already closed.')
# ..........................................................................
def _handleEvent(self, event):
'''
Handles the incoming event by filtering on event type and code.
There's possibly a more elegant way of doing this but for now this
works just fine.
'''
_message = None
_control = None
if event.type == ecodes.EV_KEY:
_control = GamepadControl.get_by_code(self, event.code)
if event.value == 1:
if event.code == GamepadControl.A_BUTTON.code:
self._log.info(Fore.RED + "A Button")
# _control = GamepadControl.A_BUTTON
elif event.code == GamepadControl.B_BUTTON.code:
self._log.info(Fore.RED + "B Button")
# _control = GamepadControl.B_BUTTON
elif event.code == GamepadControl.X_BUTTON.code:
self._log.info(Fore.RED + "X Button")
# _control = GamepadControl.X_BUTTON
elif event.code == GamepadControl.Y_BUTTON.code:
self._log.info(Fore.RED + "Y Button")
# _control = GamepadControl.Y_BUTTON
elif event.code == GamepadControl.L1_BUTTON.code:
self._log.info(Fore.YELLOW + "L1 Button")
# _control = GamepadControl.L1_BUTTON
elif event.code == GamepadControl.L2_BUTTON.code:
self._log.info(Fore.YELLOW + "L2 Button")
# _control = GamepadControl.L2_BUTTON
elif event.code == GamepadControl.R1_BUTTON.code:
self._log.info(Fore.YELLOW + "R1 Button")
# _control = GamepadControl.R1_BUTTON
elif event.code == GamepadControl.R2_BUTTON.code:
self._log.info(Fore.YELLOW + "R2 Button")
# _control = GamepadControl.R2_BUTTON
elif event.code == GamepadControl.START_BUTTON.code:
self._log.info(Fore.GREEN + "Start Button")
# _control = GamepadControl.START_BUTTON
elif event.code == GamepadControl.SELECT_BUTTON.code:
self._log.info(Fore.GREEN + "Select Button")
# _control = GamepadControl.SELECT_BUTTON
elif event.code == GamepadControl.HOME_BUTTON.code:
self._log.info(Fore.MAGENTA + "Home Button")
# _control = GamepadControl.HOME_BUTTON
else:
self._log.info(Fore.BLACK +
"event type: EV_KEY; event: {}; value: {}".
format(event.code, event.value))
else:
# self._log.info(Fore.BLACK + Style.DIM + "event type: EV_KEY; value: {}".format(event.value))
pass
elif event.type == ecodes.EV_ABS:
_control = GamepadControl.get_by_code(self, event.code)
if event.code == GamepadControl.DPAD_HORIZONTAL.code:
if event.value == 1:
self._log.info(
Fore.CYAN + Style.BRIGHT +
"D-Pad Horizontal(Right) {}".format(event.value))
elif event.value == -1:
self._log.info(
Fore.CYAN + Style.NORMAL +
"D-Pad Horizontal(Left) {}".format(event.value))
else:
self._log.info(
Fore.BLACK +
"D-Pad Horizontal(N) {}".format(event.value))
elif event.code == GamepadControl.DPAD_VERTICAL.code:
if event.value == -1:
self._log.info(Fore.CYAN + Style.NORMAL +
"D-Pad Vertical(Up) {}".format(event.value))
elif event.value == 1:
self._log.info(
Fore.CYAN + Style.BRIGHT +
"D-Pad Vertical(Down) {}".format(event.value))
else:
self._log.info(Fore.BLACK +
"D-Pad Vertical(N) {}".format(event.value))
elif event.code == GamepadControl.L3_VERTICAL.code:
self._log.debug(Fore.MAGENTA +
"L3 Vertical {}".format(event.value))
elif event.code == GamepadControl.L3_HORIZONTAL.code:
self._log.debug(Fore.YELLOW +
"L3 Horizontal {}".format(event.value))
elif event.code == GamepadControl.R3_VERTICAL.code:
self._log.debug(Fore.GREEN +
"R3 Vertical {}".format(event.value))
# _control = GamepadControl.R3_VERTICAL
elif event.code == GamepadControl.R3_HORIZONTAL.code:
self._log.debug(Fore.GREEN +
"R3 Horizontal {}".format(event.value))
# _control = GamepadControl.R3_HORIZONTAL
else:
# self._log.info(Fore.BLACK + "type: EV_ABS; event code: {}; value: {}".format(event.code, event.value))
pass
else:
# self._log.info(Fore.BLACK + Style.DIM + "ZZ. event type: {}; code: {}; value: {}".format(event.type, event.code, event.value))
pass
if _control != None:
_message = self._message_factory.get_message(
_control.event, event.value)
self._log.debug(
Fore.CYAN + Style.BRIGHT +
"triggered control with message {}".format(_message))
self._message_bus.add(_message)
# of the Robot Operating System project, released under the MIT License. Please
# see the LICENSE file included as part of this package.
#
from colorama import init, Fore, Style
init()
from lib.logger import Logger, Level
_log = Logger("color-test", Level.DEBUG)
_log.debug('debug.')
_log.info('info.')
_log.notice('notice.')
_log.warning('warning.')
_log.error('error.')
_log.critical('critical.')
_log.heading('title', 'message.', 'info [0/0]')
_log.info(Fore.RED + 'RED')
_log.info(Fore.GREEN + 'GREEN')
_log.info(Fore.BLUE + 'BLUE')
_log.info(Fore.YELLOW + 'YELLOW')
_log.info(Fore.MAGENTA + 'MAGENTA')
_log.info(Fore.CYAN + 'CYAN')
_log.info(Fore.BLACK + 'BLACK')
_log.info(Fore.WHITE + 'WHITE')
