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 stop_script(self, task):
self.stop_proc("ai_script/{}".format(task.name))
info('Stopping script {} for task {}'.format(task.script.name,
task.name))
task.stopped()
self.cleanup_task_pl(task)
self.gui_update_task(task)
def onMembershipChangedMessage(self, m):
if self.__auto:
info(
'received membership changed message, broadcasting parameters...'
)
time.sleep(0.3) #blergh
sendSavedMessages(self.__node, self.__shelf)
def run(self):
self.log('Mission time limit reached, surfacing.')
self.auv.depthAndWait(0, timeout=10)
self.ai.auv_control.disable()
while True:
info('Surface script still alive, waiting to be manually killed')
time.sleep(5)
def run(self):
while True:
estimated_power_use = 0
current_time = time.time()
for m in self.modules:
estimated_power_use += modules_dict[m](self.loggers[m],
current_time,
self.last_log_time)
#calculate the power since the last loop
interval = current_time - self.last_log_time
self.total_usage += estimated_power_use
fraction_remaining = 1 - self.total_usage / battery_total
self.log_file.write(
str(current_time) + '-' + str(estimated_power_use) + '-' +
str(self.total_usage) + '\n')
self.log_file.flush() #force write to file
info("Total power use: " + str(self.total_usage) +
" Watt Hours, estimated current use: " +
str(estimated_power_use / interval * 3600) +
" Watts, estimated battery remaining: " +
str(fraction_remaining * 100) + '%')
self.node.send(
messaging.BatteryUseMessage(
estimated_power_use / interval * 3600, self.total_usage,
fraction_remaining), "telemetry")
self.last_log_time = current_time
#sleep
time.sleep(self.frequency)
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 run(self):
for report in self.session:
info("Report received -- " + report["class"])
if (report["class"] == "TPV"
and report["mode"] == 3): #3 signals we have 3d position
if hasattr(report, "track"):
track = report.track
else:
track = 0
if hasattr(report, "speed"):
speed = report.speed
else:
speed = 0
if hasattr(report, "climb"):
climb = report.climb
else:
climb = 0
info("Location: [lat %f, lon %f, alt %f]" %
(report.lat, report.lon, report.alt))
self.__node.send(
msg.GPSLocationMessage(
msg.WGS84Coord(report.lat, report.lon, report.alt),
track, speed, climb))
def onRelativePositionMessage(self, m):
if m.origin != "AUV":
return
if m.object != "NECorner":
return
#turn off searching
self.searching = False
#face towards NE corner
info("NECorner @ "+ str(m.position.value))
bearing = atan2(m.position.value.east, m.position.value.north)
self.auv.bearingAndWait(degrees(bearing))
#calculate error
prop_error = (m.position.value.north+self.target[0])*cos(bearing)+sin(bearing)*(m.position.value.east+self.target[1])
strafe_error = (m.position.value.north+self.target[0])*sin(bearing)-cos(bearing)*(m.position.value.east+self.target[1])
if abs(prop_error) < self.target_error and abs(strafe_error) < self.target_error:
self.is_in_range.set()
prop=int(max(min(self.controller_p.update(prop_error), 127), -127))
strafe=int(max(min(self.controller_s.update(strafe_error), 127), -127))
info("Prop %f, Strafe %f" %(prop, strafe))
self.auv.prop(prop)
self.auv.strafe(strafe)
self.last_message_time = time.time()
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 onLinesMessage(self, m):
if m.name == "bay lines":
# we need some way to know when the positioning has been completed
# as there's going to be noise in the reading we need to wait for
# a few before returning a position
self.linesMessageCount += 1
if (self.linesMessageCount > 20): # wait for 20 messages
self.linesMessageCount = 0
self.positioningFinished.set()
else:
self.positioningFinished.clear()
# work out where we are from the lines...
rawpos = positionInBay(m.lines)
if rawpos != None:
a = vec(self.lastPos.x * (1 - self.alpha),
self.lastPos.y * (1 - self.alpha))
b = vec(rawpos.x * (self.alpha), rawpos.y * (self.alpha))
pos = a + b
if self.lastPos == None:
self.lastPos = rawpos
else:
self.lastPos = pos
info("Latest position: " + self.lastPos)
def getLocation(self):
info('getting Location')
r = messaging.LocationMessage()
r.location = messaging.WGS84Coord(self.location.north,
self.location.east,
-self.location.depth)
r.speed = messaging.floatXYZ(self.speed.x, self.speed.y, self.speed.z)
return r
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 load_pipeline(self, pipeline_name):
pipe_file = os.path.join(utils.dirs.config_dir('pipelines'),
pipeline_name) + ".yaml"
info("Loading pipeline {}".format(pipe_file))
with open(pipe_file) as pf:
pipeline = cauv.yamlpipe.load(pf)
model = cauv.pipeline.Model(self.node, self.map_pl_name(pipeline_name))
pipeline.fixup_inputs()
model.set(pipeline)
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 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 start_script(self, task):
if self.all_paused:
return
info("Starting script {} for task {}".format(task.script.name,
task.name))
script_path = inspect.getfile(task.script.script_class)
script_opts = task.script.options.to_cmd_opts()
script_cmd = ["python2.7", script_path, '-t', task.name, '-b'
] + script_opts
self.start_proc("ai_script/{}".format(task.name), script_cmd)
task.started()
self.gui_update_task(task)
def run(self):
while True:
time.sleep(1) # 1 second between checks
if not self.lastTimeoutSet == 0:
if time.time() - self.lastAliveTime > self.lastTimeoutSet:
self.onTimedOut()
timeUntilOut = self.lastTimeoutSet - (time.time() -
self.lastAliveTime)
bf = msg.BoundedFloat(timeUntilOut, 0, self.lastTimeoutSet,
msg.BoundedFloatType.Clamps)
self.__node.send(msg.PenultimateResortTimeoutMessage(bf))
info("sent status update")
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 setupParams(node, auv):
info('Setting calibration parameters')
auv.calibrateForFreshWater()
auv.bearingParams(2, 0, -160, 1)
auv.depthParams(100, 0.015, 50, 1)
auv.pitchParams(1, 0, 0, 1)
auv.propMap(0, 0, 127, -127)
auv.vbowMap(0, 0, 127, -127)
auv.hbowMap(0, 0, 127, -127)
auv.vsternMap(0, 0, 127, -127)
auv.hsternMap(0, 0, 127, -127)
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 buttonPressed(self, button):
if button == self.xbox_buttons.X:
info("Stop!")
self.auv.kill()
self.bearing_state = False
self.pitch_state = False
self.depth_state = False
if button == self.xbox_buttons.A:
self.auv.priority = -1
if button == self.xbox_buttons.B:
self.auv.priority = 10
#all the following controls are default locked
if self.Locked:
return
#Fine control of autopilots
if button == self.xbox_buttons.LEFT:
#auto pilot mode check first
if self.bearing_state == False:
self.bearing = self.auv.current_bearing
self.bearing_state = True
self.bearing = (self.bearing - 5) % 360
self.auv.bearing(self.bearing)
if button == self.xbox_buttons.RIGHT:
#auto pilot mode check first
if self.bearing_state == False:
self.bearing = self.auv.current_bearing
self.bearing_state = True
self.bearing = (self.bearing + 5) % 360
self.auv.bearing(self.bearing)
if button == self.xbox_buttons.UP:
#auto pilot mode check first
if self.depth_state == False:
self.depth = 0
self.depth_state = True
self.depth = self.depth - 0.1
self.auv.depth(self.depth)
if button == self.xbox_buttons.DOWN:
#auto pilot mode check first
if self.depth_state == False:
self.depth = 0
self.depth_state = True
self.depth = self.depth + 0.1
self.auv.depth(self.depth)
if button == self.xbox_buttons.JOY_R:
self.JOY_RPressed = True
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 _generate_detectors():
module_obj = sys.modules[__name__]
#basically we want to create a whole load of new classes based on this one and some data from the detector library
for detector_name, detector in detector_library.__dict__.iteritems():
info(str(detector))
if not hasattr(detector, "Detector"):
info(detector_name)
continue
name = detector_name + 'Condition'
detector_class = type(name, (DetectorCondition, ), {})
detector_class.detector_name = detector_name
detector_class.DefaultOptions = detector.Detector.DefaultOptions
detector_class.Detector = detector.Detector
setattr(module_obj, name, detector_class)
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 runLoop(auv_model):
auv_model.start()
while not rospy.is_shutdown():
# this is just a print-loop, the work is done in a separate thread,
# running at a configurable tick-rate
time.sleep(5)
(lt, ln, al, oriYPR, ori, speed) = auv_model.position()
#debug('lat:%.12g lon:%.12g alt:%.12g' % (lt, ln, al), 3)
#node.send(messaging.SimPositionMessage(messaging.WGS84Coord(lt, ln, al), oriYPR,
# messaging.quat(ori.q0, ori.q1, ori.q2, ori.q3), speed))
info('displ=%s\tvel=%s\tori=%s\tomega=%s\t' %
(fmtArr(auv_model.displacement), fmtArr(auv_model.velocity),
oriYPR, fmtArr(auv_model.angular_velocity)))
def search(self):
if not self.searching:
#if not within timeout, escape
if time.time() - self.last_message_time < self.search_timeout:
return
#timed out, so intialise search
info("Searching.")
self.searching = True
self.auv.prop(0)
self.auv.strafe(0)
#make sure searching in a suitable direction
self.search_bearing = self.auv.current_bearing
#TODO there should be some logic to avoid looking out to see here
self.search_bearing += self.search_angle*self.search_direction
self.auv.bearingAndWait(self.search_bearing)
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):
try:
iterations = 1/self.rate # send an alive message once per second
count = 0
while True:
self.handleEvents()
time.sleep(self.rate)
if count > iterations:
info("sending alive message")
self.__node.send(msg.RemoteControlsAliveMessage())
count = 0
count = count + 1
except KeyboardInterrupt:
if self.hasGamepad:
self.joystick.quit()
def run(self):
self.node.join('processing')
self.request_pl('sonar_collisions')
while True:
time.sleep(0.1)
if self.time_detected is not None and\
self.relativeTime() - self.time_detected < self.options.Run_Away_Time:
info('running away!')
if not self.in_control.is_set():
self.request_control_and_wait(1, control_timeout=5)
self.auv.prop(-127)
else:
if self.time_detected is not None and \
self.relativeTime() - self.time_detected < self.options.Run_Away_Time + 0.5:
self.auv.prop(0)
self.drop_control()
self.clearDetected()
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
