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 run(self): #head to start time.sleep(2) if self.options.useDepth: self.auv.depth(self.options.depth) #self.auv.headToLocation(self.options.initialLocation) self.auv.bearingAndWait(self.options.initialBearing) #start onMessage handler start_time = time.time() self.node.subMessage(msg.LinesMessage()) #this allows dynamic setting of running time while time.time() - start_time < self.options.maximumRunTime: time.sleep(1)
def __init__(self): event.EventLoop.__init__(self) AI.Script.__init__(self) self.load_pipeline(self.options.Pipeline_Name) self.node.subMessage(msg.CirclesMessage()) self.node.addObserver(self) self.log('Looking for the buoy') self.circles = deque(maxlen = self.options.Max_Samples) self.times = deque(maxlen = self.options.Max_Samples) self.foundBuoy = False if self.options.useDepth: self.auv.depth(self.options.depth) self.auv.bearingAndWait(self.options.initialBearing) print('Prepping to go to subMessage') self.node.subMessage(msg.LinesMessage()) print('Past submessage') self.found = False
+ p_str + c \ + p_int \ + r).streamline() p_ForceExecRequestMessage.setParseAction(lambda x: messaging.ForceExecRequestMessage(*x[0])) p_PipelineDiscoveryRequestMessage = pp.Group(l \ + r).streamline() p_PipelineDiscoveryRequestMessage.setParseAction(lambda x: messaging.PipelineDiscoveryRequestMessage(*x[0])) p_PipelineDiscoveryResponseMessage = pp.Group(l \ + p_str \ + r).streamline() p_PipelineDiscoveryResponseMessage.setParseAction(lambda x: messaging.PipelineDiscoveryResponseMessage(*x[0])) p_LinesMessage = pp.Group(l \ + p_str + c \ + p_LineVec \ + r).streamline() p_LinesMessage.setParseAction(lambda x: messaging.LinesMessage(*x[0])) p_CirclesMessage = pp.Group(l \ + p_str + c \ + p_CircleVec \ + r).streamline() p_CirclesMessage.setParseAction(lambda x: messaging.CirclesMessage(*x[0])) p_CornersMessage = pp.Group(l \ + p_str + c \ + p_CornerVec \ + r).streamline() p_CornersMessage.setParseAction(lambda x: messaging.CornersMessage(*x[0])) p_KeyPointsMessage = pp.Group(l \ + p_str + c \ + p_KeyPointVec \ + r).streamline() p_KeyPointsMessage.setParseAction(lambda x: messaging.KeyPointsMessage(*x[0]))