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)