def report(self): if psi is None: return for process in watcher.processes.values(): if process.state == utils.watch.Running: try: stats = psi.process.Process(process.pid) except psi.process.NoSuchProcessError: warning("Process {} dissapeared when looking for stats".format(process.p.name)) continue curr_time, cputime = time.time(), stats.cputime.float() try: last_time, last_cputime = process.__last_times cpu_percent = (cputime - last_cputime) / (curr_time - last_time) except AttributeError: cpu_percent = 0 pass process.__last_times = (curr_time, cputime) statmsg = messaging.ProcessStatusMessage(socket.gethostname(), process.p.name, True, 'Running', cpu_percent, stats.rss, stats.nthreads) self.node.send(statmsg) elif process.cause_of_death: statmsg = messaging.ProcessStatusMessage(socket.gethostname(), process.p.name, False, process.cause_of_death, 0, 0, 0) #reset cause of death once reported process.cause_of_death = "" self.node.send(statmsg) elif self.report_all: statmsg = messaging.ProcessStatusMessage(socket.gethostname(), process.p.name, False, 'Not started', 0, 0, 0) self.node.send(statmsg) self.report_all = False #dont report all until requested to again
def convertToCHIL(self, dname, subname=None): logger = CHIL.Logger(dname, subname) if self.playbackIsActive(): info('you must stop playback before converting to chil format') return info('converting to CHIL: %s/%s' % (dname, subname if subname else '')) sorted_keys = sorted(list(self.__cached_keys)) base_time = datetime.datetime(1970,1,1,0,0,0,0) try: for i in xrange(0, len(sorted_keys)): k = sorted_keys[i] next_thing = self.__shelf[k.hex()] if msg.__class__ and msg.__class__.__name__.endswith('Message'): #pylint: disable=E1101 # this is a message debug('t=%g, converting: %s' % (k, next_thing), 5) logger.log(next_thing, base_time + datetime.timedelta(seconds=k)) elif type(next_thing) == type(dict()): # this is a message saved in the deprecated format m = dictToMessage(next_thing) debug('t=%g, converting: %s' % (k, m), 5) logger.log(m, base_time + datetime.timedelta(seconds=k)) elif hasattr(next_thing, 'datetime') and hasattr(next_thing, 'info'): # type info of these isn't saved properly?? warning('CHIL conversion: session comment "%s" will not be saved' % next_thing.info) base_time = next_thing.datetime else: warning('CHIL conversion: %s "%s" will not be saved' % (type(next_thing), next_thing)) except Exception, e: print 'error in conversion!' traceback.print_exc()
def check_pgm_addr(addr): if addr.find('127.0.0.1') != -1: return False pid = os.fork() if pid > 0: r_pid, ret = os.wait() if ret != 0: warning("PGM address {} is invalid!".format(addr)) return False else: return True else: #quiet things down so fatal-looking error messages don't confuse people try: null_fd = os.open("/dev/null", os.O_RDWR) os.dup2(null_fd, sys.stdin.fileno()) os.dup2(null_fd, sys.stdout.fileno()) os.dup2(null_fd, sys.stderr.fileno()) zmq_context = zmq.Context(1) s = zmq.Socket(c, zmq.PUB) s.connect(addr) s.setsockopt(zmq.LINGER, 200) s.send('TESTING') zmq_context.term() #should _always_ exit - bad things will probably happen otherwise except: os._exit(1) finally: os._exit(0)
def rollFromyzZ(y, z, Z): y_cross_Z = np.cross(y, Z) len_y_cross_Z = sum(y_cross_Z**2)**0.5 if len_y_cross_Z < 1e-6: warning('gimbal lock!') return 0 return math.atan2(np.dot(z, y_cross_Z / len_y_cross_Z), np.dot(z, Z))
def pitchFromxzZ(x, z, Z): x_cross_Z = np.cross(x, Z) len_x_cross_Z = sum(x_cross_Z**2)**0.5 if len_x_cross_Z < 1e-6: warning('gimbal lock!') return 0 return math.atan2(np.dot(z, x_cross_Z / len_x_cross_Z), np.dot(z, Z))
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 death_callback(proc): vals['restarts'] += 1 if n_times == 0 or vals['restarts'] <= n_times: proc.restart() else: warning("Process {} died too many times. Not restarting".format( proc.name))
def __init__(self, cauv_node, shelf_fname, do_record, playback_rate = 1.0): LoggerBase.__init__(self, cauv_node, do_record, playback_rate) self.__shelf = None self.__cached_keys = set() self.__shelf_fname = None # set by setFile self.setFile(shelf_fname) self.doRecord(do_record) warning('This logging class is deprecated, use CHILLogger instead')
def call_wrap(self, args, kargs): t = time.time() self.add_event(self.call_wrap, (args, kargs), {}) self.func(*args, **kargs) if self.options.delay is not None and \ t + self.options.delay < time.time(): warning(("Warning: repeating function {} taking longer to " + "execute than time period!").format(self))
def onMotorStateMessage(self, m): motor = m.motorID if motor in self.motors: with self.update: self.motors[motor].speedRequest = m.speed self.motors[motor].updateSpeedInMperS(self.mode) else: warning('Motor %s unknown to displacement.py used' % motor)
def fire(self, timeout): if timeout == 0: info("Detector stopped firing") else: info("Detector fired with timout of {} seconds".format( int(timeout))) warning("Auto conversion to int timeout in fire, please fix.") #TODO fix self.node.send(msg.DetectorFiredMessage(self.task_name, int(timeout)))
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 onTimedOut(self): warning('DIE control, DIE!\n') node.send( msg.ProcessControlMessage(msg.ProcessCommand.Stop, "control", "*", [])) node.send( msg.ProcessControlMessage(msg.ProcessCommand.Stop, "mcb_bridge", "*", [])) os.system('killall -s 9 control') os.system('killall -s 9 mcb_bridge')
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 stop(self, sigs): if self.state not in (Starting, Running): warning("Asked to stop {} which is not running".format( self.p.name)) return info("Stopping {}".format(self.p.name)) self.sigs = sigs if self.state == Starting: self.cause_of_death = "Stopped (Terminated)" self.state = Stopped return self.state = Stopping
def sendSavedMessages(node, shelf): info('restoring saved settings...') for mad_id in shelf: msg_name = mad_id.split(':')[0] if not msg_name in Default_Messages_To_Watch: continue try: msg_dict = shelf[mad_id] info('restoring saved %s: %s' % (msg_name, msg_dict)) m = dictToMessage(msg_dict) node.send(m) except Exception, e: warning('Exception: %s\nattempting to continue...' % traceback.format_exc())
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 onRelativePositionMessage(self, m): if m.origin != "AUV": return if self.auv.current_bearing == None: warning("No bearing information, not calculating distance") return #depending at which stage we are at we are reporting different distances if m.object == "NorthWall" and self.stage == 0: self.distance = self.parallel_dist(m.position) self.debug.distance = self.distance self.last_time = time() elif m.object == "BackWall" and self.stage == 2: self.distance = self.parallel_dist(m.position) self.debug.distance = self.distance self.last_time = time()
def kill(self, signal): #if we call this we also want to kill any leftover linked processes self.stop(signal) last_pid = -1 while True: pid = self.p.get_pid() if last_pid == pid: warning("Unkillable process {}, pid {}, ignoring".format( self.p.name, pid)) break if pid is not None: info("Found {}, pid {}, stopping".format(self.p.name, pid)) os.kill(pid, signal) else: break last_pid = pid
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 to_dict_with_meta(self, representable_types=option_representable_types): """Returns a dict with string keys and representable values (with meta data) or further dicts containing keys and representable values. Types which are not directly representable should also implement to_dict() with the same contract""" opts = {} for option, raw_value in self.iteroptions(): if isinstance(raw_value, OptionWithMeta): value = raw_value else: value = OptionWithMeta(raw_value) try: opts[option] = value.to_dict(representable_types) except TypeError: warning("Value {} ({}) cannot be represented".format( option, value.value)) return opts
def onProcessEndedMessage(self, msg): if not msg.process.startswith('ai_script'): return try: task_name = msg.process.split('/')[1] except IndexError: warning("Badly formatted AI process name") return try: task = self.tasks[task_name] info("Script for task {} exited".format(task.name)) self.cleanup_task_pl(task) if task.state.active: task.crashed() except KeyError: #can happen when removing tasks warning("Process for task {} ended, but task not present".format( task_name))
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 calibrateDepth(self, foreOffset, foreMultiplier, aftOffset=None, aftMultiplier=None): '''Set the conversion factors between pressure sensor values and depth. See also: calibrateForSaltWater() calibrateForFreshWater() autoCalibrateDepth() ''' if aftOffset is None: aftOffset = foreOffset if aftMultiplier is None: aftMultiplier = foreMultiplier else: warning("Warning: aftMultiplier set but aftOffset not set -- using aftOffset := foreOffset") elif aftMultiplier is None: warning("Warning: aftOffset set but aftMultiplier not set -- using aftMultiplier := foreMultiplier") self.depth_cal_aft_pub.publish (ctrl_msgs.DepthCalibrationMessage(aftOffset, aftMultiplier)) self.depth_cal_fore_pub.publish(ctrl_msgs.DepthCalibrationMessage(foreOffset, foreMultiplier))
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 telemetryLoggerMainLoop(cauv_node, opts): if opts.fname.endswith('.shelf'): warning('using deprecated logging format because you specified a "shelf" filename') LoggerClass = injectBase(TelemetryLogger, messageLogger._DeprecatedShelfLogger) else: LoggerClass = injectBase(TelemetryLogger, messageLogger.CHILLogger) tl = LoggerClass(cauv_node, opts.fname, not opts.no_record) cli = TelemetryLoggerCmdPrompt(tl) playback_is_active = False try: cli.cmdloop() except KeyboardInterrupt: pass finally: # this gets run even if we were killed (software-killed that is, # nothing can save us from the kill-switch...) tl.close() info('closed down properly') info('exiting...')
def run(self): self.log('Attempting spiral search...') # Starting search at north direction #debug('setting bearing %d...' % bearing) #self.auv.bearingAndWait(bearing) time.sleep(0.3) bearing = self.auv.getBearing() if bearing: self.auv.bearingAndWait(bearing) else: self.auv.bearingAndWait(0) if self.options.depth: self.log('Setting depth to %d' %(self.options.depth,)) debug('diving...') self.auv.depthAndWait(self.options.depth, 5) debug('spiral...') # Individual half squares for i in range(1, 2*self.options.loops): debug('Performing %dth half circle' % i) # Individual half squares for j in range(2): # The time for which the AUV goes forward depends on the radius of the revolution debug('Moving forward for %d seconds' %(self.options.unit*i)) self.auv.prop(self.options.power) time.sleep(self.options.unit*i) # Stop motor & wait for stop debug('stopping') self.auv.prop(0) time.sleep(self.options.stop_time) if bearing is None: warning('no bearing available!') bearing = 0 debug('setting bearing %d' % bearing) bearing += 90 if bearing>=360: bearing-=360 self.auv.bearingAndWait(bearing) self.log('Completed %d loops.' %(i*0.5,))
def __init__(self, name, args=None, run_now=True): setDebugName(name) info('CAUV Python Node Initialisation...') messaging.CauvNode.__init__(self, name) self.__t = None try: if os.getenv('CAUV_SHUTUP') is not None: lc = getVersionInfo()[1] if lc: warning('Running with uncommitted local changes:\n%s' % lc) except IOError: # stupid OS X... apparently my os module was compiled wrong pass if args is None: #this sets up default options self.parseOptions(sys.argv[0:1]) else: self.parseOptions(sys.argv[0:1] + args) if run_now: self.run(False)
def __init__(self, ai_state, opt_dict=None, name=None): self.options = self.DefaultOptions() if opt_dict is not None: self.options.from_dict(opt_dict) self.ai_state = ai_state self.reset() _type = self.get_type() if name is not None: if name in self.instances: warning("Duplicate condition name {}!".format(name)) name = None else: self.name = name self.instances[name] = self if name is None: for i in itertools.count(1): name = "{}_{}".format(_type, i) if name not in self.instances: self.name = name self.instances[name] = self break
def __init__(self, conditions, script, opt_dict = None, state_dict = None, name = None): self.options = self.DefaultOptions() if opt_dict is not None: self.options.from_dict(opt_dict) self.conditions = conditions self.script = script self.state = self.DefaultState() if state_dict is not None: self.state.from_dict(state_dict) _type = self.script.name if name is not None: if name in self.instances: warning("Duplicate Task name {}!".format(name)) name = None else: self.name = name self.instances[name] = self if name is None: for i in itertools.count(1): name = "{}_{}".format(_type, i) if name not in self.instances: self.name = name self.instances[name] = self break