def dfu():
node = cauv.node.Node('py-dfu')
auv = control.AUV(node)
bearing = auv.getBearing()
auv.bearing(bearing)
try:
print 'setting bearing:'
time.sleep(4)
print 'diving...'
auv.depth(2)
time.sleep(5)
print 'forwards...'
auv.prop(127)
time.sleep(30)
print 'stop...'
auv.prop(0)
time.sleep(4)
print 'turning...'
except Exception:
traceback.print_exc()
auv.depth(0)
auv.stop()
finally:
auv.depth(0)
auv.stop()
print 'Complete'
auv.depth(0)
def __init__(self):
proc.Proc.__init__(self, self.__class__.__name__ + "Script")
self.options = self.get_options()
self.debug = self.Debug()
self.task_name = self.options._task_name
self.auv = control.AUV(self.node)
self.node.subMessage(msg.SetTaskStateMessage())
def __init__(self, node, test_name):
self.node = node
self.pl = pipeline.Model(node)
self.auv = control.AUV(node)
self.gemini = sonar.Gemini(node)
# configuration:
self.test_name = test_name
self.assoc_method = 'ICP' # 'ICP', 'NDT', 'non-linear ICP'
self.learn_keypoints = False
self.visualisation_video = True
#self.visualisation_files =
self.keypoints_video = False
#self.viz_superzoom =
#self.viz_midzoom =
self.inter_ping_delay = 0.1
self.resolution = 800
# important parameters:
self.reject_thr = 0.14 # higher = more support for score calculation, ransac support
self.max_correspond = 0.75 # hard to explain
self.weight_test = 0.5 # controls classifier ROC (0 = pass everything, 1 = fail almost everything)
self.score_thr = 0.04 # max error permitted for match
self.keyframe_spacing = 2.0 # minimum distance between keyframes (sort of)
self.max_matches = 3 # maximum number of pairwise correspondences attempted per scan
self.required_consensus = 1 # consensus required for match
# internal stuff:
self.video_output_nodes = []
self.setup()
def surface(delay):
node = cauv.node.Node('py-surf')
auv = control.AUV(node)
time.sleep(delay)
while True:
print 'surfacing...'
auv.stop()
time.sleep(1)
def __init__(self, node):
msg.MessageObserver.__init__(self)
self.node = node
self.auv = control.AUV(node)
self.motor_state_lock = threading.Lock()
self.motor_state = {}
self.motor_state[msg.MotorID.Prop] = 0
self.motor_state[msg.MotorID.HBow] = 0
self.motor_state[msg.MotorID.VBow] = 0
self.motor_state[msg.MotorID.HStern] = 0
self.motor_state[msg.MotorID.VStern] = 0
self.bearing = 0
self.pitch = 0
self.depth = 0
self.strafe = 0
self.prop = 0
self.last_telemetry_lock = threading.Lock()
self.last_telemetry = None
self.tk = tk.Tk()
self.tk.bind_all('<Key>', self.onKey)
#self.tk.withdraw()
self.frame = tk.Frame(self.tk)
self.frame.grid()
self.motorlabel = tk.Label(self.frame, text=self.motorText())
self.motorlabel.grid(row=1, column=1)
self.telemetrylabel = tk.Label(self.frame, text=self.telemetryText())
self.telemetrylabel.grid(row=2, column=1)
self.demandlabel = tk.Label(self.frame, text=self.demandText())
self.demandlabel.grid(row=2, column=1)
self.display_tick()
self.node.addObserver(self)
self.node.subMessage(msg.TelemetryMessage())
self.node.subMessage(msg.MotorStateMessage())
# Copyright 2013 Cambridge Hydronautics Ltd.
#
# See license.txt for details.
#
import cauv
import cauv.messaging as msg
import cauv.pipeline as pipeline
import cauv.control as control
import cauv.sonar
import cauv.node
import time
import sys
node = cauv.node.Node('sonartest', sys.argv[1:])
auv = control.AUV(node)
sonar = cauv.sonar.Sonar(node)
pl = pipeline.Model(node)
def bitVal(val, bit, off, on):
if val & bit == bit:
return on
else:
return off
def sendSonarTest(off, on, sleeptime=0.05):
print("Sending sonar test data")
for b in range(0, 64):
line = msg.SonarDataLine()
#!/usr/bin/env python2.7
#
# Copyright 2013 Cambridge Hydronautics Ltd.
#
# See license.txt for details.
#
import cauv
import rospy
#import cauv.pipeline as pipeline
import cauv.control as control
#import cauv.sonar
#import cauv.node
rospy.init_node("py_shell")
auv = control.AUV()
#sonar = cauv.sonar.Sonar(node)
#gemini = cauv.sonar.Gemini(node)
#pl = pipeline.Model(node)
try:
#All Crosby's fault... damn Mac users...
import IPython
old_ipython = [int(v) for v in IPython.__version__.split('.')] < [0, 11]
except:
old_ipython = False
if old_ipython:
from IPython.Shell import IPShellEmbed as InteractiveShellEmbed #pylint: disable=E0611
else:
from IPython.frontend.terminal.embed import InteractiveShellEmbed #pylint: disable=E0611
def Spiral():
# Create a node of the CAUV messaging service
node = cauv.node.Node('py-spiral')
# Create a python object for the control of the AUV
auv = control.AUV(node)
time.sleep(2)
#bearing = auv.getBearing()
#if bearing is None:
# print 'no bearing information!'
# time.sleep(5)
# bearing = 90
square = 2
bearing = 0
power = 64
unit = 3
try:
# Starting search at north direction
debug('setting bearing %d...' % bearing)
auv.bearingAndWait(bearing)
# 2m deep
debug('diving...')
auv.depthAndWait(2)
debug('spiral...')
# Individual half squares
for i in range(1, 2 * square):
debug('Performing %dth half circle' % i)
# Individual half squares
for j in range(2):
progress = 0
# Counter of progress in seconds
startTime = time.time()
debug('Moving forward and searching for %d seconds' % (3 * i))
# ...
# The time for which the AUV goes forward depends on the radius of the revolution
debug('Moving forward for %d seconds' % (3 * i))
auv.prop(power)
time.sleep(i)
# Stop motor & wait for stop
debug('stopping')
auv.prop(0)
time.sleep(5)
debug('setting bearing %d' % bearing)
bearing += 90
if bearing >= 360:
bearing -= 360
auv.bearingAndWait(bearing)
debug('surface...')
auv.depthAndWait(0)
except Exception:
traceback.print_exc()
auv.depth(0)
auv.stop()
debug('Complete')
def dfu():
node = cauv.node.Node('py-l')
auv = control.AUV(node)
bearing = 350
try:
time.sleep(1)
debug("Setting shit")
auv.autoCalibrateDepth()
auv.depthParams(500, 0.1, 0, scale=-1, maxError=40)
auv.bearingParams(3.5, 0, 35, scale=-1, maxError=150)
auv.pitchParams(4, 0.01, 0, scale=-1, maxError=5)
auv.pitch(0)
auv.depth_disabled = False
debug('set bearing')
auv.bearing(bearing)
time.sleep(4)
debug('down')
auv.depth(1)
time.sleep(5)
debug('forwards')
auv.prop(127)
time.sleep(31)
debug('stop')
auv.prop(0)
time.sleep(1)
auv.prop(-127)
time.sleep(1)
auv.prop(0)
time.sleep(1)
debug('turning')
auv.bearing(bearing - 90)
time.sleep(5)
debug('forwards')
auv.prop(127)
time.sleep(30)
debug('stop')
auv.prop(0)
time.sleep(1)
time.sleep(1)
auv.prop(-127)
time.sleep(1)
auv.prop(0)
time.sleep(1)
debug('surfacing')
auv.depth(0)
time.sleep(3)
auv.prop(0)
time.sleep(1)
auv.prop(-127)
time.sleep(1)
auv.prop(0)
time.sleep(1)
except Exception:
traceback.print_exc()
auv.depth(0)
auv.stop()
finally:
auv.depth(0)
auv.stop()
debug('Complete')
auv.depth(-1)
def Search():
node = cauv.node.Node(
'py-search') #Create a node of the spread messaging service
auv = control.AUV(node) #Create a python object for the control of the AUV
yellowFinder = ColourFinder(node,
[11, 12]) #Turn on the colour detection script
confirmer = PipeConfirmer(node, auv,
[11, 12]) #Holds the pipe confirm sequence
print 'setting calibration...' #setting the y intercept and gradient of the pressure/depth curve for front and back pressure sensor
# set-up calibration factors
node.send(
msg.DepthCalibrationMessage(-912.2 / 96.2, 1.0 / 96.2, -912.2 / 96.2,
1.0 / 96.2), "control")
time.sleep(2)
#bearing = auv.getBearing()
#if bearing is None:
# print 'no bearing information!'
# time.sleep(5)
# bearing = 90
#Searc parameters
revolution = 2 #Number of revolutions of the spiral square search from center
bearing = 0 #Initial bearing of the search
power = 64 #The motor power during search
unit = 3 #The time length of the smallest half revolution, the time length of subsequence half revolution will be mulituple of this one
depth = 0.5 #The depth of the search
try:
print 'setting bearing %d...' % bearing
#auv.bearingAndWait(bearing) #Starting search at north direction
auv.bearing(bearing)
print 'diving...'
#auv.depthAndWait(depth) #make sure it is at depth 2m
auv.depth(depth)
print 'spiral...'
for i in range(1, 2 * revolution): #making individual half revolutions
print 'Performing %dth half circle' % i
for j in range(2): #perform 2 turns for each half revolutions
progress = 0 #counter of progress in seconds
startTime = time.time()
print 'Moving forward and searching for %d seconds' % (3 * i)
while progress < (
unit * i
): #The time for which the AUV goes forward depends on the radius of the revolution
auv.prop(power)
progress = time.time() - startTime #Updating progress
if yellowFinder.detected() == 1:
#Quick stop
auv.prop(-127)
print 'found something, emergency stop'
time.sleep(2)
auv.prop(0)
#Pipe confirmation
if confirmer.confirm() == False:
#if pipe is not found, continue the search, and note the new startTime
startTime = time.time()
else:
#enable follower when pipe is confirmed
#follower = PipeFinder(node, auv, 'pipe', 0.4, 0.1) #Script to position the AUV to the center of yellowness and the adjust bearing
#time.sleep(20)
print 'surface...'
#auv.depthAndWait(0)
auv.depth(0)
return 0 #Insert object confirmation and reaction sequence here later
time.sleep(unit * i)
auv.prop(0) #shut off motor
time.sleep(5) #wait for the AUV to stop
print 'stoping'
bearing += 90 #Turn 90 degree
if bearing >= 360:
bearing -= 360
print 'setting bearing %d' % bearing
#auv.bearingAndWait(bearing)
auv.bearing(bearing)
print 'surface...'
#auv.depthAndWait(0)
auv.depth(0)
except Exception:
traceback.print_exc()
auv.depth(0)
auv.stop()
print 'Complete'
return 0