p_SonarID = pp.Forward()
p_LightID = pp.Forward()
p_NodeType = pp.Forward()
p_NodeStatus = pp.Forward()
p_NodeInputStatus = pp.Forward()
p_NodeIOStatus = pp.Forward()
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 \
def floatYPR(self):
return msg.floatYPR(self.yaw, self.pitch, self.roll)
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 __init__(self, fypr = msg.floatYPR()):
self.yaw = fypr.yaw
self.pitch = fypr.pitch
self.roll = fypr.roll