def moveGlobalWaypointUntilValid(state, isLast, otherWaypoint):
    '''Draws straight line between other and current global waypoint, then moves goalLatlon away from sailbot along this
    line until it is valid

    Args:
       state (BoatState): State of the sailbot and other boats
       isLast (boolean): True if current global waypoint is the last one, else False
       otherWaypoint (latlon): Previous global waypoint if isLast, else the next global waypoint

    Returns:
        goalLatlon (latlon) such that checkGoalValidity(state, newGoal) is true
    '''
    referenceLatlon = state.globalWaypoint
    goalLatlon = state.globalWaypoint
    goalX, goalY = utils.latlonToXY(goalLatlon, referenceLatlon)

    otherX, otherY = utils.latlonToXY(otherWaypoint, referenceLatlon)

    while not checkGoalValidity(state, goalLatlon):
        rospy.logwarn('goalLatlon ({},{}) not valid'.format(
            goalLatlon.lat, goalLatlon.lon))
        deltaX = goalX - otherX if isLast else otherX - goalX
        deltaY = goalY - otherY if isLast else otherY - goalY
        dist = math.sqrt(deltaX**2 + deltaY**2)

        goalX += MOVE_GOAL_WAYPOINT_STEP_SIZE_KM * deltaX / dist
        goalY += MOVE_GOAL_WAYPOINT_STEP_SIZE_KM * deltaY / dist
        goalLatlon = utils.XYToLatlon((goalX, goalY), referenceLatlon)
        rospy.logwarn('Moved goalLatlon to ({},{})'.format(
            goalLatlon.lat, goalLatlon.lon))

    return goalLatlon
Ejemplo n.º 2
0
    def test_getObstacles2(self):
        # Setup starting at (0,0) with obstacle at (1,1) heading south-west
        currentLatlon = latlon(0, 0)
        referenceLatlon = currentLatlon
        shipLatlon = utils.XYToLatlon(xy=(1, 1),
                                      referenceLatlon=referenceLatlon)
        ships = [
            AISShip(ID=1000,
                    lat=shipLatlon.lat,
                    lon=shipLatlon.lon,
                    headingDegrees=utils.HEADING_SOUTH,
                    speedKmph=1)
        ]
        state = sbot.BoatState(globalWaypoint=latlon(1, 1),
                               position=currentLatlon,
                               globalWindDirectionDegrees=0,
                               globalWindSpeedKmph=0,
                               AISData=AISMsg(ships),
                               headingDegrees=0,
                               speedKmph=1)
        obstacles = obs.getObstacles(state, referenceLatlon=referenceLatlon)

        # Uncomment below to see obstacles extended on a plot
        # self.plotObstacles(obstacles)

        self.assertFalse(utils.isValid(xy=[1, 1], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[0, 0], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[1, 0], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[0, 1], obstacles=obstacles))
Ejemplo n.º 3
0
    def test_nextWaypointReached_sameLon(self):
        # Setup path from (100,1) to (1,1)
        # Test passes when distance to next waypoint is less than or equal to WAYPOINT_REACHED_DISTANCE
        path = self.createPathWithGivenPositionsXY([[100, 1], [1, 1]])
        boundaryCoord = 1 + WAYPOINT_REACHED_DISTANCE

        # Test reachedPos
        reachedPos = utils.XYToLatlon(
            xy=(boundaryCoord - 0.01, -100),
            referenceLatlon=path.getReferenceLatlon())
        self.assertTrue(path.nextWaypointReached(reachedPos))

        # Test notReachedPos
        notReachedPos = utils.XYToLatlon(
            xy=(boundaryCoord + 0.01, 100),
            referenceLatlon=path.getReferenceLatlon())
        self.assertFalse(path.nextWaypointReached(notReachedPos))
