def test_createPath_advanced(self):
# Create tacking path (requires tacking because of wind is 45 degrees, goal is 45 degrees)
start = latlon(0, 0)
goal = latlon(0.2, 0.2)
state = sbot.BoatState(globalWaypoint=goal,
position=start,
globalWindDirectionDegrees=45,
globalWindSpeedKmph=10,
AISData=AISMsg(),
headingDegrees=0,
speedKmph=0)
path = createPath(state,
runtimeSeconds=0.5,
numRuns=2,
maxAllowableRuntimeSeconds=1)
latlons = path.getLatlons()
# Check that first state matches the setup start
startWaypoint = latlons[0]
self.assertAlmostEqual(start.lat, startWaypoint.lat, places=2)
self.assertAlmostEqual(start.lon, startWaypoint.lon, places=2)
# Check that the path reaches the goal
self.assertTrue(path.reachesGoalLatlon(goal))
# Check that total path length is reasonable
maxAcceptablePathLength = 2 * distance((start.lat, start.lon),
(goal.lat, goal.lon)).kilometers
self.assertLessEqual(path.getLength(), maxAcceptablePathLength)
# Check that path cost is acceptable
self.assertFalse(utils.pathCostThresholdExceeded(path))
def createPathWithGivenPositionsXY(self, xys):
def createState(spaceInformation, xy):
state = spaceInformation.allocState()
state.setXY(xy[0], xy[1])
return state
# Create dummy path and change private variables directly
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)
spaceInformation = path.getSpaceInformation()
# Create acutal path
actualPath = og.PathGeometric(spaceInformation)
for xy in xys:
actualPath.append(createState(spaceInformation, xy))
path.getOMPLPath()._solutionPath = actualPath
path._latlons = path._getLatlonsFromOMPLPath(path._omplPath)
return path
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 MOCK_global():
global boatLatlon
rospy.init_node('MOCK_global_planner', anonymous=True)
pub = rospy.Publisher("globalPath", msg.path, queue_size=4)
r = rospy.Rate(float(1) / PUBLISH_PERIOD_SECONDS) # Hz
# Subscribe to GPS to publish new global paths based on boat position
rospy.Subscriber("GPS", msg.GPS, gpsCallback)
# Wait to get boat position
while boatLatlon is None:
if rospy.is_shutdown():
rospy.loginfo("rospy.is_shutdown() is True. Exiting")
sys.exit()
rospy.loginfo("Waiting for boat GPS")
time.sleep(1)
rospy.loginfo("Received boat GPS")
init = msg.latlon(lat=boatLatlon.lat, lon=boatLatlon.lon)
# Create goal
goal_file = rospy.get_param('goal_file', default=None)
if goal_file:
with open(goal_file) as f:
record = json.loads(f.read())
lat = record[0]
lon = record[1]
goal = msg.latlon(lat=lat, lon=lon)
else:
goal = getGoalLatlon(defaultLat=MAUI_LATLON.lat,
defaultLon=MAUI_LATLON.lon)
path = create_path(init, goal)
# Publish new global path periodically
# Publish new global path more often with speedup
republish_counter = 0
numPublishPeriodsPerUpdate = int(NEW_GLOBAL_PATH_PERIOD_SECONDS /
PUBLISH_PERIOD_SECONDS)
while not rospy.is_shutdown():
# Send updated global path
if republish_counter >= numPublishPeriodsPerUpdate:
republish_counter = 0
init = msg.latlon(lat=boatLatlon.lat, lon=boatLatlon.lon)
goal = getGoalLatlon(defaultLat=goal.lat, defaultLon=goal.lon)
path = create_path(init, goal)
else:
speedup = rospy.get_param('speedup', default=1.0)
republish_counter += speedup
pub.publish(msg.path(path))
r.sleep()
def test_getDesiredHeadingDegrees_east(self):
# Setup latlon
position = latlon(50, -120)
# Setup destination
eastDestination = distance(kilometers=1).destination(
point=(position.lat, position.lon), bearing=utils.BEARING_EAST)
eastDestination = latlon(eastDestination.latitude,
eastDestination.longitude)
# Test desiredHeading
desiredHeading = utils.getDesiredHeadingDegrees(
position=position, localWaypoint=eastDestination)
self.assertAlmostEqual(desiredHeading, utils.HEADING_EAST, places=1)
def test_basic(self):
# Setup messages
gpsMsg = msg.GPS(10.1, 4.2, 12.4 / utils.KNOTS_TO_KMPH,
utils.headingToBearingDegrees(60.3), 12.4, 60.3)
globalWindMsg = msg.globalWind(87.5, 1.89)
ships = [msg.AISShip(i, i, i, i, i) for i in range(10)]
AISMsg = msg.AISMsg(ships)
waypoints = [msg.latlon(i, i) for i in range(10)]
globalPathMsg = msg.path(waypoints)
# Publish messages
self.gpsPublisher.publish(gpsMsg)
self.globalWindPublisher.publish(globalWindMsg)
self.AISPublisher.publish(AISMsg)
self.globalPathPublisher.publish(globalPathMsg)
rospy.sleep(1)
# Check that currentState has been updated
state = self.sailbot.getCurrentState()
self.assertAlmostEqual(state.position.lat, gpsMsg.lat, places=3)
self.assertAlmostEqual(state.position.lon, gpsMsg.lon, places=3)
self.assertAlmostEqual(state.headingDegrees,
gpsMsg.headingDegrees,
places=3)
self.assertAlmostEqual(state.speedKmph, gpsMsg.speedKmph, places=3)
self.assertAlmostEqual(state.globalWindDirectionDegrees,
globalWindMsg.directionDegrees,
places=3)
self.assertAlmostEqual(state.globalWindSpeedKmph,
globalWindMsg.speedKmph,
places=3)
self.assertEqual(len(state.AISData.ships), len(ships))
self.assertEqual(len(self.sailbot.globalPath), len(waypoints))
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_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_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 XYToLatlon(xy, referenceLatlon):
'''Calculate the latlon coordinates of the given xy, given the referenceLatlon located at (0,0)
Args:
xy (list of two floats): The xy (km) coordinates whose latlon coordinates will be calculated.
referenceLatlon (sailbot_msg.msg._latlon.latlon): The latlon that will be located at (0,0) wrt xy.
Returns:
sailbot_msg.msg._latlon.latlon representing the position of xy in latlon coordinates
'''
x_distance = distance(kilometers=xy[0])
y_distance = distance(kilometers=xy[1])
destination = x_distance.destination(point=(referenceLatlon.lat,
referenceLatlon.lon),
bearing=BEARING_EAST)
destination = y_distance.destination(point=(destination.latitude,
destination.longitude),
bearing=BEARING_NORTH)
return latlon(destination.latitude, destination.longitude)
def _getNextWaypoint(path, pathIndex):
# If path is empty, return (0, 0)
if len(path) == 0:
rospy.logwarn("Path is empty.")
rospy.logwarn("Setting next waypoint to be (0, 0).")
return latlon(0, 0)
# If index out of range, return last waypoint in path
if pathIndex >= len(path):
rospy.logwarn(
"Path index is out of range: index = {} len(path) = {}".
format(pathIndex, len(path)))
rospy.logwarn(
"Setting next waypoint to be the last element of the path")
pathIndex = len(path) - 1
return path[pathIndex]
# If index in range, return the correct waypoint
else:
return path[pathIndex]
def create_path(init, goal):
path = []
# Insert the initial position
path.append(init)
# Just do some linear interpolation
total_distance_km = distance((init.lat, init.lon),
(goal.lat, goal.lon)).kilometers
num_global_waypoints = int(
round(total_distance_km / AVG_WAYPOINT_DISTANCE_KM))
for i in range(1, num_global_waypoints):
coeff = float(i) / (num_global_waypoints)
lat = (1 - coeff) * init.lat + coeff * goal.lat
lon = (1 - coeff) * init.lon + coeff * goal.lon
wp = msg.latlon(lat=lat, lon=lon)
path.append(wp)
# Insert the goal
path.append(goal)
return path
def get_real_ships(self):
lat = self.sailbot_lat
lon = self.sailbot_lon
pos = latlon(lat, lon)
bot_left = XYToLatlon([-30.0, -30.0], pos)
top_left = XYToLatlon([-30.0, 30.0], pos)
top_right = XYToLatlon([30.0, 30.0], pos)
bot_right = XYToLatlon([30.0, -30.0], pos)
space = "%20"
pos_list = ""
pos_list += str(bot_left.lon) + space + str(bot_left.lat) + space
pos_list += str(top_left.lon) + space + str(top_left.lat) + space
pos_list += str(top_right.lon) + space + str(top_right.lat) + space
pos_list += str(bot_right.lon) + space + str(bot_right.lat) + space
pos_list += str(bot_left.lon) + space + str(bot_left.lat)
url = 'https://services.exactearth.com/gws/wms?service=WFS&version=1.1.0&authKey=' + self.token
url += '&request=GetFeature&typeName=exactAIS:LVI&outputFormat=json&'
url += 'filter=<Filter%20xmlns:gml="http://www.opengis.net/gml">'
url += '<Intersects><PropertyName>position</PropertyName>'
url += '<gml:Polygon%20xmlns:gml="http://www.opengis.net/gml"%20srsName="EPSG:4326">'
url += '<gml:exterior><gml:LinearRing><gml:posList>' + pos_list
url += '</gml:posList></gml:LinearRing></gml:exterior></gml:Polygon></Intersects></Filter>'
request = urllib2.Request(url)
print("Querying the server")
data = urllib2.urlopen(request)
data = data.read()
if data.startswith('{\n"errors"'):
print("Queried the server too often, waiting for 2 minutes")
print(data)
return
ships = json.loads(data)
self.real_ships = []
self.ais_ships_log = []
for real_ship in ships['features']:
# Speed is given in knots
real_ship = real_ship[u'properties']
new_ship = RealShip(
int(real_ship[u'mmsi']),
float(real_ship[u'latitude']),
float(real_ship[u'longitude']),
float(real_ship[u'cog']), # Should be 0 North, 90 East
float(real_ship[u'sog']) * 0.54)
self.real_ships.append(new_ship)
log_ship = {
'ship_class': {
'id': real_ship[u'mmsi'],
'lat': real_ship[u'latitude'],
'lon': real_ship[u'longitude'],
'headingDegrees':
float(real_ship[u'cog']), # Should be 0 North, 90 East
'speedKmph': float(real_ship[u'sog']) * 0.54
},
'length': real_ship[u'length'],
'width': real_ship[u'width']
}
self.ais_ships_log.append(log_ship)
if self.log_exactais:
self.log_ais_ships()
#!/usr/bin/env python
from datetime import datetime
from datetime import date
import os
import rospy
import time
import math
import numpy as np
import planner_helpers as plans
from geopy.distance import distance
from sailbot_msg.msg import latlon
# Location constants
PORT_RENFREW_LATLON = latlon(48.5, -124.8)
MAUI_LATLON = latlon(20.0, -156.0)
# Constants
PATH_UPDATE_TIME_LIMIT_SECONDS = 7200
AVG_DISTANCE_BETWEEN_LOCAL_WAYPOINTS_KM = 3.0
PI_DEG = 180.0
KNOTS_TO_KMPH = 1.852
# Scale NUM_LOOK_AHEAD_WAYPOINTS_FOR_OBSTACLES and NUM_LOOK_AHEAD_WAYPOINTS_FOR_UPWIND_DOWNWIND to change based on
# waypoint distance
# "As far as the eye can see" is ~3 miles = ~5 km
LOOK_AHEAD_FOR_OBSTACLES_KM = 5.0
NUM_LOOK_AHEAD_WAYPOINTS_FOR_OBSTACLES = int(
math.ceil(LOOK_AHEAD_FOR_OBSTACLES_KM /
AVG_DISTANCE_BETWEEN_LOCAL_WAYPOINTS_KM))
LOOK_AHEAD_FOR_UPWIND_DOWNWIND_KM = 5.0
NUM_LOOK_AHEAD_WAYPOINTS_FOR_UPWIND_DOWNWIND = int(
def getGoalLatlon(defaultLat, defaultLon):
goalLat = get_rosparam_or_default_if_invalid('goal_lat',
default=defaultLat)
goalLon = get_rosparam_or_default_if_invalid('goal_lon',
default=defaultLon)
return msg.latlon(lat=goalLat, lon=goalLon)
def gpsCallback(data):
global boatLatlon
boatLatlon = msg.latlon(lat=data.lat, lon=data.lon)
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 GPSCallback(self, data):
self.position = latlon(data.lat, data.lon)
self.headingDegrees = data.headingDegrees
self.speedKmph = data.speedKmph
self.GPSLastUpdate = time.time()
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))
def test_upwindOrDownwindOnPath(self):
# 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)
desiredHeadingDegrees = utils.getDesiredHeadingDegrees(
state.position, path.getNextWaypoint())
# Set state with global wind direction nearly same as boat current direction (sailing downwind)
downwindGlobalWindDirectionDegrees = desiredHeadingDegrees + DOWNWIND_MAX_ANGLE_DEGREES / 2
downwindState = sbot.BoatState(
globalWaypoint=latlon(0.2, 0.2),
position=latlon(0, 0),
globalWindDirectionDegrees=downwindGlobalWindDirectionDegrees,
globalWindSpeedKmph=10,
AISData=AISMsg(),
headingDegrees=120,
speedKmph=1)
# Needs to maintain downwind for time limit before returning true
self.assertFalse(
path.upwindOrDownwindOnPath(downwindState,
numLookAheadWaypoints=1))
time.sleep(UPWIND_DOWNWIND_TIME_LIMIT_SECONDS * 1.5)
self.assertTrue(
path.upwindOrDownwindOnPath(downwindState,
numLookAheadWaypoints=1))
# Set state with global wind direction nearly 180 degrees from boat current direction (sailing upwind)
upwindGlobalWindDirectionDegrees = desiredHeadingDegrees + 180 + UPWIND_MAX_ANGLE_DEGREES / 2
upwindState = sbot.BoatState(
globalWaypoint=latlon(0.2, 0.2),
position=latlon(0, 0),
globalWindDirectionDegrees=upwindGlobalWindDirectionDegrees,
globalWindSpeedKmph=10,
AISData=AISMsg(),
headingDegrees=120,
speedKmph=1)
self.assertFalse(
path.upwindOrDownwindOnPath(upwindState, numLookAheadWaypoints=1))
time.sleep(UPWIND_DOWNWIND_TIME_LIMIT_SECONDS * 1.5)
self.assertTrue(
path.upwindOrDownwindOnPath(upwindState, numLookAheadWaypoints=1))
# Set state so the boat is not going downwind or upwind
validGlobalWindDirectionDegrees = desiredHeadingDegrees + DOWNWIND_MAX_ANGLE_DEGREES * 2
validState = sbot.BoatState(
globalWaypoint=latlon(0.2, 0.2),
position=latlon(0, 0),
globalWindDirectionDegrees=validGlobalWindDirectionDegrees,
globalWindSpeedKmph=10,
AISData=AISMsg(),
headingDegrees=120,
speedKmph=1)
self.assertFalse(
path.upwindOrDownwindOnPath(validState, numLookAheadWaypoints=1))
time.sleep(UPWIND_DOWNWIND_TIME_LIMIT_SECONDS * 1.5)
self.assertFalse(
path.upwindOrDownwindOnPath(validState, numLookAheadWaypoints=1))
# Move boat so that boat is not going downwind or upwind
newPosition = latlon(0.2, 0)
desiredHeadingDegrees = utils.getDesiredHeadingDegrees(
newPosition, path.getNextWaypoint())
validGlobalWindDirectionDegrees = desiredHeadingDegrees + UPWIND_MAX_ANGLE_DEGREES * 2
validState = sbot.BoatState(
globalWaypoint=latlon(0.2, 0.2),
position=newPosition,
globalWindDirectionDegrees=validGlobalWindDirectionDegrees,
globalWindSpeedKmph=10,
AISData=AISMsg(),
headingDegrees=120,
speedKmph=1)
self.assertFalse(
path.upwindOrDownwindOnPath(validState, numLookAheadWaypoints=1))
time.sleep(UPWIND_DOWNWIND_TIME_LIMIT_SECONDS * 1.5)
self.assertFalse(
path.upwindOrDownwindOnPath(validState, numLookAheadWaypoints=1))
# Move boat so that boat is going downwind
newPosition = latlon(0, 0.2)
desiredHeadingDegrees = utils.getDesiredHeadingDegrees(
newPosition, path.getNextWaypoint())
downwindGlobalWindDirectionDegrees = desiredHeadingDegrees + DOWNWIND_MAX_ANGLE_DEGREES / 2
invalidState = sbot.BoatState(
globalWaypoint=latlon(0.2, 0.2),
position=newPosition,
globalWindDirectionDegrees=downwindGlobalWindDirectionDegrees,
globalWindSpeedKmph=10,
AISData=AISMsg(),
headingDegrees=120,
speedKmph=1)
self.assertFalse(
path.upwindOrDownwindOnPath(invalidState, numLookAheadWaypoints=1))
time.sleep(UPWIND_DOWNWIND_TIME_LIMIT_SECONDS * 1.5)
self.assertTrue(
path.upwindOrDownwindOnPath(invalidState, numLookAheadWaypoints=1))
#!/usr/bin/env python
import rospy
import math
import utilities as util
from sailbot_msg.msg import AISShip, latlon, addBoat, GPS, path, AISMsg
# Globals for callbacks
localWaypoint = latlon()
gps = latlon()
localPath = []
TRAILING_ADDED_KMPH = 20
TRAILING_DISTANCE_KM = 5
def command_callback(msg):
ships = rospy.wait_for_message('/AIS', AISMsg).ships
newShipID = max([ship.ID for ship in ships]) + 1
if msg.addType == "latlon":
add_pub.publish(msg.ship)
if msg.addType == "nextWaypoint":
add_pub.publish(
AISShip(newShipID, localWaypoint.lat, localWaypoint.lon,
msg.ship.headingDegrees, msg.ship.speedKmph))
if msg.addType == "onBoat":
add_pub.publish(
AISShip(newShipID, gps.lat, gps.lon, msg.ship.headingDegrees,
msg.ship.speedKmph))
if msg.addType == "index":
if msg.waypointIndex < len(localPath) and msg.waypointIndex >= 0:
def test_globalPath_detailed(self):
# Setup non global path messages
gpsMsg = msg.GPS(10.1, 4.2, 12.4 / utils.KNOTS_TO_KMPH,
utils.headingToBearingDegrees(60.3), 12.4, 60.3)
globalWindMsg = msg.globalWind(87.5, 1.89)
ships = [msg.AISShip(i, i, i, i, i) for i in range(10)]
AISMsg = msg.AISMsg(ships)
# Publish non global path messages
self.gpsPublisher.publish(gpsMsg)
self.globalWindPublisher.publish(globalWindMsg)
self.AISPublisher.publish(AISMsg)
rospy.sleep(1)
# Setup globalPath message
numWaypoints = 10
waypoints = [msg.latlon(i, i) for i in range(numWaypoints)]
globalPathMsg = msg.path(waypoints)
self.globalPathPublisher.publish(globalPathMsg)
rospy.sleep(0.1)
# Check that sailbot received the new path
self.assertEqual(len(self.sailbot.globalPath), numWaypoints)
# Check that currentState has been updated next global waypoint should be
# the second element of the waypoints list
state = self.sailbot.getCurrentState()
self.assertAlmostEqual(state.globalWaypoint.lat,
waypoints[1].lat,
places=3)
self.assertAlmostEqual(state.globalWaypoint.lon,
waypoints[1].lon,
places=3)
# Check that globalIndex increment works
self.sailbot.globalPathIndex += 1
state = self.sailbot.getCurrentState()
self.assertAlmostEqual(state.globalWaypoint.lat,
waypoints[2].lat,
places=3)
self.assertAlmostEqual(state.globalWaypoint.lon,
waypoints[2].lon,
places=3)
# Check that receiving the same path doesn't update the index to 1
self.globalPathPublisher.publish(globalPathMsg)
rospy.sleep(0.1)
state = self.sailbot.getCurrentState()
self.assertAlmostEqual(state.globalWaypoint.lat,
waypoints[2].lat,
places=3)
self.assertAlmostEqual(state.globalWaypoint.lon,
waypoints[2].lon,
places=3)
# Check that receiving a new path does update the index to 1
waypoints = [msg.latlon(i + 1, i + 1) for i in range(numWaypoints)]
globalPathMsg = msg.path(waypoints)
self.globalPathPublisher.publish(globalPathMsg)
rospy.sleep(0.1)
state = self.sailbot.getCurrentState()
self.assertAlmostEqual(state.globalWaypoint.lat,
waypoints[1].lat,
places=3)
self.assertAlmostEqual(state.globalWaypoint.lon,
waypoints[1].lon,
places=3)
# Check that globalIndex increment out of range returns last waypoint
self.sailbot.globalPathIndex = numWaypoints + 1
state = self.sailbot.getCurrentState()
self.assertAlmostEqual(state.globalWaypoint.lat,
waypoints[numWaypoints - 1].lat,
places=3)
self.assertAlmostEqual(state.globalWaypoint.lon,
waypoints[numWaypoints - 1].lon,
places=3)
