def find_buckets(self): ''' Looking for buckets firstly based on pinger, altenatively by vision system ''' self._logger.log("Starting searching buckets task") angle_to_pinger = self._hydrophones.get_angle(self.PINGER_FREQ) if angle_to_pinger: bbox = False i = 0 while bbox is not True and i < 5: self._control.rotate_angle(yaw=angle_to_pinger) self._control.set_lin_velocity(front=50) sleep(self.SEARCHING_BUCKETS_FORWARD_TIME) self._control.set_lin_velocity(front=0) bbox = self.darknet_client.predict()[0].normalize(480, 480) angle_to_pinger = self._hydrophones._hydrophones.get_angle( self.PINGER_FREQ) i += 1 if bbox: center_rov(self._control, Bbox=bbox) self._logger.log("Buckets task found") return 1 else: self._logger.log("Starting desparate algorythm") for i in range(18): self._control.rotate_angle(yaw=20) bbox = self.darknet_client.predict()[0].normalize(480, 480) if bbox: center_rov(self._control, Bbox=bbox) self._logger.log("Buckets task found") return 1 self._logger.log("Searching buckets task failed") return 0
def find_pinger_bucket(self): ''' After localisation of carpet and buckets finding exact position of bucket with pinger ''' self._logger.log("Locating exact position of pinger bucket") angle_to_pinger = self._hydrophones.get_angle(self.PINGER_FREQ) bbox = None if angle_to_pinger: i = 0 while bbox is None and i < 5: self._control.rotate_angle(angle_to_pinger) sleep(2) bbox = self.darknet_client.predict()[0].normalize(480, 480) angle_to_pinger = self._hydrophones.get_angle(self.PINGER_FREQ) i += 1 if bbox is None: self._logger.log("Finding pinger bucke failed") return 0 else: control = self._control center_rov(control, Bbox=bbox) self._control.set_lin_velocity(front=20) if self.center_above_bucket(): return 1 else: self._logger.log("Could not center above bucket") return 0 else: self._logger.log("Could not receive signal from hydrophones") return 0
def run(self): #self.darknet_client.load_model('coke') self._logger.log("model loaded") bbox = False while not bbox: bbox = self.darknet_client.predict() #TODO sleep(0.1) self._logger.log("out of predicting loop") bbox =bbox[0].normalize(480, 480) position_x = bbox.xc #TO DO - returning bbox instead of list position_y = bbox.yc self._logger.log("poz: x="+str(position_x)+" y="+str(position_y)) while abs(position_x) > 0.05 or abs(position_y) > 0.05: self._logger.log("in ceentring") bbox = self.darknet_client.predict() sleep(0.2) if bbox: bbox =bbox[0].normalize(480, 480) #sleep(0.01) position_x = bbox.xc #TO DO - returning bbox instead of list position_y = bbox.yc self._logger.log("pos: x="+str(position_x)+" y="+str(position_y)) center_rov(move = self._control,Bbox = bbox, depth_sensor = self.depth_sensor,logger = self._logger)
def center_on_gate(self): config = self.config['centering'] MAX_TIME_SEC = config['max_time_sec'] MAX_CENTER_DISTANCE = config['max_center_distance'] stopwatch = Stopwatch() stopwatch.start() while stopwatch.time() <= MAX_TIME_SEC: bbox = self.darknet_client.predict()[0].normalize(480, 480) if bbox.x <= MAX_CENTER_DISTANCE & bbox.y <= MAX_CENTER_DISTANCE: self._logger.log("centered on gate successfully") return True center_rov(move=self._control, Bbox=bbox, depth_sensor=self.depth_sensor) self._logger.log("couldn't center on gate") return False
def find_random_bucket(self): ''' Finding exact position of random bucket ''' self._logger.log("Searching random bucket") angle = 0 bbox = None control = self._control for i in range(18): angle += i * 20 bbox = self.darknet_client.predict()[0].normalize(480, 480) if bbox: center_rov(control, Bbox=bbox) self._control.set_lin_velocity(front=20) if self.center_above_bucket(): return 1 else: self._logger.log("Could not center above bucket") return 0 self._control.rotate_angle(yaw=20) sleep(2)