def main():
try:
_log = Logger("test", Level.INFO)
_log.info(Fore.BLUE + 'configuring...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_message_bus = MessageBus(Level.INFO)
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_clock.enable()
_ifs = IntegratedFrontSensor(_config, _clock, Level.INFO)
_ifs.enable()
_handler = MessageHandler(Level.INFO)
_message_bus.add_handler(Message, _handler.handle)
try:
_log.info(Fore.BLUE + 'starting loop... type Ctrl-C to exit.')
_rate = Rate(1)
# while True:
for i in range(360):
_rate.wait()
_log.info(Fore.BLUE + 'exited loop.')
except KeyboardInterrupt:
print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL)
except Exception as e:
print(Fore.RED + Style.BRIGHT + 'error: {}'.format(e))
traceback.print_exc(file=sys.stdout)
def test_ifs():
'''
Test the functionality of Integrated Front Sensor.
'''
_log = Logger("test-ifs", Level.INFO)
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_message_bus = MessageBus(Level.INFO)
_log.info('creating clock...')
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_ifs = IntegratedFrontSensor(_config, _clock, Level.INFO)
# establish queue to receive messages
_queue = MockMessageQueue(Level.INFO)
_message_bus.add_handler(Message, _queue.add)
_ifs.enable()
_clock.enable()
while not _queue.all_triggered:
_queue.waiting_for_message()
time.sleep(0.5)
_ifs.disable()
assert _queue.count > 0
_log.info('test complete.' + Style.RESET_ALL)
def main():
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
# _queue = MessageQueue(_message_factory, Level.INFO)
_queue = MockMessageQueue(Level.INFO)
_clock = Clock(_config, _queue, _message_factory, Level.INFO)
_clock.enable()
while True:
time.sleep(1.0)
except KeyboardInterrupt:
print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL)
except Exception as e:
print(Fore.RED + Style.BRIGHT +
'error starting ifs: {}\n{}'.format(e, traceback.format_exc()) +
Style.RESET_ALL)
finally:
pass
def test_temperature():
_temperature = None
_fan = None
_log = Logger('temp-test', Level.INFO)
try:
# scan I2C bus
_log.info('scanning I²C address bus...')
scanner = I2CScanner(_log.level)
_addresses = scanner.get_int_addresses()
# load configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
# create Fan if available
_fan_config = _config['ros'].get('fan')
_fan_i2c_address = _fan_config.get('i2c_address')
fan_available = (_fan_i2c_address in _addresses)
if fan_available:
_fan = Fan(_config, Level.INFO)
_log.info('fan is available at I²C address of 0x{:02X}.'.format(
_fan_i2c_address))
else:
_fan = None
_log.info(
'fan is not available at I²C address of 0x{:02X}.'.format(
_fan_i2c_address))
_message_factory = MessageFactory(Level.INFO)
_message_bus = MessageBus(Level.DEBUG)
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_temperature = Temperature(_config, _clock, _fan, Level.INFO)
_temperature.enable()
_counter = itertools.count()
_clock.enable()
while True:
_count = next(_counter)
# _temperature.display_temperature()
# _value = _temperature.get_cpu_temperature()
# _is_warning_temp = _temperature.is_warning_temperature()
# _is_max_temp = _temperature.is_max_temperature()
# if _is_warning_temp:
# _log.warning('[{:04d}] loop; is warning temp? {}; is max temp? {}'.format(_count, _is_warning_temp, _is_max_temp))
# else:
# _log.info('[{:04d}] loop; is warning temp? {}; is max temp? {}'.format(_count, _is_warning_temp, _is_max_temp))
time.sleep(1.0)
except KeyboardInterrupt:
_log.info(Fore.YELLOW + 'exited via Ctrl-C' + Style.RESET_ALL)
except Exception:
_log.error(Fore.RED + Style.BRIGHT +
'error getting RPI temperature: {}'.format(
traceback.format_exc()) + Style.RESET_ALL)
finally:
if _temperature:
_temperature.close()
if _fan:
_fan.disable()
def test_clock():
global tock_count
_log = Logger('clock-test', Level.INFO)
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
tock_count = 0
_message_factory = MessageFactory(Level.INFO)
_message_bus = MockMessageBus(Level.INFO)
_clock = Clock(_config, _message_bus, _message_factory, Level.INFO)
_message_bus.set_clock(_clock)
_clock.enable()
_log.info('ready; begin test.')
_loops = 3
while INFINITE or tock_count < _loops:
time.sleep(1.0)
_log.info('test 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.')
def main(argv):
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
_msg_handler = MessageHandler(Level.INFO)
# _tick_handler = TickHandler(_ifs, Level.INFO)
# create message bus...
_message_bus = MessageBus(Level.INFO)
_message_bus.add_handler(Message, _msg_handler.add)
# _message_bus.add_handler(Tick, _tick_handler.tick)
# _message_bus.add_handler(Tock, _tick_handler.tock)
# _message_bus.add_handler(Tick, _tick_handler.add)
# _message_bus.add_consumer(_ifs)
_clock = Clock(_config, _message_bus, _message_factory, Level.INFO)
_ifs = IntegratedFrontSensor(_config, _queue, _clock, _message_bus,
_message_factory, Level.INFO)
_clock.enable()
_ifs.enable()
while True:
time.sleep(1.0)
except KeyboardInterrupt:
print(Fore.CYAN + Style.BRIGHT + 'caught Ctrl-C; exiting...')
except Exception:
print(Fore.RED + Style.BRIGHT +
'error starting ros: {}'.format(traceback.format_exc()) +
Style.RESET_ALL)
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.')
