area-scanning-behaviour

#!/usr/bin/env python

import time

import boatdclient

from navigate import Navigator

#first two points must be the start line, next point must be the 
#point for the finish line
points = boatdclient.get_current_waypoints()
MAX_BOX_TIME = 30
TIME_TO_LEAVE = 10

class AreaScanningBehaviour(Navigator):
    def __init__(self):
        super(AreaScanningBehaviour, self).__init__(enable_tacking=True,
                                                    enable_cross_track_minimization=True)
        points.append(points[2])
        points.append(points[3])

        self.start_crossed = False
        self.enter_time = None
        self.have_timed_out = False

        # start point should be the fouth waypoints, but the waypoint number is
        # incremented before target is set
        self.current_point = 3
        self.waypoint_checkoff_distance = 7
        
        self.set_target((points[0]+points[1]) / 2)
                
        if self.boat.position.cross_track_distance(points[1], points[0]) < 0:
            self.start_negative = True
        else:
            self.start_negative = False

    def check_new_target(self):    
        print('current_target:', self.current_point, 'number of points:', len(points))
        if self.start_crossed is False:             
            if  (self.boat.position.cross_track_distance(points[1], points[0]) <= 0 and \
                self.start_negative is False) or (self.boat.position.cross_track_distance(points[1], \
                points[0]) >= 0 and self.start_negative is True):
                      
                print ('YOU HAVE CROSSED THE START ' * 20)
                self.start_crossed = True
                self.enter_time = time.time()
                return points[4]
            else:
                return None
                
        need_to_time_out = time.time() >= self.enter_time + (MAX_BOX_TIME - TIME_TO_LEAVE) * 60
        if self.start_crossed is True and need_to_time_out and not self.have_timed_out:
            self.have_timed_out = True
            self.current_point = len(points) - 2
            self.set_target(points[self.current_point])

        distance = self.boat.position.distance_to(self.target)
        if distance <= self.waypoint_checkoff_distance:
            print ('distance from point:', distance)
            self.current_point += 1
            return points[self.current_point]
        else:
            print ('distance to point', distance)
            return None


if __name__ == '__main__':
    behaviour = AreaScanningBehaviour()
    behaviour.run()