def report_death(proc): try: info("Notifying about death of {}".format(proc.p.name)) proc.watcher._node.send( messaging.ProcessEndedMessage(socket.gethostname(), proc.p.name)) except AttributeError: error("Failed to notify as no access to messaging system node")
def fixPosition(self): # the AUV gets stopped in AI_location before this # method is called so we don't need to do it here # set up the sonar params self.sonar.range(self.sonarRange) self.sonar.rangeRes(self.sonarRangeRes) self.sonar.angularRes(self.sonarAnglularRes) self.sonar.gain(self.sonarGain) self.sonar.width(self.sonarWidth) # set up the pipeline self.pipeline.load("pipelines/sonar_walls.pipe") # wait for a full scan, but also check if we've run over # the time we're allowed for a capture while not self.positioningFinished.is_set(): # self.stopped is set by AI_location if the # auv pause we requested times out if self.stopped.is_set(): error("Bay processing stopped by timeout") return else: debug("Still waiting for position fix") self.positioningFinished.wait(1)
def __init__(self, node, rate, deadband): messaging.MessageObserver.__init__(self) self.__node = node node.join("control") node.join("gui") self.lastButtonValues = {} self.lastAxisValues = {} self.rate = rate self.deadband = deadband self.hasGamepad = False info("Gamepad server starting up...") node.addObserver(self) node.send(msg.SetPenultimateResortTimeoutMessage(10)) pygame.init() if pygame.joystick.get_count() == 0: error ("No gamepads detected") raise IOError() else: self.hasGamepad = True if self.hasGamepad: self.joystick = pygame.joystick.Joystick(0) self.joystick.init() info( "Initialized Joystick : %s" % self.joystick.get_name() ) for i in range(0, self.joystick.get_numbuttons()): self.lastButtonValues[i] = 0 for i in range(0, self.joystick.get_numaxes()): self.lastAxisValues[i] = 0.00
def restart(self, process, signals=None): if process not in self.processes: error("Process \"{}\" is not managed by watcher".format(process)) return if signals is None: signals = [15, 0, 0, 0, 9] self.processes[process].restart(signals)
def playbackRunloop(self): debug('playback started') p = CHIL.Player(self.dirname) # find time of first message: p.setCursor(datetime.datetime(year=datetime.MINYEAR, month=1, day=1)) tzero = p.timeOfNextMessage() p.setCursor(tzero + datetime.timedelta(seconds=self.playbackStartTime())) # find the time of the last message: # ... not implemented yet (wouldn't be too difficult) # tend = p.timeOfLastMessage() tstart_playback = self.relativeTime() try: while not self.playbackHasBeenStopped(): m, td = p.nextMessage() if m is None: break time_to_sleep_for = tdToFloatSeconds(td) - self.playbackRate() * (self.relativeTime() - tstart_playback) debug('sleeping for %gs' % time_to_sleep_for, 5) if time_to_sleep_for/self.playbackRate() > 10: warning('more than 10 seconds until next message will be sent (%gs)' % (time_to_sleep_for/self.playbackRate())) while time_to_sleep_for > 0 and not self.playbackHasBeenStopped(): playback_rate = self.playbackRate() sleep_step = min((time_to_sleep_for/playback_rate, 0.2)) time_to_sleep_for -= sleep_step*playback_rate time.sleep(sleep_step) #print 's: %s' % m.__class__.__name__ sys.stdout.write('.'); sys.stdout.flush() self.node.send(m) except Exception, e: error('error in playback: ' + str(e)) raise
def fixPosition(self): # the main function for generating a position fix for i in range(10): time.sleep(1) if self.stopped.is_set(): error("stop set, returning early") return info("Position fix finished")
def onSetConditionStateMessage(self, msg): try: condition = self.conditions[msg.conditionId] except KeyError: error("Condition {} does not exist!".format(msg.conditionId)) return condition.options.from_boost_dict(msg.conditionOptions) self.gui_update_condition(condition)
def execute(self): try: ret = self.func(*self.args, **self.kargs) if ret is not None: warning("Warning!, returning value from event {}".format( self.func.func_name)) except Exception: error("Exception in event: {}".format( traceback.format_exc().encode("ascii", "ignore")))
def onRemoveTaskMessage(self, msg): try: task = self.tasks[msg.taskId] except KeyError: error("Asked to remove non-existent task {}".format(msg.taskId)) return self.stop_script(task) del self.tasks[task.name] self.node.send(messaging.TaskRemovedMessage(msg.taskId))
def onAddTaskMessage(self, msg): try: task_script = AI.tasks.TaskScript(msg.taskType) except KeyError: error("Task type {} does not exist!".format(msg.taskType)) return task = AI.tasks.Task([], task_script) self.tasks.add(task) debug("Tasks: {}".format(self.tasks)) self.gui_update_task(task)
def __run(self): debug('cauv.Node running') atexit.register(self.stop) try: messaging.CauvNode.run(self, True) except: error(traceback.format_exc()) self.stop() finally: debug('CAUV Node stopped')
def play(fname, node, tstart, rt_rate, fixed_rate, filter_names=()): p = CHIL.Player(fname) try: tstart_float = float(tstart) p.setCursor(datetime.datetime(year=datetime.MINYEAR, month=1, day=1)) tzero = p.timeOfNextMessage() p.setCursor(tzero + datetime.timedelta(seconds=tstart_float)) except: p.setCursor(datetime.datetime.strptime(tstart, Strptime_Fmt)) if rt_rate is not None: assert(fixed_rate is None) tstart_playback = relativeTime() try: while True: m, td = p.nextMessage() if m is None: break if isinstance(m, str): warning(m) m = None if len(filter_names) and m.__class__.__name__ not in filter_names: continue time_to_sleep_for = tdToFloatSeconds(td) - rt_rate * (relativeTime() - tstart_playback) if time_to_sleep_for/rt_rate > 10: warning('more than 10 seconds until next message will be sent (%gs)' % (time_to_sleep_for/rt_rate)) while time_to_sleep_for > 0: sleep_step = min((time_to_sleep_for/rt_rate, 0.2)) time_to_sleep_for -= sleep_step*rt_rate time.sleep(sleep_step) sys.stdout.write('.'); sys.stdout.flush() if m is not None: if Sonar_Timestamp_Munge and isinstance(m, msg.SonarImageMessage): unix_time = time.mktime(p.cursor().timetuple()) + p.cursor().microsecond/1e6 # for some reason the message seems to be immutable... m = msg.SonarImageMessage( m.source, msg.PolarImage( m.image.data, m.image.encoding, m.image.bearing_bins, m.image.rangeStart, m.image.rangeEnd, m.image.rangeConversion, msg.TimeStamp(int(unix_time), int(1e6*(unix_time-int(unix_time)))) ) ) node.send(m) except Exception, e: error('error in playback: ' + str(e)) raise debug('playback finished')
def run(self): self.load_pipeline('follow_pipe2') # first we need to check we've actually found the pipe # the detector doesn't really look for the pipe it just looks # for yellow things in the downward camera, so there might be # a few false positives self.log('Attempting to align over the pipe.') # now we wait for messages allowing us to work out how to align with # the pipe, but if this is taking too long then just give up as we've # probably drifted away from the pipe debug('Waiting for ready...') if not self.ready.wait(self.options.ready_timeout): self.log( 'Pipeline follower could not position itself over the pipe (timed out).' ) error("Took too long to become ready, aborting") return 'ABORT' #now make sure we're facing in (roughly) the right direction if abs((self.auv.current_bearing - self.options.initial_direction) % 360) > 90: self.log("Switching direction (was facing wrong way)") self.enabled = False self.auv.prop(0) self.auv.strafe(0) self.auv.bearingAndWait((self.auv.current_bearing + 180) % 360) self.enabled = True for i in range(self.options.follows): self.log('Attempting to follow pipe.') self.auv.prop(self.options.prop_speed) # follow the pipe along until the end if not self.followPipeUntil(self.pipeEnded): self.log('Lost the pipeline...') error("Pipeline lost on pass %d" % (i, )) return 'LOST' debug('Reached end of pipe') self.log("Reached end of pipe.") self.auv.prop(0) self.auv.strafe(0) #if this is the last run, dont turn if i == self.options.follows - 1: break debug('Turning (turn %i)' % i) self.enabled = False self.auv.bearingAndWait((self.auv.current_bearing + 180) % 360) self.enabled = True self.log('Finished following the pipe.') info('Finished pipe following') return 'SUCCESS'
def onAddConditionMessage(self, msg): try: Condition = AI.conditions.get_conditions()[msg.conditionType] except KeyError: error("Condition type {} does not exist!".format( msg.conditionType)) return condition = Condition(self.ai_state) self.conditions.add(condition) self.gui_update_condition(condition) debug("Conditions: {}".format(self.conditions)) if isinstance(condition, AI.conditions.DetectorCondition): self.start_detector(condition)
def startPlayback(self, start_time): if self.__playback_active: error("can't start playback: playback is already active") return self.__playback_lock.acquire() self.__playback_start_time = start_time self.__playback_active = True self.__playback_lock.release() if self.profile_playback: self.__playback_thread = threading.Thread(target=self.profilePlayback) else: self.__playback_thread = threading.Thread(target=self.playbackRunloop) self.__playback_thread.start()
def prereq(proc): can_run = True for other in others: if not proc.watcher.is_running(other): can_run = False try: info("Starting {} for {}".format(other, proc.p.name)) proc.watcher.start(other) except KeyError: error("Dependency for {}, {}, doesn't exist!".format( proc.p.name, other)) proc.stop([]) return False return can_run
def onScriptExitMessage(self, msg): try: task = self.tasks[msg.task] except KeyError: error("Received exit for task {} which doesn't exist!".format( msg.task)) self.stop_script(task) if msg.status == messaging.ScriptExitStatus.Success: task.succeeded() info("Task {} succeeded".format(task.name)) if msg.status == messaging.ScriptExitStatus.TempFailure: task.crashed() if msg.status == messaging.ScriptExitStatus.PermFailure: task.failed()
def onRemoveConditionMessage(self, msg): try: condition = self.conditions[msg.conditionId] except KeyError: error("Condition {} does not exist!".format(msg.conditionId)) return for task in self.tasks.values(): try: task.conditions.remove(condition) except ValueError: pass if isinstance(condition, AI.conditions.DetectorCondition): self.stop_detector(condition) del self.conditions[msg.conditionId] self.node.send(messaging.ConditionRemovedMessage(msg.conditionId))
def run_cmd(self, cmd, strip_code=True, timeout=1000): self.ctrl_s.send(cmd) if self.poller.poll(timeout): recv = self.ctrl_s.recv() debug(cmd) debug(recv) ret = json.loads(recv) else: raise Timeout("Timed out waiting for reply from daemon") if not ret["success"]: error("Error running daemon command {} : {}".format( cmd, ret["error"])) if strip_code: del ret["success"] return ret
def onSetTaskStateMessage(self, msg): try: task = self.tasks[msg.taskId] except KeyError: error("Task {} does not exist!".format(msg.taskId)) return task_conditions = [] for c in msg.conditionIds: try: task_conditions.append(self.conditions[c]) except KeyError: error("Condition {} does not exist!".format(c)) task.conditions = task_conditions task.options.from_boost_dict(msg.taskOptions) task.script.options.from_boost_dict(msg.scriptOptions) self.gui_update_task(task)
def run(self): self.load_pipeline('detect_buoy_sim') self.time_last_seen = None #get initial bearing if self.auv.current_bearing == None: time.sleep(1) if self.auv.current_bearing == None: error("No bearing information, giving up.") raise Exception("No bearing information.") start_bearing = self.auv.current_bearing total_right_angles_turned = 0 last_bearing = self.auv.current_bearing #start circle processing info('Waiting for circles...') self.node.subMessage(msg.CirclesMessage()) #monitor progress while total_right_angles_turned < self.options.TotalRightAnglesToTurn: time.sleep(min(self.options.Do_Prop_Time, self.options.Warn_Seconds_Between_Sights)/2.) time_since_seen = 0 if self.time_last_seen is None: warning('Not seen buoy, waiting') else: time_since_seen = time.time() - self.time_last_seen if time_since_seen > self.options.Do_Prop_Time: self.auv.prop(0) if time_since_seen > self.options.Warn_Seconds_Between_Sights: warning('cannot see buoy: last seen %g seconds ago' % time_since_seen) self.auv.strafe(0) self.auv.prop(0) if time_since_seen > self.options.Give_Up_Seconds_Between_Sights: self.log('Buoy Circling: lost sight of the buoy!') return False #note travelling left, so turning clockwise ie bearing increasing if (self.auv.current_bearing != None): bearing_diff = abs(self.auv.current_bearing - last_bearing) if min(bearing_diff, 360-bearing_diff) > 90: #assume we don't turn to fast last_bearing = (last_bearing+90)%360 total_right_angles_turned += 1 self.auv.stop() self.log('Buoy Circling: completed successfully') return
def onProcessControlMessage(self, msg): #Check we are the host that should be acting on this message if not (msg.host == '*' or msg.host == socket.gethostname()): return #Check that a valid name was given if msg.process not in watcher.processes: return #now try acting on the specified process try: if msg.action == messaging.ProcessCommand.Start: watcher.start(msg.process) elif msg.action == messaging.ProcessCommand.Stop: watcher.stop(msg.process) elif msg.action == messaging.ProcessCommand.Restart: watcher.restart(msg.process) except KeyError: error(str(watcher.processes)) error("Process {} does not exist!".format(msg.process))
def playbackRunloop(self): debug('playback started at %fx' % self.playbackRate()) sorted_keys = sorted(list(self.__cached_keys)) start_idx = bisect.bisect_left(sorted_keys, self.playbackStartTime()) sorted_keys = sorted_keys[start_idx:] if not len(sorted_keys): return tstart_recorded = sorted_keys[0] tstart_playback = self.relativeTime() try: for i in xrange(0, len(sorted_keys)): if self.playbackHasBeenStopped(): info('playback stopped') break next_thing = self.__shelf[sorted_keys[i].hex()] if msg.__class__ and msg.__class__.__name__.endswith('Message'): #pylint: disable=E1101 # this is a message debug('t=%g, sending: %s' % (sorted_keys[i], next_thing), 5) self.node.send(next_thing) if type(next_thing) == type(dict()): # this is a message saved in the deprecated format m = dictToMessage(next_thing) debug('t=%g, sending: %s' % (sorted_keys[i], m), 5) self.node.send(m) else: info('playback: %s' % next_thing) if i != len(sorted_keys) - 1: next_time = sorted_keys[i+1] time_to_sleep_for = (next_time - tstart_recorded) -\ self.playbackRate() * (self.relativeTime() - tstart_playback) #debug('sleeping for %gs' % time_to_sleep_for, 5) if time_to_sleep_for/self.playbackRate() > 10: warning('more than 10 seconds until next message will be sent (%gs)' % (time_to_sleep_for/self.playbackRate())) while time_to_sleep_for > 0 and not self.playbackHasBeenStopped(): playback_rate = self.playbackRate() sleep_step = min((time_to_sleep_for/playback_rate, 0.2)) time_to_sleep_for -= sleep_step*playback_rate time.sleep(sleep_step) if i == len(sorted_keys)-1: info('playback complete') except Exception, e: error('error in playback: ' + str(e)) raise
def evaluate(self, script): # TODO: apply timeout... somehow warning('timeout is ignored') debug('evaluating script %s:\n%s\n#[end script]' % (script, script.script)) self.resetEnvironment(script) with open('remote.py.script.tmp', 'w+') as tmpf: tmpf.write(script.script) try: r = execfile('remote.py.script.tmp', self.eval_context_globals, self.eval_context_globals) except Exception: message = 'error in script:\n' + traceback.format_exc() error(message) self.sendScriptResponse(script, msg.DebugType.Error, message) else: message = 'script returned: ' + str(r) info(message) self.sendScriptResponse(script, msg.DebugType.Info, message)
def head_to_distance(self, target_distance, target_error, proportion, clamp, timeout): """ Head forward until self.distance is within target_error of target_distance """ #make sure clean feed of data self.distance = None self.last_time = time() start_time = time() while time() - start_time < timeout or timeout <= 0: #Data timeout check if time() - self.last_time > self.options.DataTimeout: error("Data timeout: have not recieved lines for {}s".format( self.options.DataTimeout)) self.auv.prop(0) raise Exception( "Data timeout: have not recieved lines for {}s".format( self.options.DataTimeout)) #Distance exists check if self.distance == None: sleep(0.1) continue #Movement check #TODO #Distance check if abs(self.distance - target_distance) < target_error: info("Close enough") self.auv.prop(0) break #set speed p_speed = (self.distance - target_distance) * proportion if p_speed > clamp: p_speed = clamp elif p_speed < -clamp: p_speed = -clamp self.auv.prop(int(p_speed)) sleep(0.1) else: #didn't break from being close enough, so raise error error("Timed out heading to distance.") raise Exception("Timed out")
def onAIControlMessage(self, msg): command = msg.command.name if command == 'PauseAll': self.all_paused = True for task in self.tasks.values(): if task.state.active: self.stop_script(task) info("AI Paused") elif command == 'ResumeAll': self.all_paused = False info("AI Unpaused") elif command == 'Save': save_file = "{}_save_{}.yaml".format( self.mission_name, datetime.datetime.now().isoformat()) info("Saving mission to {}".format(save_file)) try: AI.mission.dump(self.tasks.values(), open(save_file, 'w')) except IOError: error('Could not save mission') error(traceback.format_exc().encode('ascii', 'ignore'))
def starting(self): pid = self.p.get_pid() if pid is not None: self.pid = pid info("found PID {} for process {}".format(self.pid, self.p.name)) self.state = Running return if self.p.prereq(self): try: with open(os.devnull, "rw") as nullf: self.proc = subprocess.Popen(self.p.cmd, stdout=nullf, stdin=nullf, stderr=nullf) except Exception: error("Error starting up process {}".format(self.p.name)) error(traceback.format_exc().encode('ascii', 'replace')) self.restart = False return self.pid = self.proc.pid info("Started {} with pid {} (command {})".format( self.p.name, self.pid, self.p.cmd)) self.state = Running
def stopping(self): if not self.pid_running(): if self.state == Restarting: self.p.restart_callback(self) self.state = Starting else: self.cause_of_death = "Stopped (Terminated)" self.state = Stopped return try: sig = self.sigs.pop(0) except IndexError: error("No more signals to try for killing {}. Giving up!".format( self.p.name)) self.cause_of_death = "Unresponsive (Zombie)" self.state = Zombie return try: info("Killing process {} with signal {}".format(self.p.name, sig)) os.kill(self.pid, sig) except OSError: error("Error killing process {}: {}".format( self.p.name, traceback.format_exc().encode('ascii', 'replace')))
def running(self): if self.pid_running(): return self.pid = None self.state = Stopped self.cause_of_death = "Died (Unknown CoD)" warning("Process {} died!".format(self.p.name)) try: self.p.death_callback(self) except Exception: error("Error in process {} death_callback:".format(self.p.name)) error(traceback.format_exc().encode('ascii', 'replace')) error("Not restarting {}".format(self.p.name)) self.state = Stopped
def setup_core_dumps(): core_pattern = "/var/tmp/cauv_corefiles/%e.%d.%t.%p" try: try: os.mkdir("/var/tmp/cauv_corefiles", 0777) except OSError as e: if e.errno != errno.EEXIST: error("Could not create corefile directory") raise with open("/proc/sys/kernel/core_pattern", "r") as pattern_file: if not pattern_file.read().startswith(core_pattern): with open("/proc/sys/kernel/core_pattern", "w") as pattern_w_file: pattern_w_file.write(core_pattern) soft, hard = resource.getrlimit(resource.RLIMIT_CORE) resource.setrlimit(resource.RLIMIT_CORE, (min(hard, 10 * 1024 * 1024), hard)) #10MB max except IOError as e: if e.errno == errno.EACCES: error("Could not change core pattern: not running as root?") elif e.errno == errno.ENOENT: error("Could not find core pattern file. Probably using a Mac...") else: raise