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
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))
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))
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
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))
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))
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)
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)
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()))
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))