Ejemplo n.º 4
0
 def _getLatlonsFromOMPLPath(self, omplPath):
     '''Convert solution path (in km WRT reference) into list of latlons'''
     path = []
     solutionPathObject = omplPath.getSolutionPath()
     referenceLatlon = omplPath.getReferenceLatlon()
     for state in solutionPathObject.getStates():
         xy = (state.getX(), state.getY())
         path.append(utils.XYToLatlon(xy, referenceLatlon))
     return path
Ejemplo n.º 5
0
def getTrailingBoatLatlon(GPS):
    headingRad = math.radians(GPS.headingDegrees)
    m = math.tan(headingRad)
    dx = TRAILING_DISTANCE_KM / (1 + m**2)**0.5

    if math.cos(headingRad) > 0:
        dx = -dx
    dy = m * dx

    return util.XYToLatlon([dx, dy], latlon(GPS.lat, GPS.lon))
Ejemplo n.º 6
0
    def test_getObstacles3(self):
        # Setup starting at (0,0) with obstacles at (0,3) heading south and at (-1,-1) heading north east
        currentLatlon = latlon(0, 0)
        referenceLatlon = currentLatlon
        shipLatlon = utils.XYToLatlon(xy=(0, 3),
                                      referenceLatlon=referenceLatlon)
        shipLatlon2 = utils.XYToLatlon(xy=(-1, -1),
                                       referenceLatlon=referenceLatlon)
        ship1 = AISShip(ID=1000,
                        lat=shipLatlon.lat,
                        lon=shipLatlon.lon,
                        headingDegrees=270,
                        speedKmph=1.5)
        ship2 = AISShip(ID=1001,
                        lat=shipLatlon2.lat,
                        lon=shipLatlon2.lon,
                        headingDegrees=45,
                        speedKmph=10)
        state = sbot.BoatState(globalWaypoint=latlon(1, 1),
                               position=currentLatlon,
                               globalWindDirectionDegrees=0,
                               globalWindSpeedKmph=0,
                               AISData=AISMsg([ship1, ship2]),
                               headingDegrees=0,
                               speedKmph=1)
        obstacles = obs.getObstacles(state, referenceLatlon=referenceLatlon)

        # Uncomment below to see obstacles extended on a plot
        # self.plotObstacles(obstacles)

        self.assertFalse(utils.isValid(xy=[0, 0], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[1, 1], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[2.5, 2.5], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[3, 3], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[0, -1], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[0, 4], obstacles=obstacles))
        self.assertTrue(utils.isValid(xy=[0, -2], obstacles=obstacles))
Ejemplo n.º 7
0
    def test_latlonToXY_and_XYToLatlon_basic(self):
        # Setup latlon
        testLatlon = latlon(50, -120)

        # Test latlonToXY
        x, y = utils.latlonToXY(latlon=testLatlon, referenceLatlon=testLatlon)
        self.assertAlmostEqual(x, 0, places=2)  # xy=(0,0) at reference point
        self.assertAlmostEqual(y, 0, places=2)

        # Test XYToLatlon
        calculatedLatlon = utils.XYToLatlon(xy=[x, y],
                                            referenceLatlon=testLatlon)
        # calculatedLatlon should be same as testLatlon
        self.assertAlmostEqual(calculatedLatlon.lat, testLatlon.lat, places=1)
        self.assertAlmostEqual(calculatedLatlon.lon, testLatlon.lon, places=1)
Ejemplo n.º 8
0
    def test_latlonToXY_and_XYToLatlon_advanced(self):
        # Setup latlons
        startLatlon = latlon(49.263022, -123.023447)
        endLatlon = latlon(47.7984, -125.3319)

        # Test latlonToXY
        x, y = utils.latlonToXY(latlon=endLatlon, referenceLatlon=startLatlon)
        # endLatlon is about 168.0158959716741km west of startLatlon
        self.assertAlmostEqual(x, -168.0158959716741, places=2)
        # endLatLon is about 162.8668880228988km south of startLatlon
        self.assertAlmostEqual(y, -162.8668880228988, places=2)

        # Test XYToLatlon
        calculatedEndLatlon = utils.XYToLatlon(xy=[x, y],
                                               referenceLatlon=startLatlon)
        # calculatedEndLatlon should be same as endLatlon
        self.assertAlmostEqual(calculatedEndLatlon.lat,
                               endLatlon.lat,
                               places=1)
        self.assertAlmostEqual(calculatedEndLatlon.lon,
                               endLatlon.lon,
                               places=1)
