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
