def writeLine(self):
now = time.time() - self.start_time
line = "'(%g,%g,%g,%g,'%s')'\n" % (now, self.latitude, self.longitude,
self.depth, self.comment)
debug(line)
self.log.write(line)
self.comment = ''
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 process(self):
try:
cols = map(lambda x: x.y, self.messages.pop())
except IndexError:
#no elements to process
return
cols.sort()
#check minimum distance
self.last_min = cols[0]
if cols[0] < self.options.min_distance:
#too close, reverse
self.detected = True
return
else:
self.detected = False
#take closest points
monitor_number = int(round(self.options.monitor_fraction * len(cols)))
cols = cols[:monitor_number]
#calculate average distance
self.last_mean = sum(cols) / float(len(cols))
#if mean is to far away, no limit
speed_limit = self.options.distance_factor * self.last_mean #factor*distance from object
self.speed_limit = int(round(speed_limit))
#self.ai.auv_control.limit_prop(self.speed_limit)
debug("minimum distance %f, mean %f, setting speed limit to %d" % (
cols[0],
self.last_mean,
self.speed_limit,
))
self.messages.clear()
def gui_update_condition(self, condition):
self.node.send(
messaging.ConditionStateMessage(condition.name,
condition.options.to_boost_dict(),
condition.get_state()))
debug("Condition {} has state {}".format(condition.name,
condition.get_state()))
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 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 run(self):
if self.persist.already_run:
return 'SUCCESS'
self.persist.already_run = True
self.auv.bearingAndWait(self.options.bearing + 90)
if self.options.useDepth:
self.log('Diving to %d to start mission' % (self.options.depth))
self.auv.depthAndWait(self.options.depth)
self.log('Heading forwards towards validation gate')
self.auv.prop(self.options.forward_speed)
time.sleep(self.options.to_gate_time)
self.log('Turning to face gate')
self.auv.prop(0)
self.auv.bearingAndWait(self.options.bearing)
self.log('Heading forwards through validation gate')
self.auv.prop(self.options.forward_speed)
time.sleep(self.options.forward_time)
self.log('Turning around')
self.auv.prop(0)
self.auv.bearingAndWait((self.options.bearing + 180) % 360)
self.log('Heading back through the validation gate')
self.auv.prop(self.options.forward_speed)
time.sleep(self.options.forward_time)
debug(
"Heading blindly in this direction until something stops me or %d seconds elapse"
% (self.options.forward_time))
self.auv.prop(0)
return 'SUCCESS'
def onMotorStateMessage(self, m):
debug(str(m))
if not self.keep_going:
return
with self.update_lock:
self.motor_states.update(m)
self.processUpdate(self.motor_states)
def onPointsMessage(self, m):
if m.name != self.options.points_name:
return
if len(m.points) * self.options.monitor_fraction < 0.5:
debug('Not enough points in image')
return
self.messages.append(m.points)
def logObject(self, d):
if self.recordingIsActive():
t = self.relativeTime()
debug('shelving object: t=%g' % t, 6)
while t in self.__cached_keys:
t = incFloat(t)
self.__shelf[t.hex()] = d
self.__cached_keys.add(t)
def sendScriptResponse(self, script, level, m):
debug('sending script response to %s: %s' % (script, str(m)))
response = msg.ScriptResponse()
response.response = str(m)
response.level = level
response.id = script.id
response.seq = script.seq
self.send(msg.ScriptResponseMessage(response))
def run(self):
debug('main loop started')
try:
self.tk.lift()
self.tk.mainloop()
except KeyboardInterrupt:
self.tk.destroy()
debug('main loop finished')
def print_bins(self, m):
#Routine to display all the bin values
accum = 0
message = ''
for i, bin in enumerate(m.bins):
accum += bin
message += 'bin %d: %f, accum: %f\n' % (i, bin, accum)
debug(message)
def onMessage(self, m):
debug('onMessage expect %sMessage have %s' % (self.message_name, m))
if self.message_name in Message_ID_Fields:
msg_id_field = getattr(m,
str(Message_ID_Fields[self.message_name]))
save_as_id = "%s_%s" % (self.message_name, msg_id_field)
else:
save_as_id = self.message_name
self.shelf[save_as_id] = dictFromMessage(m)
def onCirclesMessage(self, m):
#if m.name != self.options.Circles_Name:#If the data being received isn't the circle data
# return
if len(m.circles) != 1:
debug("Incorrect number of circles", 5)
return
debug("Saw circle", 5) #Note that one/more cirles seen
self.circles.append(m.circles[0]) #Add a the data in circles[0] to the right of the circles deque
self.times.append(time.time()) #Ditto with the clock time into the time deque
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 onCirclesMessage(self, m):
if m.name == self.options.Circles_Name:
# assuming time collisions are not going to happen very often!
t = self.relativeTime()
while t in self.circles_messages:
# twiddle twiddle
t = incFloat(t)
self.circles_messages[t] = m
else:
debug('ignoring circles message: %s' % m.name, 2)
def onHistogramMessage(self, m):
good = False
if m.name == self.options.Histogram_Name_A:
self.colour_detector_A.update(m)
good = True
if m.name == self.options.Histogram_Name_B:
self.colour_detector_B.update(m)
good = True
if not good:
debug('ignoring histogram message: %s' % m.name, 2)
def stop(self):
debug('stopping messaging thread...')
messaging.CauvNode.stop(self)
debug('joining messaging thread...')
try:
if self.__t is not None:
self.__t.join()
except RuntimeError:
#tried to join current thread
pass
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 onCirclesMessage(self, m):
if m.name != self.options.Circles_Name:
return
#check right number of circles
if len(m.circles) != 1:
debug("Incorrect number of circles", 5)
return
debug("Saw circle", 5)
self.circles.append(m.circles[0])
self.times.append(time.time())
def __init__(self, vehicle_name=None, ipc_dir=None):
self.zmq_context = zmq.Context(1)
daemon_control_str = "ipc://{}/daemon/control".format(
get_vehicle_dir(ipc_dir, vehicle_name))
self.vehicle = vehicle_name
debug("Connecting to daemon via {}".format(daemon_control_str))
self.ctrl_s = zmq.Socket(self.zmq_context, zmq.REQ)
self.ctrl_s.connect(daemon_control_str)
self.ctrl_s.setsockopt(zmq.LINGER, 100)
self.poller = zmq.Poller()
self.poller.register(self.ctrl_s, zmq.POLLIN)
def stopPlayback(self):
if not self.__playback_active:
return
self.__playback_lock.acquire()
self.__playback_active = False
self.__playback_lock.release()
self.__playback_finished.acquire()
debug('waiting for playback thread...')
self.__playback_finished.wait()
self.__playback_finished.release()
self.__playback_thead = None
def __init__(self, image_filename, origin_at_px_x_y, m_per_px, datum):
self.image = Image.open(image_filename)
# latitude/longitude position of world xyz coordinate system origin:
self.datum = datum
# origin in the image of the world xyz coordinate system origin:
self.origin = origin_at_px_x_y
self.m_per_px = float(m_per_px)
debug('environment is %sx%s metres' %
(self.image.size[0] * self.m_per_px,
self.image.size[1] * self.m_per_px))
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 position(self):
with self.update_lock:
ned = coordinates.NorthEastDepthCoord(self.displacement[1],
self.displacement[0],
-self.displacement[2])
ori = self.orientation
vel = self.velocity
debug('%s' % ned, 3)
r = self.datum + ned
return (float(r.latitude), float(r.longitude), float(r.altitude),
orientationToYPR(ori), ori,
ctrl_msgs.Attitude(*(float(x) for x in vel)))
def __init__(self, node, shelf, watch_list, auto):
msg.MessageObserver.__init__(self)
self.__node = node
self.__shelf = shelf
self.__auto = auto
if auto:
node.subMessage(msg.MembershipChangedMessage())
for message in watch_list:
debug('Listening for %s messages' % message)
node.subMessage(getattr(msg, message + 'Message')())
self.attachOnMessageFunc(message)
node.addObserver(self)
def doRecord(self, do_record):
if self.__recording == do_record:
return
self.__recording = do_record
if do_record:
if self.playbackIsActive():
self.stopPlayback()
self.node.addObserver(self)
debug('recording started')
else:
self.node.removeObserver(self)
debug('recording stopped')
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 detectionConfidence(self, message):
if len(message.circles) == 0:
return 0
sx_radius = sum((x.radius for x in message.circles))
mean_radius = sx_radius / len(message.circles)
sx = vecops.sx(vecops.xyz((x.centre for x in message.circles)))
sxx = vecops.sxx(vecops.xyz((x.centre for x in message.circles)))
stddev = vecops.pow(vecops.absv(vecops.sub(sxx,vecops.sqr(sx))), 0.5)
s = (stddev.x + stddev.y) / 2
confidence = 1 / (1 + self.options.Stddev_Mult * s/mean_radius)
if len(message.circles) == 1:
confidence = 0.9
debug('circle centres s=%g, confidence=%g mean radius=%g' % (s, confidence, mean_radius), 5)
return confidence
