def __init__(self, opts):
super(LocationManager, self).__init__()
self.node = cauv.node.Node("location_manager")
#subscribe to appropriate messages
self.node.subMessage(messaging.LinesMessage())
self.node.subMessage(messaging.FloatMessage())
self.node.subMessage(messaging.TelemetryMessage())
#setup appropriate image pipeline
pipe_file = os.path.join(utils.dirs.config_dir('pipelines'),
opts.pipeline_name) + ".yaml"
with open(pipe_file) as pf:
pipeline = cauv.yamlpipe.load(pf)
model = cauv.pipeline.Model(self.node, 'ai/_wall_lines')
pipeline.fixup_inputs()
#check the pipeline is responding, else raise error
if not model.isAlive():
raise Exception(
"Could not find appropriate image pipeline, required so giving up."
)
#set the pipeline
model.set(pipeline)
#setup initial values
#note that we modify the bearings so that the 'north' wall is exactly north
self.bearing = None # (corrected) bearing
self.real_bearing = None # (actual) bearing
self.image_scale = None # width of sonar image
self.wall_length = opts.wall_length
self.tolerance = opts.tolerance
self.bearing_correction = opts.arena_bearing_correction
self.scale_name = opts.scale_name
self.lines_name = opts.lines_name
self.node.addObserver(self)
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())
p_ResetMCBMessage = pp.Group(l \
+ r).streamline()
p_ResetMCBMessage.setParseAction(lambda x: messaging.ResetMCBMessage(*x[0]))
p_CalibrateNoRotationMessage = pp.Group(l \
+ p_int \
+ r).streamline()
p_CalibrateNoRotationMessage.setParseAction(lambda x: messaging.CalibrateNoRotationMessage(*x[0]))
p_StateMessage = pp.Group(l \
+ p_floatYPR \
+ r).streamline()
p_StateMessage.setParseAction(lambda x: messaging.StateMessage(*x[0]))
p_TelemetryMessage = pp.Group(l \
+ p_floatYPR + c \
+ p_float \
+ r).streamline()
p_TelemetryMessage.setParseAction(lambda x: messaging.TelemetryMessage(*x[0]))
p_BatteryUseMessage = pp.Group(l \
+ p_float + c \
+ p_float + c \
+ p_float \
+ r).streamline()
p_BatteryUseMessage.setParseAction(lambda x: messaging.BatteryUseMessage(*x[0]))
p_ProcessStatusMessage = pp.Group(l \
+ p_str + c \
+ p_str + c \
+ p_float + c \
+ p_float + c \
+ p_int \
+ r).streamline()
p_ProcessStatusMessage.setParseAction(lambda x: messaging.ProcessStatusMessage(*x[0]))
p_LocationMessage = pp.Group(l \
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))