def _connect_gamepad(self): if not self._gamepad_enabled: self._log.info('gamepad disabled.') return if self._gamepad is None: self._log.info('creating gamepad...') try: self._gamepad = Gamepad(self._config, self._queue, Level.INFO) except GamepadConnectException as e: self._log.error('unable to connect to gamepad: {}'.format(e)) self._gamepad = None self._gamepad_enabled = False self._log.info('gamepad unavailable.') return if self._gamepad is not None: self._log.info('enabling gamepad...') self._gamepad.enable() _count = 0 while not self._gamepad.has_connection(): _count += 1 if _count == 1: self._log.info('connecting to gamepad...') else: self._log.info( 'gamepad not connected; re-trying... [{:d}]'.format( _count)) self._gamepad.connect() time.sleep(0.5) if self._gamepad.has_connection() or _count > 5: break
def initUI(self, data): self.subWindows = [] gamepadID = 0 for robot in data['Robot']: if (robot['KeyConfig'] == 'gamepad'): gamepad_subwindow = Gamepad(self, robot, gamepadID) self.subWindows.append(gamepad_subwindow) self.calcVelocityGamepadThreads.append( calcVelocityGamepad(gamepad_subwindow)) gamepadID += 1 else: self.subWindows.append(Keyboard(self, robot)) self.layout = QtGui.QVBoxLayout(self) for subwindow in self.subWindows: self.layout.addWidget(subwindow) self.resize(300, len(data['Robot']) * 130)
def main(): _level = Level.INFO _log = Logger('main', _level) _loader = ConfigLoader(_level) _config = _loader.configure('config.yaml') try: _motors = Motors(_config, None, Level.WARN) _pot = Potentiometer(_config, Level.WARN) _pot.set_output_limits(0.0, 127.0) # motor configuration: starboard, port or both? _orientation = Orientation.BOTH if _orientation == Orientation.BOTH or _orientation == Orientation.PORT: _port_motor = _motors.get_motor(Orientation.PORT) _port_pid = PIDController(_config, _port_motor, level=Level.WARN) _port_pid.enable() if _orientation == Orientation.BOTH or _orientation == Orientation.STBD: _stbd_motor = _motors.get_motor(Orientation.STBD) _stbd_pid = PIDController(_config, _stbd_motor, level=Level.WARN) _stbd_pid.enable() ROTATE = True _rotate = -1.0 if ROTATE else 1.0 # sys.exit(0) _stbd_velocity = 0.0 _port_velocity = 0.0 _step = 0.5 _min = 0.0 _max = 70.0 _rate = Rate(10) try: # for _value in numpy.arange(_min, _max, _step): while True: # update RGB LED # _pot.set_rgb(_pot.get_value()) _value = 127.0 - _pot.get_scaled_value(True) if _value > 125.0: _value = 127.0 _velocity = Gamepad.convert_range(_value) if _orientation == Orientation.BOTH or _orientation == Orientation.PORT: _port_pid.setpoint = _velocity * 100.0 _port_velocity = _port_pid.setpoint if _orientation == Orientation.BOTH or _orientation == Orientation.STBD: _stbd_pid.setpoint = _rotate * _velocity * 100.0 _stbd_velocity = _stbd_pid.setpoint _log.info(Fore.WHITE + 'value: {:<5.2f}; set-point: {:5.2f}; velocity: '.format(_value, _velocity) \ + Fore.RED + ' port: {:5.2f}\t'.format(_port_velocity) + Fore.GREEN + ' stbd: {:5.2f}'.format(_stbd_velocity)) _rate.wait() _log.info(Fore.YELLOW + 'resting...') time.sleep(10.0) # for _value in numpy.arange(_min, _max, _step): while True: # update RGB LED # _pot.set_rgb(_pot.get_value()) _value = 127.0 - _pot.get_scaled_value(True) if _value > 125.0: _value = 127.0 _velocity = Gamepad.convert_range(_value) if _orientation == Orientation.BOTH or _orientation == Orientation.PORT: _port_pid.setpoint = _rotate * _velocity * 100.0 _port_velocity = _port_pid.setpoint if _orientation == Orientation.BOTH or _orientation == Orientation.STBD: _stbd_pid.setpoint = _velocity * 100.0 _stbd_velocity = _stbd_pid.setpoint _log.info(Fore.MAGENTA + 'value: {:<5.2f}; set-point: {:5.2f}; velocity: '.format(_value, _velocity) \ + Fore.RED + ' port: {:5.2f}\t'.format(_port_velocity) + Fore.GREEN + ' stbd: {:5.2f}'.format(_stbd_velocity)) _rate.wait() _log.info(Fore.YELLOW + 'end of cruising.') except KeyboardInterrupt: _log.info(Fore.CYAN + Style.BRIGHT + 'PID test complete.') finally: if _orientation == Orientation.BOTH or _orientation == Orientation.PORT: _port_pid.disable() if _orientation == Orientation.BOTH or _orientation == Orientation.STBD: _stbd_pid.disable() _motors.brake() except Exception as e: _log.info(Fore.RED + Style.BRIGHT + 'error in PID controller: {}\n{}'.format(e, traceback.format_exc())) finally: _log.info(Fore.YELLOW + Style.BRIGHT + 'C. finally.')
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.')
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.')
class ROS(AbstractTask): ''' Extends AbstractTask as a Finite State Machine (FSM) basis of a Robot Operating System (ROS) or behaviour-based system (BBS), including spawning the various tasks and an Arbitrator as separate threads, inter-communicating over a common message queue. This establishes a message queue, an Arbitrator and a Controller, as well as an optional RESTful flask-based web service. The message queue receives Event-containing messages, which are passed on to the Arbitrator, whose job it is to determine the highest priority action to execute for that task cycle. Example usage: try: _ros = ROS() _ros.start() except Exception: _ros.close() There is also a rosd linux daemon, which can be used to start, enable and disable ros (actually via its Arbitrator). ''' # .......................................................................... def __init__(self): ''' This initialises the ROS and calls the YAML configurer. ''' self._queue = MessageQueue(Level.INFO) self._mutex = threading.Lock() super().__init__("ros", self._queue, None, None, self._mutex) self._log.info('initialising...') self._active = False self._closing = False self._disable_leds = False # self._switch = None self._motors = None self._arbitrator = None self._controller = None self._gamepad = None self._features = [] # self._flask_wrapper = None # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' self._config = _loader.configure(filename) self._gamepad_enabled = self._config['ros'].get('gamepad').get( 'enabled') self._log.info('initialised.') self._configure() # .......................................................................... def _configure(self): ''' Configures ROS based on both KR01 standard hardware as well as optional features, the latter based on devices showing up (by address) on the I²C bus. Optional devices are only enabled at startup time via registration of their feature availability. ''' scanner = I2CScanner(Level.WARN) self._addresses = scanner.getAddresses() hexAddresses = scanner.getHexAddresses() self._addrDict = dict( list(map(lambda x, y: (x, y), self._addresses, hexAddresses))) for i in range(len(self._addresses)): self._log.debug( Fore.BLACK + Style.DIM + 'found device at address: {}'.format(hexAddresses[i]) + Style.RESET_ALL) self._log.info('configure default features...') # standard devices ........................................... self._log.info('configuring integrated front sensors...') self._ifs = IntegratedFrontSensor(self._config, self._queue, Level.INFO) self._log.info('configure ht0740 switch...') self._switch = Switch(Level.INFO) # since we're using the HT0740 Switch, turn it on to enable sensors that rely upon its power # self._switch.enable() self._switch.on() self._log.info('configuring button...') self._button = Button(self._config, self.get_message_queue(), self._mutex) self._log.info('configure battery check...') _battery_check = BatteryCheck(self._config, self.get_message_queue(), Level.INFO) self.add_feature(_battery_check) self._log.info('configure CPU temperature check...') _temperature_check = Temperature(self._config, self._queue, Level.INFO) self.add_feature(_temperature_check) ultraborg_available = (0x36 in self._addresses) if ultraborg_available: self._log.debug(Fore.CYAN + Style.BRIGHT + '-- UltraBorg available at 0x36.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + '-- no UltraBorg available at 0x36.' + Style.RESET_ALL) self._set_feature_available('ultraborg', ultraborg_available) thunderborg_available = (0x15 in self._addresses) if thunderborg_available: # then configure motors self._log.debug(Fore.CYAN + Style.BRIGHT + '-- ThunderBorg available at 0x15' + Style.RESET_ALL) _motor_configurer = MotorConfigurer(self, self._config, Level.INFO) self._motors = _motor_configurer.configure() else: self._log.debug(Fore.RED + Style.BRIGHT + '-- no ThunderBorg available at 0x15.' + Style.RESET_ALL) self._set_feature_available('thunderborg', thunderborg_available) # optional devices ........................................... # the 5x5 RGB Matrix is at 0x74 for port, 0x77 for starboard rgbmatrix5x5_stbd_available = (0x74 in self._addresses) if rgbmatrix5x5_stbd_available: self._log.debug(Fore.CYAN + Style.BRIGHT + '-- RGB Matrix available at 0x74.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + '-- no RGB Matrix available at 0x74.' + Style.RESET_ALL) self._set_feature_available('rgbmatrix5x5_stbd', rgbmatrix5x5_stbd_available) rgbmatrix5x5_port_available = (0x77 in self._addresses) if rgbmatrix5x5_port_available: self._log.debug(Fore.CYAN + Style.BRIGHT + '-- RGB Matrix available at 0x77.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + '-- no RGB Matrix available at 0x77.' + Style.RESET_ALL) self._set_feature_available('rgbmatrix5x5_port', rgbmatrix5x5_port_available) if rgbmatrix5x5_stbd_available or rgbmatrix5x5_port_available: self._log.info('configure rgbmatrix...') self._rgbmatrix = RgbMatrix(Level.INFO) self.add_feature(self._rgbmatrix) # FIXME this is added twice # ............................................ # 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. matrix11x7_stbd_available = (0x75 in self._addresses) if matrix11x7_stbd_available: self._log.debug(Fore.CYAN + Style.BRIGHT + '-- 11x7 Matrix LEDs available at 0x75.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + '-- no 11x7 Matrix LEDs available at 0x75.' + Style.RESET_ALL) self._set_feature_available('matrix11x7_stbd', matrix11x7_stbd_available) # device availability ........................................ bno055_available = (0x28 in self._addresses) if bno055_available: self._log.info('configuring BNO055 9DoF sensor...') self._bno055 = BNO055(self._config, self.get_message_queue(), Level.INFO) else: self._log.debug(Fore.RED + Style.BRIGHT + 'no BNO055 orientation sensor available at 0x28.' + Style.RESET_ALL) self._set_feature_available('bno055', bno055_available) # NOTE: the default address for the ICM20948 is 0x68, but this conflicts with the PiJuice icm20948_available = (0x69 in self._addresses) if icm20948_available: self._log.debug(Fore.CYAN + Style.BRIGHT + 'ICM20948 available at 0x69.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + 'no ICM20948 available at 0x69.' + Style.RESET_ALL) self._set_feature_available('icm20948', icm20948_available) lsm303d_available = (0x1D in self._addresses) if lsm303d_available: self._log.debug(Fore.CYAN + Style.BRIGHT + 'LSM303D available at 0x1D.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + 'no LSM303D available at 0x1D.' + Style.RESET_ALL) self._set_feature_available('lsm303d', lsm303d_available) vl53l1x_available = (0x29 in self._addresses) if vl53l1x_available: self._log.debug(Fore.CYAN + Style.BRIGHT + 'VL53L1X available at 0x29.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + 'no VL53L1X available at 0x29.' + Style.RESET_ALL) self._set_feature_available('vl53l1x', vl53l1x_available) as7262_available = (0x49 in self._addresses) if as7262_available: self._log.debug(Fore.CYAN + Style.BRIGHT + '-- AS7262 Spectrometer available at 0x49.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + '-- no AS7262 Spectrometer available at 0x49.' + Style.RESET_ALL) self._set_feature_available('as7262', as7262_available) pijuice_available = (0x68 in self._addresses) if pijuice_available: self._log.debug(Fore.CYAN + Style.BRIGHT + 'PiJuice hat available at 0x68.' + Style.RESET_ALL) else: self._log.debug(Fore.RED + Style.BRIGHT + 'no PiJuice hat available at 0x68.' + Style.RESET_ALL) self._set_feature_available('pijuice', pijuice_available) self._log.info('configured.') # .......................................................................... def summation(): ''' Displays unaccounted I2C addresses. This is currently non-functional, as we don't remove a device from the list when its address is found. ''' self._log.info(Fore.YELLOW + '-- unaccounted for self._addresses:') for i in range(len(self._addresses)): hexAddr = self._addrDict.get(self._addresses[i]) self._log.info(Fore.YELLOW + Style.BRIGHT + '-- address: {}'.format(hexAddr) + Style.RESET_ALL) # .......................................................................... def _set_feature_available(self, name, value): ''' Sets a feature's availability to the boolean value. ''' self._log.debug(Fore.BLUE + Style.BRIGHT + '-- set feature available. name: \'{}\' value: \'{}\'.' .format(name, value)) self.set_property('features', name, value) # .......................................................................... def get_message_queue(self): return self._queue # .......................................................................... def get_controller(self): return self._controller # .......................................................................... def get_configuration(self): return self._config # .......................................................................... def get_property(self, section, property_name): ''' Return the value of the named property of the application configuration, provided its section and property name. ''' return self._config[section].get(property_name) # .......................................................................... def set_property(self, section, property_name, property_value): ''' Set the value of the named property of the application configuration, provided its section, property name and value. ''' self._log.debug( Fore.GREEN + 'set config on section \'{}\' for property key: \'{}\' to value: {}.' .format(section, property_name, property_value)) if section == 'ros': self._config[section].update(property_name=property_value) else: _ros = self._config['ros'] _ros[section].update(property_name=property_value) # .......................................................................... def _set_pi_leds(self, enable): ''' Enables or disables the Raspberry Pi's board LEDs. ''' sudo_name = self.get_property('pi', 'sudo_name') led_0_path = self._config['pi'].get('led_0_path') led_1_path = self._config['pi'].get('led_1_path') if enable: self._log.info('re-enabling LEDs...') os.system('echo 1 | {} tee {}'.format(sudo_name, led_0_path)) os.system('echo 1 | {} tee {}'.format(sudo_name, led_1_path)) else: self._log.debug('disabling LEDs...') os.system('echo 0 | {} tee {}'.format(sudo_name, led_0_path)) os.system('echo 0 | {} tee {}'.format(sudo_name, led_1_path)) # .......................................................................... def _connect_gamepad(self): if not self._gamepad_enabled: self._log.info('gamepad disabled.') return if self._gamepad is None: self._log.info('creating gamepad...') try: self._gamepad = Gamepad(self._config, self._queue, Level.INFO) except GamepadConnectException as e: self._log.error('unable to connect to gamepad: {}'.format(e)) self._gamepad = None self._gamepad_enabled = False self._log.info('gamepad unavailable.') return if self._gamepad is not None: self._log.info('enabling gamepad...') self._gamepad.enable() _count = 0 while not self._gamepad.has_connection(): _count += 1 if _count == 1: self._log.info('connecting to gamepad...') else: self._log.info( 'gamepad not connected; re-trying... [{:d}]'.format( _count)) self._gamepad.connect() time.sleep(0.5) if self._gamepad.has_connection() or _count > 5: break # .......................................................................... def has_connected_gamepad(self): return self._gamepad is not None and self._gamepad.has_connection() # .......................................................................... def get_arbitrator(self): return self._arbitrator # .......................................................................... def add_feature(self, feature): ''' Add the feature to the list of features. Features must have an enable() method. ''' self._features.append(feature) self._log.info('added feature {}.'.format(feature.name())) # .......................................................................... def _callback_shutdown(self): _enable_self_shutdown = self._config['ros'].get('enable_self_shutdown') if _enable_self_shutdown: self._log.critical('callback: shutting down os...') self.close() sys.exit(0) else: self._log.critical('self-shutdown disabled.') # .......................................................................... 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() # end main ................................... # .......................................................................... def configure_web_server(self): ''' Start flask web server. ''' try: self._mutex.acquire() self._log.info('starting web service...') self._flask_wrapper = FlaskWrapperService(self._queue, self._controller) self._flask_wrapper.start() except KeyboardInterrupt: self._log.error('caught Ctrl-C interrupt.') finally: self._mutex.release() time.sleep(1) self._log.info(Fore.BLUE + 'web service started.' + Style.RESET_ALL) # .......................................................................... def emergency_stop(self): ''' Stop immediately, something has hit the top feelers. ''' self._motors.stop() self._log.info(Fore.RED + Style.BRIGHT + 'emergency stop: contact on upper feelers.') # .......................................................................... def send_message(self, message): ''' Send the Message into the MessageQueue. ''' self._queue.add(message) # .......................................................................... def enable(self): super(AbstractTask, self).enable() # .......................................................................... def disable(self): super(AbstractTask, self).disable() # .......................................................................... def close(self): ''' This sets the ROS back to normal following a session. ''' if self._closing: # this also gets called by the arbitrator so we ignore that self._log.info('already closing.') return else: self._active = False self._closing = True self._log.info(Style.BRIGHT + 'closing...') if self._gamepad: self._gamepad.close() if self._motors: self._motors.close() if self._ifs: self._ifs.close() # close features for feature in self._features: self._log.info('closing feature {}...'.format(feature.name())) feature.close() self._log.info('finished closing features.') if self._arbitrator: self._arbitrator.disable() self._arbitrator.close() # self._arbitrator.join(timeout=1.0) if self._controller: self._controller.disable() # if self._flask_wrapper is not None: # self._flask_wrapper.close() super().close() # self._info.close() # self._rgbmatrix.close() # if self._switch: # self._switch.close() # super(AbstractTask, self).close() if self._disable_leds: # restore LEDs self._set_pi_leds(True) # GPIO.cleanup() # self._log.info('joining main thread...') # self.join(timeout=0.5) self._log.info('os closed.') sys.stderr = DevNull() sys.exit(0)
def handle_message(self, message): message.number = next(self._counter) # show elapsed time _delta = dt.datetime.now() - self._start_time _elapsed_ms = int(_delta.total_seconds() * 1000) # we want to limit the messages to 1/5ms, so if elapsed time is less than that we'll just pad it out # set pad to -1 to disable feature if _elapsed_ms < self._min_loop_ms: time.sleep((self._min_loop_ms - _elapsed_ms) / 1000) _delta = dt.datetime.now() - self._start_time _elapsed_ms = int(_delta.total_seconds() * 1000) # self._log.debug('elapsed since last message: {}ms (padded)'.format(_elapsed_ms)) self._log.debug( Fore.MAGENTA + 'handling message #{}: priority {}: {};\telapsed: {}ms'.format( message.number, message.priority, message.description, _elapsed_ms) + " (padded)") else: # self._log.debug('elapsed since last message: {}ms'.format(_elapsed_ms)) self._log.debug( Fore.MAGENTA + 'handling message #{}: priority {}: {};\telapsed: {}ms'.format( message.number, message.priority, message.description, _elapsed_ms) + "") # ........................................ event = message.event if event is Event.INFRARED_PORT_SIDE: self._log.debug(Fore.RED + Style.BRIGHT + 'event: {};\tvalue: {:5.2f}cm'.format( event.description, message.value)) elif event is Event.INFRARED_PORT: self._log.debug(Fore.RED + Style.BRIGHT + 'event: {};\tvalue: {:5.2f}cm'.format( event.description, message.value)) # elif event is Event.INFRARED_CNTR: # _cruising_velocity = self._behaviours.get_cruising_velocity() # _distance = message.value # _clamped_distance = Behaviours.clamp(_distance, 20.0, 100.0) # _mapped_velocity = Behaviours.remap(_clamped_distance, 20.0, 100.0, 0.0, _cruising_velocity) # self._log.info(Fore.BLUE + Style.NORMAL + 'event: {};'.format(event.description) + Fore.YELLOW + '\tmapped velocity: {:5.2f}cm'.format(message.value )) # self._log.info(Fore.BLUE + Style.NORMAL + '_distance: {:5.2f}\t'.format(_distance) + Style.BRIGHT + '\tcruising velocity: {:5.2f}cm;'.format(_cruising_velocity) \ # + Fore.YELLOW + Style.NORMAL + '\tvelocity: {:5.2f}'.format(_mapped_velocity)) # pids = self._pid_motor_ctrl.set_max_velocity(_mapped_velocity) elif event is Event.INFRARED_CNTR: self._log.debug(Fore.BLUE + Style.BRIGHT + 'event: {};\tvalue: {:5.2f}cm'.format( event.description, message.value)) elif event is Event.INFRARED_STBD: self._log.debug(Fore.GREEN + Style.BRIGHT + 'event: {};\tvalue: {:5.2f}cm'.format( event.description, message.value)) elif event is Event.INFRARED_STBD_SIDE: self._log.debug(Fore.GREEN + Style.BRIGHT + 'event: {};\tvalue: {:5.2f}cm'.format( event.description, message.value)) # ........................................ elif event is Event.BUMPER_PORT: self._log.info(Fore.RED + Style.BRIGHT + 'event: {};\tvalue: {:d}'.format( event.description, message.value)) elif event is Event.BUMPER_CNTR: self._log.info(Fore.BLUE + Style.BRIGHT + 'event: {};\tvalue: {:d}'.format( event.description, message.value)) elif event is Event.BUMPER_STBD: self._log.info(Fore.GREEN + Style.BRIGHT + 'event: {};\tvalue: {:d}'.format( event.description, message.value)) elif event is Event.PORT_VELOCITY: if not self._port_pid.enabled: self._port_pid.enable() # if message.value == 0: # on push of button # _value = message.value # with no hysteresis _value = self._hysteresis(message.value) _velocity = Gamepad.convert_range(_value) self._log.debug(Fore.RED + 'PORT: {};\tvalue: {:>5.2f}; velocity: {:>5.2f};'. format(event.description, _value, _velocity)) # self._motors.set_motor(Orientation.PORT, _velocity) self._port_pid.velocity = _velocity * 100.0 elif event is Event.PORT_THETA: # if message.value == 0: # on push of button _velocity = -1 * Gamepad.convert_range(message.value) self._log.debug('{};\tvalue: {:>5.2f}'.format( event.description, _velocity)) elif event is Event.STBD_VELOCITY: if not self._stbd_pid.enabled: self._stbd_pid.enable() # if message.value == 0: # on push of button # _value = message.value # with no hysteresis _value = self._hysteresis(message.value) _velocity = Gamepad.convert_range(_value) self._log.info(Fore.GREEN + 'STBD: {};\tvalue: {:>5.2f}; velocity: {:>5.2f};'. format(event.description, _value, _velocity)) # self._motors.set_motor(Orientation.STBD, _velocity) self._stbd_pid.velocity = _velocity * 100.0 elif event is Event.STBD_THETA: # if message.value == 0: # on push of button _velocity = -1 * Gamepad.convert_range(message.value) self._log.debug('{};\tvalue: {:>5.2f}'.format( event.description, _velocity)) elif event is Event.SHUTDOWN: if message.value == 1: # shutdown on push of button self._log.info('SHUTDOWN event.') self._close() elif event is Event.STANDBY: self._log.info(Fore.YELLOW + Style.DIM + 'STANDBY event: {};\tvalue: {:d}'.format( event.description, message.value)) elif event is Event.HALT: self._log.info(Fore.RED + Style.BRIGHT + 'HALT event: {};\tvalue: {:d}'.format( event.description, message.value)) elif event is Event.BRAKE: self._log.info(Fore.RED + Style.BRIGHT + 'BRAKE event: {};\tvalue: {:d}'.format( event.description, message.value)) if message.value == 1: # shutdown on push of button self._enable_pid(not self._port_pid.enabled) elif event is Event.STOP: self._log.info(Fore.RED + Style.BRIGHT + 'STOP event: {};\tvalue: {:d}'.format( event.description, message.value)) elif event is Event.ROAM: # if message.value == 0: # self._behaviours.roam() pass elif event is Event.SNIFF: # if message.value == 0: # self._behaviours.one_meter() pass elif event is Event.NO_ACTION: self._log.info(Fore.BLACK + Style.DIM + 'NO_ACTION event: {};\tvalue: {:d}'.format( event.description, message.value)) elif event is Event.VIDEO: if message.value == 0: if self._video: if self._video.active: self._log.info( Fore.GREEN + Style.DIM + 'STOP VIDEO event: {};\tvalue: {:d}'.format( event.description, message.value)) self._enable_video(False) else: self._log.info( Fore.GREEN + Style.DIM + 'START VIDEO event: {};\tvalue: {:d}'.format( event.description, message.value)) self._enable_video(True) # time.sleep(0.5) # debounce else: self._log.warning('video not available.') elif event is Event.EVENT_L2: if message.value == 0: self._log.info(Fore.GREEN + Style.DIM + 'BLOB_SENSOR event: {};\tvalue: {:d}'.format( event.description, message.value)) self._capture_blob() # time.sleep(0.5) # debounce elif event is Event.LIGHTS: if message.value == 0: self._enable_lights(not self._lights_on) elif event is Event.EVENT_R1: if message.value == 0: self._log.info( Fore.MAGENTA + 'EVENT_R1 event (cruise): {}'.format(event.description)) if self._cruise_behaviour is None: # create a new CruiseBehaviour self._cruise_behaviour = CruiseBehaviour( self._config, self._pid_motor_ctrl, self._ifs, Level.INFO) self._queue.add_consumer(self._cruise_behaviour) self._cruise_behaviour.enable() self._log.info(Fore.MAGENTA + 'enabled cruise.') else: self._cruise_behaviour.disable() # self._cruise_behaviour.close() self._cruise_behaviour = None self._log.info(Fore.MAGENTA + 'disabled cruise.') else: self._log.debug(Fore.RED + 'other event: {}'.format(event.description)) pass self._start_time = dt.datetime.now()
def act(self, message, callback): ''' Responds to the Event contained within the Message. The callback method's API is: callback(self._current_message, _current_power_levels) ''' if not self._enabled: self._log.warning('action ignored: controller disabled.') return _start_time = dt.datetime.now() self._current_message = message # _port_speed = self._motors.get_current_power_level(Orientation.PORT) # _stbd_speed = self._motors.get_current_power_level(Orientation.STBD) # if _port_speed is not None and _stbd_speed is not None: # self._log.debug('last set power port: {:6.1f}, starboard: {:6.1f}'.format(_port_speed, _stbd_speed)) # else: # self._log.debug('last set power port: {}, starboard: {}'.format(_port_speed, _stbd_speed)) _event = self._current_message.get_event() self._log.debug(Fore.CYAN + 'act()' + Style.BRIGHT + ' event: {}.'.format(_event) + Fore.YELLOW) self._current_message.start() # no action ............................................................ if _event is Event.NO_ACTION: self._log.info('event: no action.') pass # BUTTON .............................................. # button ............................................................... elif _event is Event.BUTTON: # if button pressed we change standby mode but don't do anything else _value = self._current_message.get_value() # toggle value # self.set_standby(not _value) if _value: self._log.critical('event: button value: {}.'.format(_value)) self.set_standby(False) else: self._log.error('event: button value: {}.'.format(_value)) self.set_standby(True) # if in standby mode we don't process the event, but we do perform the callback # # stopping and halting ................................................... # SHUTDOWN .............................................. # shutdown ............................................................ elif _event is Event.SHUTDOWN: self._log.info('event: shutdown.') self._motors.stop() self._callback_shutdown() # STOP .............................................. # stop ................................................................. elif _event is Event.STOP: self._log.info('event: stop.') self._motors.stop() # HALT .............................................. # halt ................................................................. elif _event is Event.HALT: self._log.info('event: halt.') self._motors.halt() # BRAKE ................................... # brake ................................................................ elif _event is Event.BRAKE: self._log.info('event: brake.') self._motors.brake() # STANDBY .............................................. # standby .............................................................. elif _event is Event.STANDBY: # toggle standby state _value = self._current_message.get_value() if _value == 1: if self._standby: self.set_standby(False) else: self.set_standby(True) else: pass # ROAM ................................................. elif _event is Event.ROAM: self._log.info('event: roam.') self._roam_behaviour.roam() # SNIFF ................................................. elif _event is Event.SNIFF: self._log.info('event: sniff.') self._behaviours.sniff() time.sleep(0.5) # debounce gamepad # D-PAD HORIZONTAL ................................................. elif _event is Event.FORWARD_VELOCITY: _direction = message.get_value() _speed = 80.0 if _direction == -1: # then ahead self._log.info('event: d-pad movement: ahead.') # self._motors.change_velocity(0.5, 0.5, SlewRate.SLOW, -1) self._motors.ahead(_speed) time.sleep(2.0) elif _direction == 1: # then astern self._log.info('event: d-pad movement: astern.') # self._motors.change_velocity(-0.5, -0.5, SlewRate.SLOW, -1) self._motors.astern(_speed) time.sleep(2.0) else: self._log.info('event: d-pad movement: halt.') # D-PAD VERTICAL ................................................. elif _event is Event.THETA: _direction = message.get_value() if _direction == -1: # then ahead self._log.info('event: d-pad rotate: counter-clockwise.') self._motors.step(-100.0, 100.0, -1, -1) time.sleep(2.0) elif _direction == 1: # then astern self._log.info('event: d-pad rotate: clockwise.') self._motors.step(100.0, -100.0, -1, -1) time.sleep(2.0) else: self._log.info('event: d-pad rotate: none.') # PORT_VELOCITY ................................................ elif _event is Event.PORT_VELOCITY: _velocity = Gamepad.convert_range(message.get_value()) self._log.info( Fore.GREEN + Style.BRIGHT + '{};\tvalue: {:>5.2f}'.format(_event.description, _velocity)) self._port_motor.set_motor_power(_velocity) # PORT_THETA ................................................... elif _event is Event.PORT_THETA: _velocity = -1 * Gamepad.convert_range(message.get_value()) self._log.info( Fore.MAGENTA + Style.BRIGHT + '{};\tvalue: {:>5.2f}'.format(_event.description, _velocity)) # STBD_VELOCITY ................................................ elif _event is Event.STBD_VELOCITY: _velocity = Gamepad.convert_range(message.get_value()) self._log.info( Fore.GREEN + Style.BRIGHT + '{};\tvalue: {:>5.2f}'.format(_event.description, _velocity)) self._stbd_motor.set_motor_power(_velocity) # STBD_THETA ................................................... elif _event is Event.STBD_THETA: _velocity = -1 * Gamepad.convert_range(message.get_value()) self._log.info( Fore.MAGENTA + Style.BRIGHT + '{};\tvalue: {:>5.2f}'.format(_event.description, _velocity)) # # bumper ............................................................. # BUMPER_PORT .............................................. elif _event is Event.BUMPER_PORT: self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED + ' port bumper.' + Style.RESET_ALL) if self._motors.is_in_motion(): # if we're moving then halt self._ifs.disable() self._motors.stop() # self._motors.astern(Speed.HALF.value) self._motors.turn_astern(Speed.THREE_QUARTER.value, Speed.HALF.value) time.sleep(0.5) self._motors.brake() self._ifs.enable() self._log.info('action complete: port bumper.') else: self._log.info('no action required (not moving): port bumper.') # BUMPER_CNTR .............................................. elif _event is Event.BUMPER_CNTR: self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE + ' center bumper.' + Style.RESET_ALL) if self._motors.is_in_motion(): # if we're moving then halt self._ifs.disable() self._motors.stop() self._motors.astern(Speed.HALF.value) time.sleep(0.5) self._motors.brake() self._ifs.enable() self._log.info('action complete: center bumper.') else: self._log.info( 'no action required (not moving): center bumper.') # BUMPER_STBD .............................................. elif _event is Event.BUMPER_STBD: self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.GREEN + ' starboard bumper.' + Style.RESET_ALL) if self._motors.is_in_motion(): # if we're moving then halt self._motors.stop() # self._motors.astern(Speed.FULL.value) self._motors.turn_astern(Speed.HALF.value, Speed.THREE_QUARTER.value) time.sleep(0.5) self._motors.brake() self._log.info('action complete: starboard bumper.') else: self._log.info( 'no action required (not moving): starboard bumper.') # # infrared ........................................................... # INFRARED_PORT_SIDE .............................................. elif _event is Event.INFRARED_PORT_SIDE: _value = message.get_value() self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED + ' port side infrared; ' + Fore.YELLOW + 'value: {}'.format(_value)) pass # INFRARED_PORT .............................................. elif _event is Event.INFRARED_PORT: _value = message.get_value() self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED + ' port infrared; ' + Fore.YELLOW + 'value: {}'.format(_value)) if self._motors.is_in_motion(): # if we're moving then avoid self._avoid(Orientation.PORT) self._log.info('action complete: port infrared.') else: self._log.info( 'no action required (not moving): port infrared.') pass # INFRARED_CNTR .............................................. elif _event is Event.INFRARED_CNTR: _value = message.get_value() self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE + ' center infrared; ' + Fore.YELLOW + 'value: {}'.format(_value)) if self._motors.is_in_motion(): # if we're moving then avoid self._avoid(Orientation.CNTR) self._log.info('action complete: center infrared.') else: self._log.info( 'no action required (not moving): center infrared.') pass # INFRARED_STBD ................................................... elif _event is Event.INFRARED_STBD: _value = message.get_value() self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.GREEN + ' starboard infrared; ' + Fore.YELLOW + 'value: {}'.format(_value)) if self._motors.is_in_motion(): # if we're moving then avoid self._avoid(Orientation.STBD) self._log.info('action complete: starboard infrared.') else: self._log.info( 'no action required (not moving): starboard infrared.') pass # INFRARED_STBD_SIDE ........................................... elif _event is Event.INFRARED_STBD_SIDE: _value = message.get_value() self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.GREEN + ' starboard side infrared; ' + Fore.YELLOW + 'value: {}'.format(_value)) pass # EVENT UNKNOWN: FAILURE ............................................... else: self._log.error('unrecognised event: {}'.format(_event)) pass _current_power_levels = self._motors.get_current_power_levels() if callback is not None: self._log.debug( Fore.MAGENTA + 'callback message: {}; '.format( self._current_message.get_value()) + Fore.CYAN + 'current power levels: {}'.format(_current_power_levels)) callback(self._current_message, _current_power_levels) self._clear_current_message() _delta = dt.datetime.now() - _start_time _elapsed_ms = int(_delta.total_seconds() * 1000) self._log.debug(Fore.MAGENTA + Style.DIM + 'elapsed: {}ms'.format(_elapsed_ms) + Style.DIM)