def __init__(self, config={}):
self._config.update(config)
self._stepper = Stepper()
self._stepper.disable()
self._servo = Servo()
self.set_vertical_position(0)
self._lidar = Lidar()
def __init__(self):
destiny_coords = [29.15168, -81.01726];
partial_degree = 0;
camera = Camera();
camera_frame = None;
lidar = Lidar();
radar = Radar();
imu = Imu();
navigation = Navigation();
destiny = imu.get_degrees_and_distance_to_gps_coords(29.15168, -81.01726);
# Create new threads
thread_camera = CameraThread(1, "CameraThread");
thread_lidar = LidarThread(2, "LidarThread");
thread_navigation = NavigationThread(3, "NavigationThread");
thread_main = MainThread(4, "MainThread");
# Start Threads
thread_camera.start();
thread_lidar.start();
thread_main.start();
thread_navigation.start();
thread_camera.join();
thread_lidar.join();
thread_main.join();
thread_navigation.join();
print ("Terminating Main Program");
def main():
signal.signal(signal.SIGINT, signal_handler)
_log = Logger("scanner", Level.INFO)
_log.info(Fore.CYAN + Style.BRIGHT + ' INFO : starting test...')
_log.info(Fore.YELLOW + Style.BRIGHT + ' INFO : Press Ctrl+C to exit.')
try:
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.getAddresses()
hexAddresses = _i2c_scanner.getHexAddresses()
_addrDict = dict(
list(map(lambda x, y: (x, y), _addresses, hexAddresses)))
for i in range(len(_addresses)):
_log.debug(Fore.BLACK + Style.DIM +
'found device at address: {}'.format(hexAddresses[i]))
vl53l1x_available = (0x29 in _addresses)
ultraborg_available = (0x36 in _addresses)
if not vl53l1x_available:
raise OSError('VL53L1X hardware dependency not available.')
elif not ultraborg_available:
raise OSError('UltraBorg hardware dependency not available.')
_log.info('starting scan...')
# _player = Player(Level.INFO)
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_lidar = Lidar(_config, Level.INFO)
_lidar.enable()
values = _lidar.scan()
_angle_at_min = values[0]
_min_mm = values[1]
_angle_at_max = values[2]
_max_mm = values[3]
_log.info(
Fore.CYAN + Style.BRIGHT +
'min. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_min, _min_mm))
_log.info(
Fore.CYAN + Style.BRIGHT +
'max. distance at {:>5.2f}°:\t{}mm'.format(_angle_at_max, _max_mm))
time.sleep(1.0)
_lidar.close()
_log.info(Fore.CYAN + Style.BRIGHT + 'test complete.')
except Exception:
_log.info(traceback.format_exc())
sys.exit(1)
def run(self):
'''
This first disables the Pi's status LEDs, establishes the
message queue arbitrator, the controller, enables the set
of features, then starts the main OS loop.
'''
super(AbstractTask, self).run()
loop_count = 0
self._disable_leds = self._config['pi'].get('disable_leds')
if self._disable_leds:
# disable Pi LEDs since they may be distracting
self._set_pi_leds(False)
self._log.info('enabling features...')
for feature in self._features:
self._log.info('enabling feature {}...'.format(feature.name()))
feature.enable()
__enable_player = self._config['ros'].get('enable_player')
if __enable_player:
self._log.info('configuring sound player...')
self._player = Player(Level.INFO)
else:
self._player = None
vl53l1x_available = True # self.get_property('features', 'vl53l1x')
self._log.critical('vl53l1x_available? {}'.format(vl53l1x_available))
ultraborg_available = True # self.get_property('features', 'ultraborg')
self._log.critical(
'ultraborg available? {}'.format(ultraborg_available))
if vl53l1x_available and ultraborg_available:
self._log.critical('starting scanner tool...')
self._lidar = Lidar(self._config, self._player, Level.INFO)
self._lidar.enable()
else:
self._log.critical(
'scanner tool does not have necessary dependencies.')
# wait to stabilise features?
# configure the Controller and Arbitrator
self._log.info('configuring controller...')
self._controller = Controller(Level.INFO, self._config, self._switch,
self._infrareds, self._motors,
self._rgbmatrix, self._lidar,
self._callback_shutdown)
self._log.info('configuring arbitrator...')
self._arbitrator = Arbitrator(_level, self._queue, self._controller,
self._mutex)
flask_enabled = self._config['flask'].get('enabled')
if flask_enabled:
self._log.info('starting flask web server...')
self.configure_web_server()
else:
self._log.info(
'not starting flask web server (suppressed from command line).'
)
self._log.warning('Press Ctrl-C to exit.')
wait_for_button_press = self._config['ros'].get(
'wait_for_button_press')
self._controller.set_standby(wait_for_button_press)
# begin main loop ..............................
# self._log.info('starting battery check thread...')
# self._battery_check.enable()
self._log.info('starting button thread...')
self._button.start()
# self._log.info('enabling bno055 sensor...')
# self._bno055.enable()
self._log.info('enabling bumpers...')
self._bumpers.enable()
# self._log.info('starting info thread...')
# self._info.start()
# self._log.info('starting blinky thread...')
# self._rgbmatrix.enable(DisplayType.RANDOM)
# enable arbitrator tasks (normal functioning of robot)
main_loop_delay_ms = self._config['ros'].get('main_loop_delay_ms')
self._log.info(
'begin main os loop with {:d}ms delay.'.format(main_loop_delay_ms))
_loop_delay_sec = main_loop_delay_ms / 1000
self._arbitrator.start()
while self._arbitrator.is_alive():
# The sensors and the flask service sends messages to the message queue,
# which forwards those messages on to the arbitrator, which chooses the
# highest priority message to send on to the controller. So the timing
# of this loop is inconsequential; it exists solely as a keep-alive.
time.sleep(_loop_delay_sec)
# end application loop .........................
if not self._closing:
self._log.warning('closing following loop...')
self.close()
def run(self):
'''
This first disables the Pi's status LEDs, establishes the
message queue arbitrator, the controller, enables the set
of features, then starts the main OS loop.
'''
super(AbstractTask, self).run()
loop_count = 0
# display banner!
_banner = '\n' \
'ros\n' \
'ros █▒▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒ \n' \
+ 'ros █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ \n' \
+ 'ros █▒▒▒▒▒▒ █▒▒ █▒▒ █▒▒▒▒▒▒ █▒▒ \n' \
+ 'ros █▒▒ █▒▒ █▒▒ █▒▒ █▒▒ \n' \
+ 'ros █▒▒ █▒▒ █▒▒▒▒▒▒ █▒▒▒▒▒▒ █▒▒ \n' \
+ 'ros\n'
self._log.info(_banner)
self._disable_leds = self._config['pi'].get('disable_leds')
if self._disable_leds:
# disable Pi LEDs since they may be distracting
self._set_pi_leds(False)
self._log.info('enabling features...')
for feature in self._features:
self._log.info('enabling feature {}...'.format(feature.name()))
feature.enable()
# __enable_player = self._config['ros'].get('enable_player')
# if __enable_player:
# self._log.info('configuring sound player...')
# self._player = Player(Level.INFO)
# else:
# self._player = None
# i2c_slave_address = config['ros'].get('i2c_master').get('device_id') # i2c hex address of I2C slave device
vl53l1x_available = True # self.get_property('features', 'vl53l1x')
ultraborg_available = True # self.get_property('features', 'ultraborg')
if vl53l1x_available and ultraborg_available:
self._log.critical('starting scanner tool...')
self._lidar = Lidar(self._config, Level.INFO)
self._lidar.enable()
else:
self._log.critical(
'lidar scanner tool does not have necessary dependencies.')
# wait to stabilise features?
# configure the Controller and Arbitrator
self._log.info('configuring controller...')
self._controller = Controller(self._config, self._ifs, self._motors,
self._callback_shutdown, Level.INFO)
self._log.info('configuring arbitrator...')
self._arbitrator = Arbitrator(self._config, self._queue,
self._controller, Level.WARN)
_flask_enabled = self._config['flask'].get('enabled')
if _flask_enabled:
self._log.info('starting flask web server...')
self.configure_web_server()
else:
self._log.info(
'not starting flask web server (suppressed from command line).'
)
# bluetooth gamepad controller
if self._gamepad_enabled:
self._connect_gamepad()
self._log.warning('Press Ctrl-C to exit.')
_wait_for_button_press = self._config['ros'].get(
'wait_for_button_press')
self._controller.set_standby(_wait_for_button_press)
# begin main loop ..............................
self._log.info('starting button thread...')
self._button.start()
# self._log.info('enabling bno055 sensor...')
# self._bno055.enable()
# self._bumpers.enable()
self._indicator = Indicator(Level.INFO)
# add indicator as message consumer
self._queue.add_consumer(self._indicator)
self._log.info(Fore.MAGENTA + 'enabling integrated front sensor...')
self._ifs.enable()
# self._log.info('starting info thread...')
# self._info.start()
# self._log.info('starting blinky thread...')
# self._rgbmatrix.enable(DisplayType.RANDOM)
# enable arbitrator tasks (normal functioning of robot)
main_loop_delay_ms = self._config['ros'].get('main_loop_delay_ms')
self._log.info(
'begin main os loop with {:d}ms delay.'.format(main_loop_delay_ms))
_loop_delay_sec = main_loop_delay_ms / 1000
_main_loop_count = 0
self._arbitrator.start()
self._active = True
while self._active:
# The sensors and the flask service sends messages to the message queue,
# which forwards those messages on to the arbitrator, which chooses the
# highest priority message to send on to the controller. So the timing
# of this loop is inconsequential; it exists solely as a keep-alive.
_main_loop_count += 1
self._log.debug(Fore.BLACK + Style.DIM +
'[{:d}] main loop...'.format(_main_loop_count))
time.sleep(_loop_delay_sec)
# end application loop .........................
if not self._closing:
self._log.warning('closing following loop...')
self.close()