Ejemplo n.º 9
0
    def test_indexOfObstacleOnPath(self):
        '''To visualize the obstacleOnPath check, go to updated_geometric_planner.py
           and uncomment the plotting in indexOfObstacleOnPath()'''
        # Create simple path from latlon(0,0) to latlon(0.2,0.2)
        state = sbot.BoatState(globalWaypoint=latlon(0.2, 0.2),
                               position=latlon(0, 0),
                               globalWindDirectionDegrees=90,
                               globalWindSpeedKmph=10,
                               AISData=AISMsg(),
                               headingDegrees=0,
                               speedKmph=0)
        path = createPath(state, runtimeSeconds=1, numRuns=2)
        omplPath = path.getOMPLPath()
        '''
        Test the following:
        Let positionXY be between index 2 and 3 in the omplPath
        Thus, nextLocalWaypointIndex = 3
        Let numLookAheadWaypoints = 2
        Let B = sailbot. X = obstacle. Numbers = waypoint indices in omplPath.
        Scenario 0: 0     1     2  B X3     4     5     6     7  => Returns 3  (1 index forward from B)
        Scenario 1: 0     1     2  B  3  X  4     5     6     7  => Returns 4  (2 indices forward from B)
        Scenario 2: 0     1     2  B  3     4  X  5     6     7  => Returns -1 (Nothing btwn B->3->4, look ahead 2)
        Scenario 3: 0     1     2X B  3     4     5     6     7  => Returns -1 (Nothing btwn B->3->4, not look behind)
        Scenario 4: 0     1     2 XBX 3     4     5     6     7  => Returns 0 (B invalidState)
        '''
        # Get waypoints setup
        waypoint2 = path.getOMPLPath().getSolutionPath().getState(2)
        waypoint3 = path.getOMPLPath().getSolutionPath().getState(3)
        waypoint4 = path.getOMPLPath().getSolutionPath().getState(4)
        waypoint5 = path.getOMPLPath().getSolutionPath().getState(5)

        # Get in between positions
        between2and3XY = [
            waypoint2.getX() + (waypoint3.getX() - waypoint2.getX()) / 2,
            waypoint2.getY() + (waypoint3.getY() - waypoint2.getY()) / 2
        ]
        closeTo2XY = [
            waypoint2.getX() + (waypoint3.getX() - waypoint2.getX()) / 5,
            waypoint2.getY() + (waypoint3.getY() - waypoint2.getY()) / 5
        ]
        closeTo3XY = [
            waypoint2.getX() + (waypoint3.getX() - waypoint2.getX()) * 4 / 5,
            waypoint2.getY() + (waypoint3.getY() - waypoint2.getY()) * 4 / 5
        ]
        between3and4XY = [
            waypoint3.getX() + (waypoint4.getX() - waypoint3.getX()) / 2,
            waypoint3.getY() + (waypoint4.getY() - waypoint3.getY()) / 2
        ]
        between4and5XY = [
            waypoint4.getX() + (waypoint5.getX() - waypoint4.getX()) / 2,
            waypoint4.getY() + (waypoint5.getY() - waypoint4.getY()) / 2
        ]

        # Setup values from tests
        sailbotPositionXY = between2and3XY
        nextLocalWaypointIndex = 3
        numLookAheadWaypoints = 2

        # Scenario 0
        closeTo3Latlon = utils.XYToLatlon(closeTo3XY,
                                          path.getReferenceLatlon())
        state.AISData = AISMsg([
            AISShip(ID=0,
                    lat=closeTo3Latlon.lat,
                    lon=closeTo3Latlon.lon,
                    headingDegrees=utils.HEADING_EAST,
                    speedKmph=0)
        ])
        omplPath.updateObstacles(state)
        self.assertEqual(
            3,
            indexOfObstacleOnPath(
                positionXY=sailbotPositionXY,
                nextLocalWaypointIndex=nextLocalWaypointIndex,
                numLookAheadWaypoints=numLookAheadWaypoints,
                omplPath=path.getOMPLPath()))

        # Scenario 1
        between3and4Latlon = utils.XYToLatlon(between3and4XY,
                                              path.getReferenceLatlon())
        state.AISData = AISMsg([
            AISShip(ID=0,
                    lat=between3and4Latlon.lat,
                    lon=between3and4Latlon.lon,
                    headingDegrees=utils.HEADING_EAST,
                    speedKmph=0)
        ])
        omplPath.updateObstacles(state)
        self.assertEqual(
            4,
            indexOfObstacleOnPath(
                positionXY=sailbotPositionXY,
                nextLocalWaypointIndex=nextLocalWaypointIndex,
                numLookAheadWaypoints=numLookAheadWaypoints,
                omplPath=path.getOMPLPath()))

        # Scenario 2
        between4and5Latlon = utils.XYToLatlon(between4and5XY,
                                              path.getReferenceLatlon())
        state.AISData = AISMsg([
            AISShip(ID=0,
                    lat=between4and5Latlon.lat,
                    lon=between4and5Latlon.lon,
                    headingDegrees=utils.HEADING_EAST,
                    speedKmph=0)
        ])
        omplPath.updateObstacles(state)
        self.assertEqual(
            -1,
            indexOfObstacleOnPath(
                positionXY=sailbotPositionXY,
                nextLocalWaypointIndex=nextLocalWaypointIndex,
                numLookAheadWaypoints=numLookAheadWaypoints,
                omplPath=path.getOMPLPath()))

        # Scenario 3
        closeTo2Latlon = utils.XYToLatlon(closeTo2XY,
                                          path.getReferenceLatlon())
        state.AISData = AISMsg([
            AISShip(ID=0,
                    lat=closeTo2Latlon.lat,
                    lon=closeTo2Latlon.lon,
                    headingDegrees=utils.HEADING_EAST,
                    speedKmph=0)
        ])
        omplPath.updateObstacles(state)
        self.assertEqual(
            -1,
            indexOfObstacleOnPath(
                positionXY=sailbotPositionXY,
                nextLocalWaypointIndex=nextLocalWaypointIndex,
                numLookAheadWaypoints=numLookAheadWaypoints,
                omplPath=path.getOMPLPath()))

        # Scenario 4
        between2and3Latlon = utils.XYToLatlon(between2and3XY,
                                              path.getReferenceLatlon())
        state.AISData = AISMsg([
            AISShip(ID=0,
                    lat=between2and3Latlon.lat,
                    lon=between2and3Latlon.lon,
                    headingDegrees=utils.HEADING_EAST,
                    speedKmph=0)
        ])
        omplPath.updateObstacles(state)
        self.assertEqual(
            0,
            indexOfObstacleOnPath(
                positionXY=sailbotPositionXY,
                nextLocalWaypointIndex=nextLocalWaypointIndex,
                numLookAheadWaypoints=numLookAheadWaypoints,
                omplPath=path.getOMPLPath()))
