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
