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 __init__(self, node):
msg.MessageObserver.__init__(self)
self.__node = node
self.__node.addObserver(self)
self.log = open(time.strftime('cauv-mission-%H-%M-%S.log'), 'w')
self.__node.join("telemetry")
self.__node.join("ai")
# Properties:
self.start_time = time.time()
# From LocationMessage:
self.latitude = 0
self.longitude = 0
self.altitude = 0
self.speed = msg.floatXYZ()
# From TelemetryMessage:
self.depth = 0
self.bearing = 0
# From AILogMessage:
self.comment = ""
p_OutputType = pp.Forward()
p_Controller = pp.Forward()
# Parse Structs
p_floatYPR << pp.Group(l \
+ p_float + c \
+ p_float + c \
+ p_float \
+ r)
p_floatYPR.setParseAction(lambda x: messaging.floatYPR(*x[0]))
p_floatXYZ << pp.Group(l \
+ p_float + c \
+ p_float + c \
+ p_float \
+ r)
p_floatXYZ.setParseAction(lambda x: messaging.floatXYZ(*x[0]))
p_floatXY << pp.Group(l \
+ p_float + c \
+ p_float \
+ r)
p_floatXY.setParseAction(lambda x: messaging.floatXY(*x[0]))
p_NodeInput << pp.Group(l \
+ p_int + c \
+ p_str + c \
+ p_int \
+ r)
p_NodeInput.setParseAction(lambda x: messaging.NodeInput(*x[0]))
p_LocalNodeInput << pp.Group(l \
+ p_str + c \
+ p_int \
+ r)
-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()
def xyz(vec, z=0):
for v in vec:
yield floatXYZ(v.x, v.y, z)
def add(v1, v2):
v = floatXYZ(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z)
return v
def absv(v):
return floatXYZ(abs(v.x), abs(v.y), abs(v.z))
def sxx(v_vec):
r = floatXYZ(0, 0, 0)
for v in v_vec:
r = add(r, sqr(v))
return r
def sx(v_vec):
r = floatXYZ(0, 0, 0)
for v in v_vec:
r = add(r, v)
return r
def pow(v1, p):
return floatXYZ(v1.x**p, v1.y**p, v1.z**p)
def sub(v1, v2):
v = floatXYZ(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z)
return v
