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