Ejemplo n.º 10
0
    def test_obstacleOnPath(self):
        '''To visualize the obstacleOnPath check, go to updated_geometric_planner.py
           and uncomment the plotting in indexOfObstacleOnPath()'''
        # Create simple path from latlon(0,0) to latlon(0.2,0.2)
        state = sbot.BoatState(globalWaypoint=latlon(0.2, 0.2),
                               position=latlon(0, 0),
                               globalWindDirectionDegrees=90,
                               globalWindSpeedKmph=10,
                               AISData=AISMsg(),
                               headingDegrees=0,
                               speedKmph=0)
        path = createPath(state, runtimeSeconds=1, numRuns=2)

        waypoint0 = path.getOMPLPath().getSolutionPath().getState(0)
        waypoint1 = path.getOMPLPath().getSolutionPath().getState(1)

        # No obstacles at all
        self.assertFalse(path.obstacleOnPath(state))

        # Put obstacle on path between waypoints 0 and 1, going east quickly
        between0and1XY = [
            waypoint0.getX() + (waypoint1.getX() - waypoint0.getX()) / 2,
            waypoint0.getY() + (waypoint1.getY() - waypoint0.getY()) / 2
        ]
        between0and1Latlon = utils.XYToLatlon(between0and1XY,
                                              path.getReferenceLatlon())
        state.AISData = AISMsg([
            AISShip(ID=0,
                    lat=between0and1Latlon.lat,
                    lon=between0and1Latlon.lon,
                    headingDegrees=utils.HEADING_EAST,
                    speedKmph=0)
        ])

        # Obstacle on path
        self.assertTrue(path.obstacleOnPath(state))

        # Only look ahead 1 waypoint, but still has obstacle on path
        # (defaults to all waypoints if numLookAheadWaypoints not given)
        self.assertTrue(path.obstacleOnPath(state, numLookAheadWaypoints=1))

        # Move obstacle to be off the path
        between0and1shiftedRightXY = [
            waypoint0.getX() + (waypoint1.getX() - waypoint0.getX()) * 3 / 4,
            waypoint0.getY() + (waypoint1.getY() - waypoint0.getY()) / 2
        ]
        between0and1shiftedRightLatlon = utils.XYToLatlon(
            between0and1shiftedRightXY, path.getReferenceLatlon())
        state.AISData = AISMsg([
            AISShip(ID=0,
                    lat=between0and1shiftedRightLatlon.lat,
                    lon=between0and1shiftedRightLatlon.lon,
                    headingDegrees=utils.HEADING_EAST,
                    speedKmph=0)
        ])
        self.assertFalse(path.obstacleOnPath(state))
        ''' Tests not working, need to investigate or remove
        # Move boat position, so that new "path" has an obstacle on it
        waypoint0Latlon = utils.XYToLatlon([waypoint0.getX(), waypoint0.getY()], path.getReferenceLatlon())
        waypoint1Latlon = utils.XYToLatlon([waypoint1.getX(), waypoint1.getY()], path.getReferenceLatlon())
        state.position = latlon(waypoint0Latlon.lat, waypoint1Latlon.lon)
        self.assertTrue(path.obstacleOnPath(state))

        # Move boat position, so that new "path" does not have an obstacle on it
        state.position = latlon(waypoint1Latlon.lat, waypoint0Latlon.lon)
        self.assertFalse(path.obstacleOnPath(state))

        waypoint2 = path.getOMPLPath().getSolutionPath().getState(2)
        between1and2XY = [waypoint1.getX() + (waypoint2.getX() - waypoint1.getX()) / 2,
                          waypoint1.getY() + (waypoint2.getY() - waypoint1.getY()) / 2]
        between1and2Latlon = utils.XYToLatlon(between1and2XY, path.getReferenceLatlon())
        state.AISData = AISMsg([AISShip(ID=0, lat=between1and2Latlon.lat, lon=between1and2Latlon.lon,
                                        headingDegrees=utils.HEADING_EAST, speedKmph=0)])

        # Obstacle on path
        self.assertTrue(path.obstacleOnPath(state))
        '''

        # Only look ahead 1 waypoint, so does not have obstacle on path
        # (defaults to all waypoints if numLookAheadWaypoints not given)
        self.assertFalse(path.obstacleOnPath(state, numLookAheadWaypoints=1))