def main(): _log = Logger("logger", Level.INFO, log_to_file=True) for i in range(10): _log.file("log message {}".format(i)) print("log message {}".format(i))
class GamepadController(): ''' This is a controller for the Gamepad, and also acts as a message consumer by implementing the queue.add(Message) method. :param config: the application configuration :param queue: the message queue :param pid_motor_ctrl: the PID motor controller :param ifs: the integrated front sensor (optional) :param video: the video capture (optional) :param blob: the blob detector (optional) :param matrix11x7_available: a flag indicating the 11x7 LED matrix display is available, determining if the 'light' feature is available. :param level: the log level :param close_callback: a callback executed upon closing the controller ''' def __init__(self, config, queue, pid_motor_ctrl, ifs, video, blob, matrix11x7_available, level, close_callback): super().__init__() if config is None: raise ValueError('null configuration argument.') self._log = Logger("gp-ctrl", level) self._config = config self._queue = queue _config = config['ros'].get('gamepad_demo').get('controller') self._log_to_file = _config.get('log_to_file') self._log_to_console = _config.get('log_to_console') self._min_loop_ms = _config.get( 'min_loop_time_ms') # minimum gamepad loop time (ms) self._hysteresis_limit = 3.0 self._pid_motor_ctrl = pid_motor_ctrl self._motors = self._pid_motor_ctrl.get_motors() _controllers = pid_motor_ctrl.get_pid_controllers() self._port_pid = _controllers[0] self._stbd_pid = _controllers[1] self._ifs = ifs self._video = video self._blob = blob # self._behaviours = Behaviours(config, self._pid_motor_ctrl, level) if matrix11x7_available: self._port_light = Matrix(Orientation.PORT, Level.INFO) self._stbd_light = Matrix(Orientation.STBD, Level.INFO) else: self._port_light = None self._stbd_light = None self._close_callback = close_callback # get ready... self._counter = itertools.count() self._cruise_behaviour = None self._monitor_thread = None self._pid_enabled = False self._enabled = False self._lights_on = False self._queue.add_consumer(self) self._start_time = dt.datetime.now() self._log.info('ready.') # .......................................................................... def enable(self): self._enabled = True # .......................................................................... def disable(self): self._enabled = False # self._behaviours.disable() # .......................................................................... def _close(self): self.disable() self._close_callback() # .......................................................................... @property def enabled(self): return self._enabled # .......................................................................... def set_led_color(self, color): self._log.info('set LED color to {}.'.format(color)) self._motors.set_led_show_battery(False) self._motors.set_led_color(color) # .......................................................................... def set_led_show_battery(self, enabled): self._log.info('set LED show battery to {}.'.format(enabled)) self._motors.set_led_show_battery(enabled) # .......................................................................... def _enable_video(self, enabled): if self._video is not None: if enabled: self._log.info(Fore.GREEN + Style.BRIGHT + 'START VIDEO') self._video.start() self.set_led_color( Color.BLACK) # so it doesn't show up on video else: self._log.info(Fore.GREEN + Style.BRIGHT + 'STOP VIDEO') if self._video is not None: self._video.stop() # self.set_led_color(Color.DARK_GREEN) self.set_led_show_battery(True) else: self._log.warning('video is not available.') # .......................................................................... def _capture_blob(self): if self._blog: self._log.info('capture blob image.') # self.set_led_color(Color.BLACK) # so it doesn't show up on video self._blob.capture() # time.sleep(0.5) # self.set_led_show_battery(True) self._log.info('blob capture complete.') else: self._log.info('blob capture not enabled.') # .......................................................................... def _enable_lights(self, enabled): if self._port_light and self._stbd_light: if enabled: self._port_light.light() self._stbd_light.light() else: self._port_light.clear() self._stbd_light.clear() self._lights_on = enabled # .......................................................................... def _enable_pid(self, enabled): if self._port_pid.enabled: self._pid_enabled = False self._port_pid.disable() self._stbd_pid.disable() else: if self._monitor_thread is None: self._pid_enabled = True if self._log_to_file or self._log_to_console: # if logging to file and/or console start monitor thread raise Exception('monitor unsupported.') self._monitor_thread = threading.Thread( name='gamepad-ctrl', target=GamepadController._monitor, args=[self, lambda: self._pid_enabled], daemon=True) self._monitor_thread.start() self._port_pid.enable() self._stbd_pid.enable() # ...................................................... def add(self, message): if not self._enabled: return message.number = next(self._counter) event = message.event if event is Event.CLOCK_TICK: # self._log.debug('TICK RECEIVED: {};\tcount: {:5.2f}'.format(event.description, message.value)) _priority_message = self._queue.next() if _priority_message and not _priority_message.event.is_ignoreable: self.handle_message(_priority_message) elif event is Event.CLOCK_TOCK: # self._log.debug(Fore.YELLOW + Style.NORMAL + 'TOCK RECEIVED: {};\tcount: {:5.2f}'.format(event.description, message.value)) pass else: # self._log.debug(Fore.WHITE + Style.DIM + 'message RECEIVED with event type: {};\tcount: {:5.2f}'.format(event.description, message.value)) pass # ...................................................... def _hysteresis(self, value): ''' If the value is less than the hysteresis limit returns zero, otherwise the argument. This assumes the midpoint of the 0-255 range is 127.0. ''' # return value return 127.0 if abs(value - 127.0) < self._hysteresis_limit else value # ...................................................... 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 _monitor(self, f_is_enabled): ''' A 20Hz loop that prints PID statistics to the log while enabled. Note that this loop is not synchronised with the two PID controllers, which each have their own loop. ''' _indent = ' ' self._log.info('PID monitor header:\n' \ + _indent + 'name : description\n' \ + _indent + 'kp : proportional constant\n' \ + _indent + 'ki : integral constant\n' \ + _indent + 'kd : derivative constant\n' \ + _indent + 'p_cp : port proportial value\n' \ + _indent + 'p_ci : port integral value\n' \ + _indent + 'p_cd : port derivative value\n' \ + _indent + 'p_lpw : port last power\n' \ + _indent + 'p_cpw : port current motor power\n' \ + _indent + 'p_spwr : port set power\n' \ + _indent + 'p_cvel : port current velocity\n' \ + _indent + 'p_stpt : port velocity setpoint\n' \ + _indent + 's_cp : starboard proportial value\n' \ + _indent + 's_ci : starboard integral value\n' \ + _indent + 's_cd : starboard derivative value\n' \ + _indent + 's_lpw : starboard proportional value\n' \ + _indent + 's_cpw : starboard integral value\n' \ + _indent + 's_spw : starboard derivative value\n' \ + _indent + 's_cvel : starboard current velocity\n' \ + _indent + 's_stpt : starboard velocity setpoint\n' \ + _indent + 'p_stps : port encoder steps\n' \ + _indent + 's_stps : starboard encoder steps') if self._log_to_file: self._file_log = Logger("pid", Level.INFO, log_to_file=True) # write header self._file_log.file( "kp|ki|kd|p_cp|p_ci|p_cd|p_lpw|p_cpw|p_spwr|p_cvel|p_stpt|s_cp|s_ci|s_cd|s_lpw|s_cpw|s_spw|s_cvel|s_stpt|p_stps|s_stps|" ) self._log.info(Fore.GREEN + 'starting PID monitor...') _rate = Rate(20) while f_is_enabled(): kp, ki, kd, p_cp, p_ci, p_cd, p_last_power, p_current_motor_power, p_power, p_current_velocity, p_setpoint, p_steps = self._port_pid.stats _x, _y, _z, s_cp, s_ci, s_cd, s_last_power, s_current_motor_power, s_power, s_current_velocity, s_setpoint, s_steps = self._stbd_pid.stats _msg = ('{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:>5.2f}|{:d}|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:>5.2f}|{:d}|').format(\ kp, ki, kd, p_cp, p_ci, p_cd, p_last_power, p_current_motor_power, p_power, p_current_velocity, p_setpoint, p_steps, \ s_cp, s_ci, s_cd, s_last_power, s_current_motor_power, s_power, s_current_velocity, s_setpoint, s_steps) _p_hilite = Style.BRIGHT if p_power > 0.0 else Style.NORMAL _s_hilite = Style.BRIGHT if s_power > 0.0 else Style.NORMAL _msg2 = (Fore.RED+_p_hilite+'{:7.4f}|{:7.4f}|{:7.4f}|{:<5.2f}|{:<5.2f}|{:<5.2f}|{:>5.2f}|{:d}'+Fore.GREEN+_s_hilite+'|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:<5.2f}|{:d}|').format(\ p_cp, p_ci, p_cd, p_last_power, p_power, p_current_velocity, p_setpoint, p_steps, \ s_cp, s_ci, s_cd, s_last_power, s_power, s_current_velocity, s_setpoint, s_steps) _rate.wait() if self._log_to_file: self._file_log.file(_msg) if self._log_to_console: self._log.info(_msg2) self._log.info('PID monitor stopped.')