def __init__(self, opts, host, port=None, osd_logsaver=None, json_logsaver=None, test_sim_keys=None): self.opts = opts self.logger = logging.getLogger('control') self.osd_logsaver = osd_logsaver self.json_logsaver = json_logsaver self.asyncio_loop = asyncio.get_event_loop() self.addr = (host, port or self.DEFAULT_PORT) self.sock = socket.socket( socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) self.c_cal = calibration.CameraCalibration() # TODO mafanasyev: load yaml here? Does it make sense if we do # not support distortion? self.sock.setblocking(0) self.logger.info('Connecting to %s:%d' % self.addr) self.sock.connect(self.addr) # Wallclock and pipeline time self.last_video_wall_time = None self.last_video_pp_time = None self.video_extra_stats = dict() self.control_dict = dict(self._INITIAL_CONTROL_DICT) # If True, some gait-related key is being held. self.key_gait_active = False # state log file and last svg self.state_log_file = None self.last_svg_name = None self.state_savefile_name = None if opts.log_prefix: # Last svg needs no timestamp self.last_svg_name = os.path.join( os.path.dirname(opts.log_prefix + 'test'), 'last.svg') # Open file line-buffered self.state_log_file = open( opts.log_prefix + '.jsonlist', 'w', 1) # Prepare savefile name self.state_savefile_name = os.path.join( os.path.dirname(opts.log_prefix + 'test'), self.UI_STATE_SAVEFILE) # Log startup events self._emit_json_logsaver_data() # Install handler to log future events (from the right thread) self.json_logsaver.on_record.append( lambda: self.asyncio_loop.call_soon_threadsafe( self._emit_json_logsaver_data)) self.osd = osd.OnScreenDisplay(self.c_cal) # Control UI state. Should be a simple dict, as we will be serializing # it for later log replay. self.ui_state = dict(self._INITIAL_UI_STATE) self._logged_state = None # timestamp of last received remote log message # (set to 0 or None to disable received logs) self.remote_logs_from = 1 # Most recent server state self.server_state = dict() self.server_time_offset = None self.fire_cmd_seq = 0 restore_from = opts.restore_state # default -- only restore if file exists if (restore_from is None and self.state_savefile_name is not None and os.path.exists(self.state_savefile_name)): restore_from = self.state_savefile_name if restore_from: # not empty string or None # Restore state logging.info('Loading saved state from %r', restore_from) with open(restore_from, 'r') as fh: restored = json.load(fh) self.ui_state.update(restored) # Always start with autofire disabled. self.ui_state['autofire_mode'] = 0 self.video = None self._mouse_click_info = None self._update_video_overlay_pending = False # Log state before video, to check for code errors in rendering function self._state_updated() if not opts.external_video: video_log = None if opts.log_prefix: video_log = opts.log_prefix + '.mkv' self.video = VideoWindow(host, self.VIDEO_PORT, video_log=video_log) self.video.on_video_mouse_click = self._handle_video_mouse_click self.video.on_video_mouse_move = self._handle_video_mouse_move self.video.on_video_mouse_release = self._handle_video_mouse_release self.video.on_key_press = self._handle_key_press self.video.on_key_release = self._handle_key_release self.video.on_got_video = functools.partial( self._state_updated, force=True) self.video.on_get_extra_stats = self._get_video_extra_stats def metric(device): abs_axis = device.get_features(linux_input.EV.ABS) if len(abs_axis) >= 3: return 1 return -1 enumerator = joystick.JoystickEnumerator(metric) joysticks = enumerator.joysticks() if len(joysticks): self.joystick = joysticks[0] CriticalTask(self._read_joystick()) else: self.joystick = None self.logger.info("No joysticks found!") GLib.timeout_add(int(self.SEND_INTERVAL * 1000), self._on_send_timer) GLib.io_add_watch(self.sock.fileno(), GLib.PRIORITY_DEFAULT, GLib.IO_IN | GLib.IO_ERR | GLib.IO_HUP, self._on_readable) # Refresh overlay when new logs are arriving if self.osd_logsaver: self.osd_logsaver.on_record.append( lambda *_: self.update_video_overlay()) if test_sim_keys is not None: # Exit once replay completes CriticalTask(self._replay_test_data(test_sim_keys))
class ControlInterface(object): SEND_INTERVAL = 0.25 DEFAULT_PORT = 13356 VIDEO_PORT = 13357 UI_STATE_SAVEFILE = "last.jsonlist" _INITIAL_UI_STATE = dict( # type marker used to recognize records in logfile. Do not change. _type = 'ui-state', # SVG is stretched to video after rendering, so small width/height # will make all lines thicker. Force to video resolution: 1920x1080 image_size=(1920, 1080), reticle_mode=1, reticle_offset=(0, 0), reticle_rotate=0, status_on=True, msg_font_size=20, # fire command duration (seconds) fire_duration = 0.5, # Autofire mode. This is initialized in the constructor, and saved value # is always ignored. autofire_mode = 0, speed = 150, ) _INITIAL_CONTROL_DICT = dict( # type marker used to recognize records in logfile. Do not change. _type = 'control-dict', boot_time = time.time(), # Incremented on each packet TX seq = 0, # Set before each packet TX cli_time = None, video_port=VIDEO_PORT, # Not set: turret - pair of (x, y) degrees # Not set: gait - a dict based off IDLE/RIPPLE_COMMAND # 0 = off; 1 = on laser_on = 0, green_led_on = 0, # 0 = off, 1 = auto, 2 = force on agitator_mode = 1, # pwm values in 0..1 range agitator_pwm = 0.5, fire_motor_pwm = 0.75, # fire command duration (seconds) -- copied from ui_state # (as records in this struct are not saved) fire_duration = None, # fire control. # The fire control must be: # (1) idempotent -- we can send control packet for any reason # (2) resilent to packet losses # (3) time limited -- network delays should not cause delayed fire. # Firing command deadline. If this is zero or less than server's current # time, all new firing is inhibited (current shot is not interrupted) fire_cmd_deadline = 0, # Fire command. This is a tuple (command, seq). A new command starts # every time a tuple changes. 'command' is one of the FCMD constants from # vui_helpers. 'seq' must be an interger which increases by one every time # new command appears. fire_cmd = None, ) def __init__(self, opts, host, port=None, osd_logsaver=None, json_logsaver=None, test_sim_keys=None): self.opts = opts self.logger = logging.getLogger('control') self.osd_logsaver = osd_logsaver self.json_logsaver = json_logsaver self.asyncio_loop = asyncio.get_event_loop() self.addr = (host, port or self.DEFAULT_PORT) self.sock = socket.socket( socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) self.c_cal = calibration.CameraCalibration() # TODO mafanasyev: load yaml here? Does it make sense if we do # not support distortion? self.sock.setblocking(0) self.logger.info('Connecting to %s:%d' % self.addr) self.sock.connect(self.addr) # Wallclock and pipeline time self.last_video_wall_time = None self.last_video_pp_time = None self.video_extra_stats = dict() self.control_dict = dict(self._INITIAL_CONTROL_DICT) # If True, some gait-related key is being held. self.key_gait_active = False # state log file and last svg self.state_log_file = None self.last_svg_name = None self.state_savefile_name = None if opts.log_prefix: # Last svg needs no timestamp self.last_svg_name = os.path.join( os.path.dirname(opts.log_prefix + 'test'), 'last.svg') # Open file line-buffered self.state_log_file = open( opts.log_prefix + '.jsonlist', 'w', 1) # Prepare savefile name self.state_savefile_name = os.path.join( os.path.dirname(opts.log_prefix + 'test'), self.UI_STATE_SAVEFILE) # Log startup events self._emit_json_logsaver_data() # Install handler to log future events (from the right thread) self.json_logsaver.on_record.append( lambda: self.asyncio_loop.call_soon_threadsafe( self._emit_json_logsaver_data)) self.osd = osd.OnScreenDisplay(self.c_cal) # Control UI state. Should be a simple dict, as we will be serializing # it for later log replay. self.ui_state = dict(self._INITIAL_UI_STATE) self._logged_state = None # timestamp of last received remote log message # (set to 0 or None to disable received logs) self.remote_logs_from = 1 # Most recent server state self.server_state = dict() self.server_time_offset = None self.fire_cmd_seq = 0 restore_from = opts.restore_state # default -- only restore if file exists if (restore_from is None and self.state_savefile_name is not None and os.path.exists(self.state_savefile_name)): restore_from = self.state_savefile_name if restore_from: # not empty string or None # Restore state logging.info('Loading saved state from %r', restore_from) with open(restore_from, 'r') as fh: restored = json.load(fh) self.ui_state.update(restored) # Always start with autofire disabled. self.ui_state['autofire_mode'] = 0 self.video = None self._mouse_click_info = None self._update_video_overlay_pending = False # Log state before video, to check for code errors in rendering function self._state_updated() if not opts.external_video: video_log = None if opts.log_prefix: video_log = opts.log_prefix + '.mkv' self.video = VideoWindow(host, self.VIDEO_PORT, video_log=video_log) self.video.on_video_mouse_click = self._handle_video_mouse_click self.video.on_video_mouse_move = self._handle_video_mouse_move self.video.on_video_mouse_release = self._handle_video_mouse_release self.video.on_key_press = self._handle_key_press self.video.on_key_release = self._handle_key_release self.video.on_got_video = functools.partial( self._state_updated, force=True) self.video.on_get_extra_stats = self._get_video_extra_stats def metric(device): abs_axis = device.get_features(linux_input.EV.ABS) if len(abs_axis) >= 3: return 1 return -1 enumerator = joystick.JoystickEnumerator(metric) joysticks = enumerator.joysticks() if len(joysticks): self.joystick = joysticks[0] CriticalTask(self._read_joystick()) else: self.joystick = None self.logger.info("No joysticks found!") GLib.timeout_add(int(self.SEND_INTERVAL * 1000), self._on_send_timer) GLib.io_add_watch(self.sock.fileno(), GLib.PRIORITY_DEFAULT, GLib.IO_IN | GLib.IO_ERR | GLib.IO_HUP, self._on_readable) # Refresh overlay when new logs are arriving if self.osd_logsaver: self.osd_logsaver.on_record.append( lambda *_: self.update_video_overlay()) if test_sim_keys is not None: # Exit once replay completes CriticalTask(self._replay_test_data(test_sim_keys)) def select_axes(self): complete = self.joystick.get_features(linux_input.EV.ABS) print "Joystick available axes:", complete # If we have all of X, Y, and RX, RY, then the axes are # probably mapped correctly and we can return what we expect. if (linux_input.ABS.X in complete and linux_input.ABS.Y in complete and linux_input.ABS.RX in complete and linux_input.ABS.RY in complete): return [linux_input.ABS.X, linux_input.ABS.Y, linux_input.ABS.RX, linux_inpu.ABS.RY] # Otherwise, just return the first axes on the hope that # this is meaningful. return complete[0:4] @wrap_event def _read_joystick(self): axes = self.select_axes() print "Selected joystick axes:", axes _DEADBAND = 0.2 def deadband(x): if abs(x) < _DEADBAND: return 0.0 if x > 0.0: return (x - _DEADBAND) / (1.0 - _DEADBAND) if x < 0.0: return (x + _DEADBAND) / (1.0 - _DEADBAND) while True: ev = yield From(self.joystick.read()) if ev.ev_type != linux_input.EV.ABS: continue dx, dy, dr, dz = [ deadband(self.joystick.absinfo(axes[x]).scaled()) for x in range(4)] if abs(dx) == 0.0 and abs(dy) == 0.0 and abs(dr) == 0.0: if self.control_dict.get('gait') is not None: self.control_dict['gait'] = IDLE_COMMAND else: gait = RIPPLE_COMMAND.copy() speed = self.ui_state['speed'] gait['translate_x_mm_s'] = dx * 0.7 * speed gait['translate_y_mm_s'] = -dy * 1.0 * speed gait['rotate_deg_s'] = dr * 0.5 * speed self.control_dict['gait'] = gait if 'gait' in self.control_dict: z_mapping = 0.0 if dz < 0.0: z_mapping = -dz * self._MAX_Z_VALUE else: z_mapping = dz * self._MIN_Z_VALUE self.control_dict['gait']['body_z_mm'] = z_mapping @wrap_event def _on_send_timer(self): self._send_control() if self.video: tm_pp = self.video.get_time() tm_wc = time.time() if self.last_video_pp_time is not None: dt_pp = tm_pp - self.last_video_pp_time dt_wc = tm_wc - self.last_video_wall_time lost = (dt_wc - dt_pp) if abs(lost) > 0.01: self.logger.info('Video time lost: %.3f msec (%.1f%%)' % ( lost * 1000.0, lost * 100.0 / (dt_wc or 1))) self.last_video_wall_time = tm_wc self.last_video_pp_time = tm_pp return True @wrap_event def _on_readable(self, source, cond): while True: try: pkt, addr = self.sock.recvfrom(65535) except socket.error as e: if e.errno != errno.EAGAIN: self.logger.error('Cannot read response: %s', e) break assert addr == self.addr, \ 'Got packet from %r, expected from %r' % (addr, self.addr) self._handle_packet(pkt) return True def _send_control(self): self.control_dict['seq'] += 1 self.control_dict.update( logs_from=self.remote_logs_from, cli_time=time.time()) serialized = self._log_struct(self.control_dict) self.sock.send(serialized) if self.control_dict['seq'] == 3 and self.video: self.video.start() def _handle_packet(self, pkt_raw): # Get the structure pkt = json.loads(pkt_raw) # Prepare to log pkt.update(cli_time=time.time(), _type='srv-state') if self.video: # Add locally derived values pkt['cli_pts'] = self.video.get_video_pts() if 'srv_pts' in pkt and (pkt.get('cli_pts') is not None): # Video latency latency = pkt['srv_pts'] - pkt['cli_pts'] pkt['latency_video'] = latency # Store min and max control latency for stats output self.video_extra_stats['lat_video'] = max( self.video_extra_stats.get('lat_video', -1), latency) self.video_extra_stats['lat_video_min'] = min( self.video_extra_stats.get('lat_video_min', 1e12), latency) if pkt['est_cli_time']: # Control link latency latency = pkt['cli_time'] - pkt['est_cli_time'] pkt['latency_ctrl'] = latency # Store max control latency for stats output self.video_extra_stats['lat_ctrl'] = max( self.video_extra_stats.get('lat_ctrl', -1), latency) server_time_offset = pkt['srv_time'] - pkt['cli_time'] if (self.server_time_offset is None) or \ abs(self.server_time_offset - server_time_offset) > 1.0: self.logger.debug('Server time offset %.3f sec', server_time_offset) self.server_time_offset = server_time_offset # Parse out messages -- no need to log them twice logs = pkt.pop('logs_data', None) or [] for entry in logs: if entry[0] > self.remote_logs_from: # new record self.remote_logs_from = entry[0] log_dict = MemoryLoggingHandler.to_dict( entry, time_field='srv_time') log_dict.update( _type='srv-log', cli_time=pkt['cli_time']) self._log_struct(log_dict) MemoryLoggingHandler.relog(entry, prefix='srv.') # Log it and store (for display) self._log_struct(pkt) self.server_state = pkt self.update_video_overlay() def _handle_video_mouse_move(self, pos, orig_button): """User is moving mouse in the window while the button is down (for performance, these events are not sent when button is up) @p pos - current mouse position @p orig_button - button from the click event This function is rate-limited. """ if not self._mouse_click_info: self.logger.debug('Ignoring spurious mouse move') return # How much we moved (in screen coordinates) -- so the range is # (-0.5..0.5) for each axis. delta_screen = add_pair(pos, self._mouse_click_info[0], -1.0) # Convert to new servo position by multiplying by arbitrary factor new_pos = add_pair(self._mouse_click_info[1], delta_screen, 25.0) if False: self.logger.debug( 'Moving turret to (%+.1f, %+.1f) deg in response ' 'to mouse move at (%+.4f, %+.4f)', new_pos[0], new_pos[1], delta_screen[0], delta_screen[1]) self.control_dict['turret'] = new_pos self._send_control() def _handle_video_mouse_click(self, pos, button): """User clicked button 1 on video window. pos is (x, y) tuple in 0..1 range""" assert len(pos) == 2 # This image_size will be broken if aspect ratio is different -- this # usually means truncated image, not resized pixels; but we do not care # for now. w2d, = self.c_cal.to_world2d([pos], image_size=(1.0, 1.0)) offset = self.ui_state['reticle_offset'] w2ang = ( math.degrees(math.atan(w2d[0])) - offset[0], math.degrees(math.atan(w2d[1])) - offset[1] ) self._mouse_click_info = None fire_cmd = None if (button == 3) and self.ui_state['autofire_mode']: # autofire mode fire_cmd = self.ui_state['autofire_mode'] elif button != 1: self.logger.info('Click with B%d at (w2d %.3f,%.3f, move by ' '%.1f,%.1f deg)' % ((button, ) + w2d + w2ang)) return old_turret = self.control_dict.get('turret') if old_turret is None: self.logger.warn('Cannot move turret -- center it first ' '(w2d %.3f,%.3f, w2a %.1f,%.1f)' % (w2d + w2ang)) return ang_x, ang_y = old_turret ang_x += w2ang[0] ang_y += w2ang[1] # TODO mafanasyev: add reticle_rotate support self.logger.debug('Moving turret to (%+.1f, %+.1f) deg in response ' 'to click at (%+.4f, %+.4f)', ang_x, ang_y, w2d[0], w2d[1]) self.control_dict['turret'] = (ang_x, ang_y) # Remember turret position self._mouse_click_info = (pos, self.control_dict['turret']) if not fire_cmd: pass elif fire_cmd != FCMD.cont: self._prepare_fire_command(fire_cmd) else: # TODO mafanasyev: implement FCMD.cont self.logger.error('Fire command %d not implemented', fire_cmd) self._send_control() def _handle_video_mouse_release(self, button): """User has release mouse button in a video window""" pass _GAIT_KEYS = { # key->(dx,dy,dr) 'w': (0, 1, 0), 's': (0, -1, 0), 'a': (-1, 0, 0), 'd': (1, 0, 0), 'q': (0, 0, -1), 'e': (0, 0, 1) } _Z_KEYS = { # key->dz 'f': -10, 'r': 10, } _MIN_Z_VALUE = -25 _MAX_Z_VALUE = 20 _SPEED_KEYS = { # key->dspeed 'z': -10, 'x': 10, } _MIN_SPEED_VALUE = 10 _MAX_SPEED_VALUE = 220 _ARROW_KEYS = { # key->(dx, dy) 'Left': (-1, 0), 'Right': (1, 0), 'Up': (0, -1), 'Down': (0, 1) } _FIRE_COMMANDS = { '1': FCMD.inpos1, '2': FCMD.inpos2, '3': FCMD.inpos3, '5': FCMD.inpos5, '9': FCMD.cont, '0': FCMD.off } def _handle_key_press(self, base_name, modifiers): arrows = self._ARROW_KEYS.get(base_name) if arrows: base_name = 'Arrows' name = modifiers + base_name speed = self.ui_state['speed'] if name in self._GAIT_KEYS: dx, dy, dr = self._GAIT_KEYS[name] gait = RIPPLE_COMMAND.copy() gait['translate_x_mm_s'] = dx * 0.7 * speed gait['translate_y_mm_s'] = dy * 1.0 * speed gait['rotate_deg_s'] = dr * 0.3 * speed self.control_dict['gait'] = gait self.key_gait_active = True elif name in self._Z_KEYS: dz = self._Z_KEYS[name] self._add_z(dz) elif name in ['g']: change = self._MAX_Z_VALUE - self._MIN_Z_VALUE if RIPPLE_COMMAND['body_z_mm'] == self._MIN_Z_VALUE: self._add_z(change) else: self._add_z(-change) elif name in self._SPEED_KEYS: dspeed = self._SPEED_KEYS[name] self.ui_state['speed'] += dspeed self.ui_state['speed'] = max( self._MIN_SPEED_VALUE, min(self._MAX_SPEED_VALUE, self.ui_state['speed'])) elif name in ['h', 'S-question']: self._print_help() elif name == 'l': self.control_dict['laser_on'] ^= 1 self.logger.info('Laser set to %d', self.control_dict['laser_on']) elif name in ['m', 'S-M']: val = self.control_dict['agitator_mode'] if name == 'm' and val != 1: val, val_str = 1, 'auto' elif name == 'S-M' and val != 2: val, val_str = 2, 'force-on' else: val, val_str = 0, 'off' self.control_dict['agitator_mode'] = val self.logger.info('Agitator mode to %s(%d) (pwm %.3f)', val_str, val, self.control_dict['agitator_pwm']) elif name == 'S-G': self.control_dict['green_led_on'] ^= 1 self.logger.info('Green LED set to %d', self.control_dict['green_led_on']) elif name in ['Return']: self._prepare_fire_command(FCMD.now1) elif name == 'c': self.control_dict['turret'] = (0.0, 0.0) self.logger.info('Centered turret') elif name in ['C-Arrows', 'C-S-Arrows']: # Ctrl + arrows to move reticle center step = 1.0 if ('S-' in modifiers) else 0.25 self.ui_state['reticle_offset'] = add_pair( self.ui_state['reticle_offset'], arrows, step) self.logger.info('Reticle center offset: %.3f, %.3f' % self.ui_state['reticle_offset']) elif name == 'C-r': self.ui_state['reticle_offset'] = (0, 0) self.logger.info('Zeroed out reticle center offset') elif name in ['Arrows', 'S-Arrows']: # Move turret step = 5.0 if ('S-' in modifiers) else 0.5 if self.control_dict.get('turret') is None: self.logger.warn('Cannot move turret -- center it first') else: self.control_dict['turret'] = new = add_pair( self.control_dict['turret'], arrows, step) self.logger.debug('Moving turret to (%+.1f, %+.1f) deg ' 'in response to keys', new[0], new[1]) elif name == 'C-S-M-Arrows': # Method will do its own logging step = 0.25 self.c_cal.tweak_fixups(arrows[0] * step, arrows[1] * step) elif name == 't': newmode = (self.ui_state['reticle_mode'] + 1) % 3 self.ui_state['reticle_mode'] = newmode self.logger.info('Set reticle_mode=%r', newmode) elif name == 'KP_Add': self.ui_state['msg_font_size'] += 1 elif name == 'KP_Subtract': self.ui_state['msg_font_size'] = max( 4, self.ui_state['msg_font_size'] - 1) elif name in ['S-parenleft', 'S-parenright', 'S-asterisk']: newval = max( 0, self.ui_state['fire_duration'] + (-0.05 if name == 'S-parenleft' else 0.05 if name == 'S-parenright' else 0)) self.ui_state['fire_duration'] = newval self.logger.info('Fire duration set to %.2f sec', newval) elif name == 'Escape': if self.osd_logsaver and self.osd_logsaver.data: numlines = len(self.osd_logsaver.data) self.logger.info('Cleared on-screen display (%d lines)', numlines) self._log_event('cli-log-osd-cleared', numlines=numlines) del self.osd_logsaver.data[:] else: self.ui_state['status_on'] ^= True self.logger.debug('Set status_on=%r', self.ui_state['status_on']) elif name in self._FIRE_COMMANDS: self.ui_state['autofire_mode'] = newval = self._FIRE_COMMANDS[name] self.logger.info('Right-button autofire mode %d', newval) if newval == FCMD.off: # Halt firing immediately when autofire is disabled self.control_dict['fire_cmd'] = None elif base_name in ['Shift_R', 'Shift_L', 'Control_R', 'Control_L', 'Alt_R', 'Alt_L']: # Ignore modifiers pass else: self.logger.debug('Unknown key %r' % name) self._state_updated() self._send_control() def _handle_key_release(self, evt): if self.key_gait_active: self.key_gait_active = False if self.control_dict['gait'] is not None: self.control_dict['gait'] = IDLE_COMMAND self._send_control() def _add_z(self, dz): for command in [RIPPLE_COMMAND, IDLE_COMMAND]: command['body_z_mm'] += dz command['body_z_mm'] = max( self._MIN_Z_VALUE, min(self._MAX_Z_VALUE, command['body_z_mm'])) def _get_video_extra_stats(self): # Return extra lines to print in periodic video stats line rv = dict() for key, val in self.video_extra_stats.items(): if val != 0: rv[key] = '%.1fmS' % (val * 1000.0) self.video_extra_stats = dict() return rv def _print_help(self): helpmsg = """ Help on Keys: w/s, a/d - move q/e - rotate r/f - move up/down g - toggle between lowest and highest position z/x - go slower/speed up l - laser on/off m/M - toggle agitator auto/forced G - green LED on/off c - enable && center turret left click - point turret to this location (must center first) right click - print location coordinates, or autofire ENTER - fire arrows - move turret S+arrows - move turret (move faster) C+arrows - set reticle center (use shift for more precision) C+S+arrows - set reticle center (move slower) C+r - zero out reticle offset C+S+M+arrows - temporarily tweak camera calibration t - toggle reticle S-( S-) - change fire duration S-* - show fire duration ESC - clear logs from OSD; then toggle status visibility 1/2/3/5/9/0 - right-button autofire: 1-5 shots, continuous, off Numpad +/- - change OSD font size """ for line in textwrap.dedent(helpmsg).strip().splitlines(): self.logger.info('| ' + line) @asyncio.coroutine def _replay_test_data(self, data): self.logger.info('Simulating keypresses based on a command line') for word in data.split(' '): if word.startswith('@'): # Sleep interval = float(word[1:]) yield From(asyncio.sleep(interval)) else: # Keypress if '-' in word: modifiers, base_name = word.rsplit('-', 1) modifiers += '-' else: modifiers, base_name = '', word self.logger.info('Simulating key: (%r, %r)', modifiers, base_name) self._handle_key_press(base_name, modifiers) # Maintain spacing yield From(asyncio.sleep(0.5)) # Exit program self.logger.info('Replay complete, exiting') # We will exit on return since this is a CriticalTask def _log_struct(self, rec): """Log @p rec (which must be a json-serialized dict) to logfile. @p rec must contain fields '_type' and 'cli_time' @returns serialized string """ sertext = json.dumps(rec, sort_keys=True) if self.state_log_file: print >>self.state_log_file, sertext # No need to flush, file should have autoflush return sertext def _log_event(self, name, **kwargs): rec = { "_type": "event", "cli_time": time.time(), "name": name } rec.update(kwargs) self._log_struct(rec) def _emit_json_logsaver_data(self): while self.json_logsaver.data: entry = self.json_logsaver.data.pop(0) log_dict = MemoryLoggingHandler.to_dict( entry, time_field='cli_time') if log_dict["name"].startswith("srv."): # Do not double-save server messages return log_dict.update(_type='cli-log') self._log_struct(log_dict) def _state_updated(self, force=False): sertext = json.dumps(self.ui_state, sort_keys=True) if (sertext == self._logged_state) and not force: return self.ui_state['cli_time'] = time.time() sertext = self._log_struct(self.ui_state) self._logged_text = sertext if self.state_savefile_name is not None: with open(self.state_savefile_name + '~', 'w') as fh: print >>fh, sertext # Atomically install new version os.rename(self.state_savefile_name + '~', self.state_savefile_name) self.update_video_overlay() def update_video_overlay(self): """Request update of video overlay in thread-safe way""" if self._update_video_overlay_pending: # An update is pending return self._update_video_overlay_pending = True self.asyncio_loop.call_soon_threadsafe(self._update_video_overlay_now) def _prepare_fire_command(self, command): """Prepare fire command. You still need to send_control for it to take an effect """ if self.server_state.get('srv_time') is None: self.logger.error('Cannot fire -- no server time') return server_time = (self.server_state['srv_time'] - self.server_state['cli_time'] + time.time()) self.fire_cmd_seq += 1 timeout = NORMAL_FIRE_TIMEOUT if FCMD._is_inpos(command): # Give extra time for servoes to settle timeout += INPOS_EXTRA_FIRE_TIMEOUT self.control_dict.update( fire_duration = self.ui_state['fire_duration'], fire_cmd = [command, self.fire_cmd_seq], # Ignore command if it is delayed by >0.5 sec fire_cmd_deadline = server_time + timeout) self.logger.debug('Sent fire command %d', command) @wrap_event def _update_video_overlay_now(self): self._update_video_overlay_pending = False if self.osd_logsaver: logs = self.osd_logsaver.data else: logs = [] stream = StringIO.StringIO() self.osd.render_svg(stream, self.ui_state, self.control_dict, self.server_state, logs) if self.video: self.video.set_svg_overlay(stream.getvalue()) if self.last_svg_name: with open(self.last_svg_name + '~', 'w') as f: f.write(stream.getvalue()) os.rename(self.last_svg_name + '~', self.last_svg_name)
def __init__(self, opts, host, port=None, osd_logsaver=None, json_logsaver=None, test_sim_keys=None): self.opts = opts self.logger = logging.getLogger('control') self.osd_logsaver = osd_logsaver self.json_logsaver = json_logsaver self.asyncio_loop = asyncio.get_event_loop() self.addr = (host, port or self.DEFAULT_PORT) self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) self.c_cal = calibration.CameraCalibration() # TODO mafanasyev: load yaml here? Does it make sense if we do # not support distortion? self.sock.setblocking(0) self.logger.info('Connecting to %s:%d' % self.addr) self.sock.connect(self.addr) # Wallclock and pipeline time self.last_video_wall_time = None self.last_video_pp_time = None self.video_extra_stats = dict() self.control_dict = dict(self._INITIAL_CONTROL_DICT) # If True, some gait-related key is being held. self.key_gait_active = False # state log file and last svg self.state_log_file = None self.last_svg_name = None self.state_savefile_name = None if opts.log_prefix: # Last svg needs no timestamp self.last_svg_name = os.path.join( os.path.dirname(opts.log_prefix + 'test'), 'last.svg') # Open file line-buffered self.state_log_file = open(opts.log_prefix + '.jsonlist', 'w', 1) # Prepare savefile name self.state_savefile_name = os.path.join( os.path.dirname(opts.log_prefix + 'test'), self.UI_STATE_SAVEFILE) # Log startup events self._emit_json_logsaver_data() # Install handler to log future events (from the right thread) self.json_logsaver.on_record.append( lambda: self.asyncio_loop.call_soon_threadsafe( self._emit_json_logsaver_data)) self.osd = osd.OnScreenDisplay(self.c_cal) # Control UI state. Should be a simple dict, as we will be serializing # it for later log replay. self.ui_state = dict(self._INITIAL_UI_STATE) self._logged_state = None # timestamp of last received remote log message # (set to 0 or None to disable received logs) self.remote_logs_from = 1 # Most recent server state self.server_state = dict() self.server_time_offset = None self.fire_cmd_seq = 0 restore_from = opts.restore_state # default -- only restore if file exists if (restore_from is None and self.state_savefile_name is not None and os.path.exists(self.state_savefile_name)): restore_from = self.state_savefile_name if restore_from: # not empty string or None # Restore state logging.info('Loading saved state from %r', restore_from) with open(restore_from, 'r') as fh: restored = json.load(fh) self.ui_state.update(restored) # Always start with autofire disabled. self.ui_state['autofire_mode'] = 0 self.video = None self._mouse_click_info = None self._update_video_overlay_pending = False # Log state before video, to check for code errors in rendering function self._state_updated() if not opts.external_video: video_log = None if opts.log_prefix: video_log = opts.log_prefix + '.mkv' self.video = VideoWindow(host, self.VIDEO_PORT, video_log=video_log) self.video.on_video_mouse_click = self._handle_video_mouse_click self.video.on_video_mouse_move = self._handle_video_mouse_move self.video.on_video_mouse_release = self._handle_video_mouse_release self.video.on_key_press = self._handle_key_press self.video.on_key_release = self._handle_key_release self.video.on_got_video = functools.partial(self._state_updated, force=True) self.video.on_get_extra_stats = self._get_video_extra_stats def metric(device): abs_axis = device.get_features(linux_input.EV.ABS) if len(abs_axis) >= 3: return 1 return -1 enumerator = joystick.JoystickEnumerator(metric) joysticks = enumerator.joysticks() if len(joysticks): self.joystick = joysticks[0] CriticalTask(self._read_joystick()) else: self.joystick = None self.logger.info("No joysticks found!") GLib.timeout_add(int(self.SEND_INTERVAL * 1000), self._on_send_timer) GLib.io_add_watch(self.sock.fileno(), GLib.PRIORITY_DEFAULT, GLib.IO_IN | GLib.IO_ERR | GLib.IO_HUP, self._on_readable) # Refresh overlay when new logs are arriving if self.osd_logsaver: self.osd_logsaver.on_record.append( lambda *_: self.update_video_overlay()) if test_sim_keys is not None: # Exit once replay completes CriticalTask(self._replay_test_data(test_sim_keys))
from PyQt5.QtCore import Qt from PyQt5.QtWidgets import QApplication from PyQt5.QtGui import QPalette, QColor from video_window import VideoWindow if __name__ == '__main__': app = QApplication(sys.argv) app.setStyle('Fusion') palette = QPalette() palette.setColor(QPalette.Window, QColor(53,53,53)) palette.setColor(QPalette.WindowText, Qt.white) palette.setColor(QPalette.Base, QColor(15,15,15)) palette.setColor(QPalette.AlternateBase, QColor(53,53,53)) palette.setColor(QPalette.ToolTipBase, Qt.white) palette.setColor(QPalette.ToolTipText, Qt.white) palette.setColor(QPalette.Text, Qt.white) palette.setColor(QPalette.Button, QColor(53,53,53)) palette.setColor(QPalette.ButtonText, Qt.white) palette.setColor(QPalette.BrightText, Qt.red) palette.setColor(QPalette.Highlight, QColor(142,45,197).lighter()) palette.setColor(QPalette.HighlightedText, Qt.black) app.setPalette(palette) player = VideoWindow(app) player.resize(940, 480) player.show() sys.exit(app.exec_())
class ControlInterface(object): SEND_INTERVAL = 0.25 DEFAULT_PORT = 13356 VIDEO_PORT = 13357 UI_STATE_SAVEFILE = "last.jsonlist" _INITIAL_UI_STATE = dict( # type marker used to recognize records in logfile. Do not change. _type='ui-state', # SVG is stretched to video after rendering, so small width/height # will make all lines thicker. Force to video resolution: 1920x1080 image_size=(1920, 1080), reticle_mode=1, reticle_offset=(0, 0), reticle_rotate=0, status_on=True, msg_font_size=20, # fire command duration (seconds) fire_duration=0.5, # Autofire mode. This is initialized in the constructor, and saved value # is always ignored. autofire_mode=0, speed=150, ) _INITIAL_CONTROL_DICT = dict( # type marker used to recognize records in logfile. Do not change. _type='control-dict', boot_time=time.time(), # Incremented on each packet TX seq=0, # Set before each packet TX cli_time=None, video_port=VIDEO_PORT, # Not set: turret - pair of (x, y) degrees # Not set: gait - a dict based off IDLE/RIPPLE_COMMAND # 0 = off; 1 = on laser_on=0, green_led_on=0, # 0 = off, 1 = auto, 2 = force on agitator_mode=1, # pwm values in 0..1 range agitator_pwm=0.5, fire_motor_pwm=0.75, # fire command duration (seconds) -- copied from ui_state # (as records in this struct are not saved) fire_duration=None, # fire control. # The fire control must be: # (1) idempotent -- we can send control packet for any reason # (2) resilent to packet losses # (3) time limited -- network delays should not cause delayed fire. # Firing command deadline. If this is zero or less than server's current # time, all new firing is inhibited (current shot is not interrupted) fire_cmd_deadline=0, # Fire command. This is a tuple (command, seq). A new command starts # every time a tuple changes. 'command' is one of the FCMD constants from # vui_helpers. 'seq' must be an interger which increases by one every time # new command appears. fire_cmd=None, ) def __init__(self, opts, host, port=None, osd_logsaver=None, json_logsaver=None, test_sim_keys=None): self.opts = opts self.logger = logging.getLogger('control') self.osd_logsaver = osd_logsaver self.json_logsaver = json_logsaver self.asyncio_loop = asyncio.get_event_loop() self.addr = (host, port or self.DEFAULT_PORT) self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) self.c_cal = calibration.CameraCalibration() # TODO mafanasyev: load yaml here? Does it make sense if we do # not support distortion? self.sock.setblocking(0) self.logger.info('Connecting to %s:%d' % self.addr) self.sock.connect(self.addr) # Wallclock and pipeline time self.last_video_wall_time = None self.last_video_pp_time = None self.video_extra_stats = dict() self.control_dict = dict(self._INITIAL_CONTROL_DICT) # If True, some gait-related key is being held. self.key_gait_active = False # state log file and last svg self.state_log_file = None self.last_svg_name = None self.state_savefile_name = None if opts.log_prefix: # Last svg needs no timestamp self.last_svg_name = os.path.join( os.path.dirname(opts.log_prefix + 'test'), 'last.svg') # Open file line-buffered self.state_log_file = open(opts.log_prefix + '.jsonlist', 'w', 1) # Prepare savefile name self.state_savefile_name = os.path.join( os.path.dirname(opts.log_prefix + 'test'), self.UI_STATE_SAVEFILE) # Log startup events self._emit_json_logsaver_data() # Install handler to log future events (from the right thread) self.json_logsaver.on_record.append( lambda: self.asyncio_loop.call_soon_threadsafe( self._emit_json_logsaver_data)) self.osd = osd.OnScreenDisplay(self.c_cal) # Control UI state. Should be a simple dict, as we will be serializing # it for later log replay. self.ui_state = dict(self._INITIAL_UI_STATE) self._logged_state = None # timestamp of last received remote log message # (set to 0 or None to disable received logs) self.remote_logs_from = 1 # Most recent server state self.server_state = dict() self.server_time_offset = None self.fire_cmd_seq = 0 restore_from = opts.restore_state # default -- only restore if file exists if (restore_from is None and self.state_savefile_name is not None and os.path.exists(self.state_savefile_name)): restore_from = self.state_savefile_name if restore_from: # not empty string or None # Restore state logging.info('Loading saved state from %r', restore_from) with open(restore_from, 'r') as fh: restored = json.load(fh) self.ui_state.update(restored) # Always start with autofire disabled. self.ui_state['autofire_mode'] = 0 self.video = None self._mouse_click_info = None self._update_video_overlay_pending = False # Log state before video, to check for code errors in rendering function self._state_updated() if not opts.external_video: video_log = None if opts.log_prefix: video_log = opts.log_prefix + '.mkv' self.video = VideoWindow(host, self.VIDEO_PORT, video_log=video_log) self.video.on_video_mouse_click = self._handle_video_mouse_click self.video.on_video_mouse_move = self._handle_video_mouse_move self.video.on_video_mouse_release = self._handle_video_mouse_release self.video.on_key_press = self._handle_key_press self.video.on_key_release = self._handle_key_release self.video.on_got_video = functools.partial(self._state_updated, force=True) self.video.on_get_extra_stats = self._get_video_extra_stats def metric(device): abs_axis = device.get_features(linux_input.EV.ABS) if len(abs_axis) >= 3: return 1 return -1 enumerator = joystick.JoystickEnumerator(metric) joysticks = enumerator.joysticks() if len(joysticks): self.joystick = joysticks[0] CriticalTask(self._read_joystick()) else: self.joystick = None self.logger.info("No joysticks found!") GLib.timeout_add(int(self.SEND_INTERVAL * 1000), self._on_send_timer) GLib.io_add_watch(self.sock.fileno(), GLib.PRIORITY_DEFAULT, GLib.IO_IN | GLib.IO_ERR | GLib.IO_HUP, self._on_readable) # Refresh overlay when new logs are arriving if self.osd_logsaver: self.osd_logsaver.on_record.append( lambda *_: self.update_video_overlay()) if test_sim_keys is not None: # Exit once replay completes CriticalTask(self._replay_test_data(test_sim_keys)) def select_axes(self): complete = self.joystick.get_features(linux_input.EV.ABS) print "Joystick available axes:", complete # If we have all of X, Y, and RX, RY, then the axes are # probably mapped correctly and we can return what we expect. if (linux_input.ABS.X in complete and linux_input.ABS.Y in complete and linux_input.ABS.RX in complete and linux_input.ABS.RY in complete): return [ linux_input.ABS.X, linux_input.ABS.Y, linux_input.ABS.RX, linux_inpu.ABS.RY ] # Otherwise, just return the first axes on the hope that # this is meaningful. return complete[0:4] @wrap_event def _read_joystick(self): axes = self.select_axes() print "Selected joystick axes:", axes _DEADBAND = 0.2 def deadband(x): if abs(x) < _DEADBAND: return 0.0 if x > 0.0: return (x - _DEADBAND) / (1.0 - _DEADBAND) if x < 0.0: return (x + _DEADBAND) / (1.0 - _DEADBAND) while True: ev = yield From(self.joystick.read()) if ev.ev_type != linux_input.EV.ABS: continue dx, dy, dr, dz = [ deadband(self.joystick.absinfo(axes[x]).scaled()) for x in range(4) ] if abs(dx) == 0.0 and abs(dy) == 0.0 and abs(dr) == 0.0: if self.control_dict.get('gait') is not None: self.control_dict['gait'] = IDLE_COMMAND else: gait = RIPPLE_COMMAND.copy() speed = self.ui_state['speed'] gait['translate_x_mm_s'] = dx * 0.7 * speed gait['translate_y_mm_s'] = -dy * 1.0 * speed gait['rotate_deg_s'] = dr * 0.5 * speed self.control_dict['gait'] = gait if 'gait' in self.control_dict: z_mapping = 0.0 if dz < 0.0: z_mapping = -dz * self._MAX_Z_VALUE else: z_mapping = dz * self._MIN_Z_VALUE self.control_dict['gait']['body_z_mm'] = z_mapping @wrap_event def _on_send_timer(self): self._send_control() if self.video: tm_pp = self.video.get_time() tm_wc = time.time() if self.last_video_pp_time is not None: dt_pp = tm_pp - self.last_video_pp_time dt_wc = tm_wc - self.last_video_wall_time lost = (dt_wc - dt_pp) if abs(lost) > 0.01: self.logger.info('Video time lost: %.3f msec (%.1f%%)' % (lost * 1000.0, lost * 100.0 / (dt_wc or 1))) self.last_video_wall_time = tm_wc self.last_video_pp_time = tm_pp return True @wrap_event def _on_readable(self, source, cond): while True: try: pkt, addr = self.sock.recvfrom(65535) except socket.error as e: if e.errno != errno.EAGAIN: self.logger.error('Cannot read response: %s', e) break assert addr == self.addr, \ 'Got packet from %r, expected from %r' % (addr, self.addr) self._handle_packet(pkt) return True def _send_control(self): self.control_dict['seq'] += 1 self.control_dict.update(logs_from=self.remote_logs_from, cli_time=time.time()) serialized = self._log_struct(self.control_dict) self.sock.send(serialized) if self.control_dict['seq'] == 3 and self.video: self.video.start() def _handle_packet(self, pkt_raw): # Get the structure pkt = json.loads(pkt_raw) # Prepare to log pkt.update(cli_time=time.time(), _type='srv-state') if self.video: # Add locally derived values pkt['cli_pts'] = self.video.get_video_pts() if 'srv_pts' in pkt and (pkt.get('cli_pts') is not None): # Video latency latency = pkt['srv_pts'] - pkt['cli_pts'] pkt['latency_video'] = latency # Store min and max control latency for stats output self.video_extra_stats['lat_video'] = max( self.video_extra_stats.get('lat_video', -1), latency) self.video_extra_stats['lat_video_min'] = min( self.video_extra_stats.get('lat_video_min', 1e12), latency) if pkt['est_cli_time']: # Control link latency latency = pkt['cli_time'] - pkt['est_cli_time'] pkt['latency_ctrl'] = latency # Store max control latency for stats output self.video_extra_stats['lat_ctrl'] = max( self.video_extra_stats.get('lat_ctrl', -1), latency) server_time_offset = pkt['srv_time'] - pkt['cli_time'] if (self.server_time_offset is None) or \ abs(self.server_time_offset - server_time_offset) > 1.0: self.logger.debug('Server time offset %.3f sec', server_time_offset) self.server_time_offset = server_time_offset # Parse out messages -- no need to log them twice logs = pkt.pop('logs_data', None) or [] for entry in logs: if entry[0] > self.remote_logs_from: # new record self.remote_logs_from = entry[0] log_dict = MemoryLoggingHandler.to_dict(entry, time_field='srv_time') log_dict.update(_type='srv-log', cli_time=pkt['cli_time']) self._log_struct(log_dict) MemoryLoggingHandler.relog(entry, prefix='srv.') # Log it and store (for display) self._log_struct(pkt) self.server_state = pkt self.update_video_overlay() def _handle_video_mouse_move(self, pos, orig_button): """User is moving mouse in the window while the button is down (for performance, these events are not sent when button is up) @p pos - current mouse position @p orig_button - button from the click event This function is rate-limited. """ if not self._mouse_click_info: self.logger.debug('Ignoring spurious mouse move') return # How much we moved (in screen coordinates) -- so the range is # (-0.5..0.5) for each axis. delta_screen = add_pair(pos, self._mouse_click_info[0], -1.0) # Convert to new servo position by multiplying by arbitrary factor new_pos = add_pair(self._mouse_click_info[1], delta_screen, 25.0) if False: self.logger.debug( 'Moving turret to (%+.1f, %+.1f) deg in response ' 'to mouse move at (%+.4f, %+.4f)', new_pos[0], new_pos[1], delta_screen[0], delta_screen[1]) self.control_dict['turret'] = new_pos self._send_control() def _handle_video_mouse_click(self, pos, button): """User clicked button 1 on video window. pos is (x, y) tuple in 0..1 range""" assert len(pos) == 2 # This image_size will be broken if aspect ratio is different -- this # usually means truncated image, not resized pixels; but we do not care # for now. w2d, = self.c_cal.to_world2d([pos], image_size=(1.0, 1.0)) offset = self.ui_state['reticle_offset'] w2ang = (math.degrees(math.atan(w2d[0])) - offset[0], math.degrees(math.atan(w2d[1])) - offset[1]) self._mouse_click_info = None fire_cmd = None if (button == 3) and self.ui_state['autofire_mode']: # autofire mode fire_cmd = self.ui_state['autofire_mode'] elif button != 1: self.logger.info('Click with B%d at (w2d %.3f,%.3f, move by ' '%.1f,%.1f deg)' % ((button, ) + w2d + w2ang)) return old_turret = self.control_dict.get('turret') if old_turret is None: self.logger.warn('Cannot move turret -- center it first ' '(w2d %.3f,%.3f, w2a %.1f,%.1f)' % (w2d + w2ang)) return ang_x, ang_y = old_turret ang_x += w2ang[0] ang_y += w2ang[1] # TODO mafanasyev: add reticle_rotate support self.logger.debug( 'Moving turret to (%+.1f, %+.1f) deg in response ' 'to click at (%+.4f, %+.4f)', ang_x, ang_y, w2d[0], w2d[1]) self.control_dict['turret'] = (ang_x, ang_y) # Remember turret position self._mouse_click_info = (pos, self.control_dict['turret']) if not fire_cmd: pass elif fire_cmd != FCMD.cont: self._prepare_fire_command(fire_cmd) else: # TODO mafanasyev: implement FCMD.cont self.logger.error('Fire command %d not implemented', fire_cmd) self._send_control() def _handle_video_mouse_release(self, button): """User has release mouse button in a video window""" pass _GAIT_KEYS = { # key->(dx,dy,dr) 'w': (0, 1, 0), 's': (0, -1, 0), 'a': (-1, 0, 0), 'd': (1, 0, 0), 'q': (0, 0, -1), 'e': (0, 0, 1) } _Z_KEYS = { # key->dz 'f': -10, 'r': 10, } _MIN_Z_VALUE = -25 _MAX_Z_VALUE = 20 _SPEED_KEYS = { # key->dspeed 'z': -10, 'x': 10, } _MIN_SPEED_VALUE = 10 _MAX_SPEED_VALUE = 220 _ARROW_KEYS = { # key->(dx, dy) 'Left': (-1, 0), 'Right': (1, 0), 'Up': (0, -1), 'Down': (0, 1) } _FIRE_COMMANDS = { '1': FCMD.inpos1, '2': FCMD.inpos2, '3': FCMD.inpos3, '5': FCMD.inpos5, '9': FCMD.cont, '0': FCMD.off } def _handle_key_press(self, base_name, modifiers): arrows = self._ARROW_KEYS.get(base_name) if arrows: base_name = 'Arrows' name = modifiers + base_name speed = self.ui_state['speed'] if name in self._GAIT_KEYS: dx, dy, dr = self._GAIT_KEYS[name] gait = RIPPLE_COMMAND.copy() gait['translate_x_mm_s'] = dx * 0.7 * speed gait['translate_y_mm_s'] = dy * 1.0 * speed gait['rotate_deg_s'] = dr * 0.3 * speed self.control_dict['gait'] = gait self.key_gait_active = True elif name in self._Z_KEYS: dz = self._Z_KEYS[name] self._add_z(dz) elif name in ['g']: change = self._MAX_Z_VALUE - self._MIN_Z_VALUE if RIPPLE_COMMAND['body_z_mm'] == self._MIN_Z_VALUE: self._add_z(change) else: self._add_z(-change) elif name in self._SPEED_KEYS: dspeed = self._SPEED_KEYS[name] self.ui_state['speed'] += dspeed self.ui_state['speed'] = max( self._MIN_SPEED_VALUE, min(self._MAX_SPEED_VALUE, self.ui_state['speed'])) elif name in ['h', 'S-question']: self._print_help() elif name == 'l': self.control_dict['laser_on'] ^= 1 self.logger.info('Laser set to %d', self.control_dict['laser_on']) elif name in ['m', 'S-M']: val = self.control_dict['agitator_mode'] if name == 'm' and val != 1: val, val_str = 1, 'auto' elif name == 'S-M' and val != 2: val, val_str = 2, 'force-on' else: val, val_str = 0, 'off' self.control_dict['agitator_mode'] = val self.logger.info('Agitator mode to %s(%d) (pwm %.3f)', val_str, val, self.control_dict['agitator_pwm']) elif name == 'S-G': self.control_dict['green_led_on'] ^= 1 self.logger.info('Green LED set to %d', self.control_dict['green_led_on']) elif name in ['Return']: self._prepare_fire_command(FCMD.now1) elif name == 'c': self.control_dict['turret'] = (0.0, 0.0) self.logger.info('Centered turret') elif name in ['C-Arrows', 'C-S-Arrows']: # Ctrl + arrows to move reticle center step = 1.0 if ('S-' in modifiers) else 0.25 self.ui_state['reticle_offset'] = add_pair( self.ui_state['reticle_offset'], arrows, step) self.logger.info('Reticle center offset: %.3f, %.3f' % self.ui_state['reticle_offset']) elif name == 'C-r': self.ui_state['reticle_offset'] = (0, 0) self.logger.info('Zeroed out reticle center offset') elif name in ['Arrows', 'S-Arrows']: # Move turret step = 5.0 if ('S-' in modifiers) else 0.5 if self.control_dict.get('turret') is None: self.logger.warn('Cannot move turret -- center it first') else: self.control_dict['turret'] = new = add_pair( self.control_dict['turret'], arrows, step) self.logger.debug( 'Moving turret to (%+.1f, %+.1f) deg ' 'in response to keys', new[0], new[1]) elif name == 'C-S-M-Arrows': # Method will do its own logging step = 0.25 self.c_cal.tweak_fixups(arrows[0] * step, arrows[1] * step) elif name == 't': newmode = (self.ui_state['reticle_mode'] + 1) % 3 self.ui_state['reticle_mode'] = newmode self.logger.info('Set reticle_mode=%r', newmode) elif name == 'KP_Add': self.ui_state['msg_font_size'] += 1 elif name == 'KP_Subtract': self.ui_state['msg_font_size'] = max( 4, self.ui_state['msg_font_size'] - 1) elif name in ['S-parenleft', 'S-parenright', 'S-asterisk']: newval = max( 0, self.ui_state['fire_duration'] + (-0.05 if name == 'S-parenleft' else 0.05 if name == 'S-parenright' else 0)) self.ui_state['fire_duration'] = newval self.logger.info('Fire duration set to %.2f sec', newval) elif name == 'Escape': if self.osd_logsaver and self.osd_logsaver.data: numlines = len(self.osd_logsaver.data) self.logger.info('Cleared on-screen display (%d lines)', numlines) self._log_event('cli-log-osd-cleared', numlines=numlines) del self.osd_logsaver.data[:] else: self.ui_state['status_on'] ^= True self.logger.debug('Set status_on=%r', self.ui_state['status_on']) elif name in self._FIRE_COMMANDS: self.ui_state['autofire_mode'] = newval = self._FIRE_COMMANDS[name] self.logger.info('Right-button autofire mode %d', newval) if newval == FCMD.off: # Halt firing immediately when autofire is disabled self.control_dict['fire_cmd'] = None elif base_name in [ 'Shift_R', 'Shift_L', 'Control_R', 'Control_L', 'Alt_R', 'Alt_L' ]: # Ignore modifiers pass else: self.logger.debug('Unknown key %r' % name) self._state_updated() self._send_control() def _handle_key_release(self, evt): if self.key_gait_active: self.key_gait_active = False if self.control_dict['gait'] is not None: self.control_dict['gait'] = IDLE_COMMAND self._send_control() def _add_z(self, dz): for command in [RIPPLE_COMMAND, IDLE_COMMAND]: command['body_z_mm'] += dz command['body_z_mm'] = max( self._MIN_Z_VALUE, min(self._MAX_Z_VALUE, command['body_z_mm'])) def _get_video_extra_stats(self): # Return extra lines to print in periodic video stats line rv = dict() for key, val in self.video_extra_stats.items(): if val != 0: rv[key] = '%.1fmS' % (val * 1000.0) self.video_extra_stats = dict() return rv def _print_help(self): helpmsg = """ Help on Keys: w/s, a/d - move q/e - rotate r/f - move up/down g - toggle between lowest and highest position z/x - go slower/speed up l - laser on/off m/M - toggle agitator auto/forced G - green LED on/off c - enable && center turret left click - point turret to this location (must center first) right click - print location coordinates, or autofire ENTER - fire arrows - move turret S+arrows - move turret (move faster) C+arrows - set reticle center (use shift for more precision) C+S+arrows - set reticle center (move slower) C+r - zero out reticle offset C+S+M+arrows - temporarily tweak camera calibration t - toggle reticle S-( S-) - change fire duration S-* - show fire duration ESC - clear logs from OSD; then toggle status visibility 1/2/3/5/9/0 - right-button autofire: 1-5 shots, continuous, off Numpad +/- - change OSD font size """ for line in textwrap.dedent(helpmsg).strip().splitlines(): self.logger.info('| ' + line) @asyncio.coroutine def _replay_test_data(self, data): self.logger.info('Simulating keypresses based on a command line') for word in data.split(' '): if word.startswith('@'): # Sleep interval = float(word[1:]) yield From(asyncio.sleep(interval)) else: # Keypress if '-' in word: modifiers, base_name = word.rsplit('-', 1) modifiers += '-' else: modifiers, base_name = '', word self.logger.info('Simulating key: (%r, %r)', modifiers, base_name) self._handle_key_press(base_name, modifiers) # Maintain spacing yield From(asyncio.sleep(0.5)) # Exit program self.logger.info('Replay complete, exiting') # We will exit on return since this is a CriticalTask def _log_struct(self, rec): """Log @p rec (which must be a json-serialized dict) to logfile. @p rec must contain fields '_type' and 'cli_time' @returns serialized string """ sertext = json.dumps(rec, sort_keys=True) if self.state_log_file: print >> self.state_log_file, sertext # No need to flush, file should have autoflush return sertext def _log_event(self, name, **kwargs): rec = {"_type": "event", "cli_time": time.time(), "name": name} rec.update(kwargs) self._log_struct(rec) def _emit_json_logsaver_data(self): while self.json_logsaver.data: entry = self.json_logsaver.data.pop(0) log_dict = MemoryLoggingHandler.to_dict(entry, time_field='cli_time') if log_dict["name"].startswith("srv."): # Do not double-save server messages return log_dict.update(_type='cli-log') self._log_struct(log_dict) def _state_updated(self, force=False): sertext = json.dumps(self.ui_state, sort_keys=True) if (sertext == self._logged_state) and not force: return self.ui_state['cli_time'] = time.time() sertext = self._log_struct(self.ui_state) self._logged_text = sertext if self.state_savefile_name is not None: with open(self.state_savefile_name + '~', 'w') as fh: print >> fh, sertext # Atomically install new version os.rename(self.state_savefile_name + '~', self.state_savefile_name) self.update_video_overlay() def update_video_overlay(self): """Request update of video overlay in thread-safe way""" if self._update_video_overlay_pending: # An update is pending return self._update_video_overlay_pending = True self.asyncio_loop.call_soon_threadsafe(self._update_video_overlay_now) def _prepare_fire_command(self, command): """Prepare fire command. You still need to send_control for it to take an effect """ if self.server_state.get('srv_time') is None: self.logger.error('Cannot fire -- no server time') return server_time = (self.server_state['srv_time'] - self.server_state['cli_time'] + time.time()) self.fire_cmd_seq += 1 timeout = NORMAL_FIRE_TIMEOUT if FCMD._is_inpos(command): # Give extra time for servoes to settle timeout += INPOS_EXTRA_FIRE_TIMEOUT self.control_dict.update( fire_duration=self.ui_state['fire_duration'], fire_cmd=[command, self.fire_cmd_seq], # Ignore command if it is delayed by >0.5 sec fire_cmd_deadline=server_time + timeout) self.logger.debug('Sent fire command %d', command) @wrap_event def _update_video_overlay_now(self): self._update_video_overlay_pending = False if self.osd_logsaver: logs = self.osd_logsaver.data else: logs = [] stream = StringIO.StringIO() self.osd.render_svg(stream, self.ui_state, self.control_dict, self.server_state, logs) if self.video: self.video.set_svg_overlay(stream.getvalue()) if self.last_svg_name: with open(self.last_svg_name + '~', 'w') as f: f.write(stream.getvalue()) os.rename(self.last_svg_name + '~', self.last_svg_name)