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 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 sendSonarTest(off, on, sleeptime=0.05):
print("Sending sonar test data")
for b in range(0, 64):
line = msg.SonarDataLine()
line.data.clear()
for i in range(0, 6):
line.data.append(bitVal(b, 1 << i, off, on))
line.bearing = b * 100
line.bearingRange = 100
node.send(msg.SonarDataMessage(line))
time.sleep(sleeptime)
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 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())
-self.location.depth)
r.speed = messaging.floatXYZ(self.speed.x, self.speed.y, self.speed.z)
return r
if __name__ == '__main__':
parser = argparse.ArgumentParser(
usage='usage: %prog -m simple|exponential')
parser.add_argument(
"-m",
"--mode",
dest="mode",
default="simple",
help="integration mode: 'simple' or 'exponential' see" +
"displacement_integrator.py for exponential integrator constants")
opts, args = parser.parse_known_args()
node = cauv.node.Node('py-dspl', args)
try:
auv = control.AUV(node)
d = Location(node, opts.mode)
while True:
time.sleep(3)
ll = d.getLocation()
info('%s : %s' % (d.location, ll))
node.send(
messaging.LocationMessage(ll.location,
messaging.floatXYZ(0, 0, 0)))
finally:
node.stop()
import cauv
import cauv.messaging as msg
import cauv.control as control
import cauv.node
from cauv.debug import debug, warning, error, info
import time
import traceback
import math
import re
YPR_re = re.compile(
'.*\{ yaw = ([-0-9.]+), pitch = ([-0-9.]+), roll = ([-0-9.]+) \}.*')
if __name__ == '__main__':
import sys
node = cauv.node.Node('py-fake', sys.argv[1:])
auv = control.AUV(node)
with open('/Users/james/2010-12-10-control.log') as clog:
for line in clog:
time.sleep(0.01)
if YPR_re.match(line):
groups = YPR_re.match(line).groups()
print 'ypr:', groups
node.send(
msg.TelemetryMessage(
msg.floatYPR(float(groups[0]), float(groups[1]),
float(groups[2])), 0.0))
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
parser.add_argument("action",
choices=['start', 'stop', 'restart'],
help='action to take on process')
parser.add_argument("process", help="process name to control")
#parser.add_argument("--cmd", "-c", help="Command line to execute", nargs = argparse.REMAINDER, default = [])
parser.add_argument("--loud", help="Don't silence stdout", action='store_true')
parser.add_argument("--host",
"-t",
help="Host to execute on. * for all hosts",
default=socket.gethostname())
args = parser.parse_args()
if not args.loud:
#hacky hack hack
null_fd = os.open("/dev/null", os.O_RDWR)
os.dup2(null_fd, sys.stdout.fileno())
node = cauv.node.Node('watchctl.py')
action_map = {
'start': messaging.ProcessCommand.Start,
'stop': messaging.ProcessCommand.Stop,
'restart': messaging.ProcessCommand.Restart,
}
node.send(
messaging.ProcessControlMessage(action_map[args.action], args.host,
args.process))
node.stop()
if Sonar_Timestamp_Munge and isinstance(m, msg.SonarImageMessage):
unix_time = time.mktime(p.cursor().timetuple()) + p.cursor().microsecond/1e6
# !!! TODO: 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)
tlast = relativeTime()
except Exception, e:
error('error in playback: ' + str(e))
raise
debug('playback finished')
if __name__ == '__main__':
p = argparse.ArgumentParser(description='play a CHIL log file')
p.add_argument('file', metavar='FILE', type=str)
p.add_argument('-s', '--start', dest='start_t', default="0",
help='start time: offset seconds or absolute %s' % Strptime_Fmt.replace('%','%%'))
p.add_argument('-r', '--rate', dest='rate', type=str, default='x1',
help='messages per second (x1 for real-time, 2 for 2 messages per second)')
p.add_argument('-p', '--profile', dest='profile', default=False, action='store_true')