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 obstacleOnStart(state):
'''Checks if there are any obstacles projected to be on the state's starting position
Args:
state (BoatState): State of the sailbot and other boats
Returns:
ObstacleInterface object that is at the start state causing it to be invalid, else returns None
If there are multiple obstacles on start, this just returns one of them
'''
referenceLatlon = state.globalWaypoint
obstacles = obs.getObstacles(state, referenceLatlon)
for obstacle in obstacles:
xy = utils.latlonToXY(state.position, referenceLatlon)
if not obstacle.isValid(xy):
return obstacle
return None
def checkGoalValidity(state, goalLatlon=None):
'''Checks if the goal of the given state is valid (no obstacle there)
Args:
state (BoatState): State of the sailbot and other boats
goalLatlon (latlon): Location to check the validity of. If is None, goal is the current global waypoint
Returns:
bool True iff there is no obstacle projected to be on the state's goal waypoint
'''
if goalLatlon is None:
goalLatlon = state.globalWaypoint
referenceLatlon = goalLatlon
obstacles = obs.getObstacles(state, referenceLatlon)
for obstacle in obstacles:
goalXY = utils.latlonToXY(goalLatlon, referenceLatlon)
if not obstacle.isValid(goalXY):
return False
return True
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))
nextGlobalWaypointXY = utils.latlonToXY(nextGlobalWaypoint, referenceLatlon)
previousGlobalWaypointXY = utils.latlonToXY(previousGlobalWaypoint, referenceLatlon)
_, glob_isStartEast, glob_slope, glob_y = getPerpLine(previousGlobalWaypointXY, nextGlobalWaypointXY, True)
if nextLocalWaypoint:
nextLocalWaypointXY = utils.latlonToXY(nextLocalWaypoint, referenceLatlon)
previousLocalWaypointXY = utils.latlonToXY(previousLocalWaypoint, referenceLatlon)
_, isStartEast, slope, y_intercept = getPerpLine(previousLocalWaypointXY, nextLocalWaypointXY)
if localPath:
localPathXY = [utils.latlonToXY(localWaypoint, referenceLatlon) for localWaypoint in localPath]
localPathX = [xy[0] for xy in localPathXY]
localPathY = [xy[1] for xy in localPathXY]
positionXY = utils.latlonToXY(state.position, referenceLatlon)
shipsXY = obs.getObstacles(state, referenceLatlon)
# Update plots, or plot them if they don't yet exist and the required variables are defined
if 'nextGlobalWaypointXY' in globals():
# Resize axes and markers if needed
if needAxesResized(positionXY, nextGlobalWaypointXY, xPLim, xNLim, yPLim, yNLim):
xPLim, xNLim, yPLim, yNLim = getXYLimits(positionXY, nextGlobalWaypointXY)
axes.set_xlim(xNLim, xPLim)
axes.set_ylim(yNLim, yPLim)
glob_X, glob_Y = getPerpPlot(glob_isStartEast, glob_slope, glob_y, nextGlobalWaypointXY)
if 'nextGlobalWaypointPlot' in globals():
nextGlobalWaypointPlot.set_xdata(nextGlobalWaypointXY[0])
nextGlobalWaypointPlot.set_ydata(nextGlobalWaypointXY[1])
nextGlobalWaypointPlot.set_markersize(markersize)
globalWaypointReachedPlot.set_xdata(glob_X)
def createPath(
state,
runtimeSeconds=None,
numRuns=None,
resetSpeedupDuringPlan=False,
maxAllowableRuntimeSeconds=MAX_ALLOWABLE_PATHFINDING_TOTAL_RUNTIME_SECONDS
):
'''Create a Path from state.position to state.globalWaypoint. Runs OMPL pathfinding multiple times and returns
the best path.
Args:
state (BoatState): Current state of the boat, which contains all necessary information to perform pathfinding
runtimeSeconds (float): Number of seconds that the each pathfinding attempt should run.
If None, read from rospy param "runtime_seconds"
numRuns (int): Number of pathfinding attempts that are run in normal case
If None, read from rospy param "num_runs"
resetSpeedupDuringPlan (bool): Decides if pathfinding should set the speedup value to 1.0 during the pathfinding
maxAllowableRuntimeSeconds (double): Maximum total time that this method should take to run. Will take longer
than runtimeSeconds*numRuns only if pathfinding is unable to find a path.
Returns:
Path object representing the path from state.position to state.globalWaypoint
Returns None if cannot find a single valid solution in the given maxAllowableRuntimeSeconds
'''
# Helper methods
def getXYLimits(start, goal, extraLengthFraction=0.6):
# Calculate extra length to allow wider solution space
width = math.fabs(goal[0] - start[0])
height = math.fabs(goal[1] - start[1])
extraKm = extraLengthFraction * max(width, height)
xMin = min(start[0], goal[0]) - extraKm
yMin = min(start[1], goal[1]) - extraKm
xMax = max(start[0], goal[0]) + extraKm
yMax = max(start[1], goal[1]) + extraKm
return [xMin, yMin, xMax, yMax]
def isValidSolution(solution, referenceLatlon, state):
if not solution.haveExactSolutionPath():
return False
return True
def plotPathfindingProblem(globalWindDirectionDegrees, dimensions, start,
goal, obstacles, headingDegrees):
# Clear plot if already there
plt.cla()
# Create plot with start and goal
x_min, y_min, x_max, y_max = dimensions
markersize = min(x_max - x_min, y_max - y_min) / 2
plt.ion()
axes = plt.gca()
goalPlot, = axes.plot(goal[0],
goal[1],
marker='*',
color='y',
markersize=markersize) # Yellow star
# Red triangle with correct heading. The (-90) is because the triangle
# default heading 0 points North, but this heading has 0 be East.
startPlot, = axes.plot(start[0],
start[1],
marker=(3, 0, headingDegrees - 90),
color='r',
markersize=markersize)
# Setup plot xy limits and labels
axes.set_xlim(x_min, x_max)
axes.set_ylim(y_min, y_max)
axes.set_aspect('equal')
plt.grid(True)
axes.set_xlabel('X distance to position (km)')
axes.set_ylabel('Y distance to position (km)')
axes.set_title('Setup of pathfinding problem')
# Add boats and wind speed arrow
for obstacle in obstacles:
obstacle.addPatch(axes)
arrowLength = min(dimensions[2] - dimensions[0],
dimensions[3] - dimensions[1]) / 15
arrowCenter = (dimensions[0] + 1.5 * arrowLength,
dimensions[3] - 1.5 * arrowLength)
arrowStart = (arrowCenter[0] - 0.5 * arrowLength *
math.cos(math.radians(globalWindDirectionDegrees)),
arrowCenter[1] - 0.5 * arrowLength *
math.sin(math.radians(globalWindDirectionDegrees)))
windDirection = patches.FancyArrow(
arrowStart[0],
arrowStart[1],
arrowLength * math.cos(math.radians(globalWindDirectionDegrees)),
arrowLength * math.sin(math.radians(globalWindDirectionDegrees)),
width=arrowLength / 4)
axes.add_patch(windDirection)
# Draw plot
plt.draw()
plt.pause(0.001)
def setAverageDistanceBetweenWaypoints(solutionPath):
# Set the average distance between waypoints
localPathLengthKm = solutionPath.length()
numberOfLocalWaypoints = int(
localPathLengthKm / utils.AVG_DISTANCE_BETWEEN_LOCAL_WAYPOINTS_KM)
solutionPath.interpolate(numberOfLocalWaypoints)
def findBestSolution(validSolutions, invalidSolutions):
# If no valid solutions found, use the best invalid one. Do not perform any path simplifying on invalid paths.
if len(validSolutions) == 0:
# Set the average distance between waypoints. Must be done before cost calculation and comparison
for solution in invalidSolutions:
setAverageDistanceBetweenWaypoints(solution.getSolutionPath())
bestSolution = min(invalidSolutions,
key=lambda x: x.getSolutionPath().cost(
x.getOptimizationObjective()).value())
bestSolutionPath = bestSolution.getSolutionPath()
minCost = bestSolutionPath.cost(
bestSolution.getOptimizationObjective()).value()
else:
# Set the average distance between waypoints. Must be done before cost calculation and comparison
for solution in validSolutions:
setAverageDistanceBetweenWaypoints(solution.getSolutionPath())
# Find path with minimum cost. Can be either simplified or unsimplified path.
# Need to recheck that simplified paths are valid before using
minCost = sys.maxsize
bestSolution = None
bestSolutionPath = None
for solution in validSolutions:
# Check unsimplified path
unsimplifiedPath = og.PathGeometric(solution.getSolutionPath())
unsimplifiedCost = unsimplifiedPath.cost(
solution.getOptimizationObjective()).value()
if unsimplifiedCost < minCost:
bestSolution = solution
bestSolutionPath = unsimplifiedPath
minCost = unsimplifiedCost
return bestSolution, bestSolutionPath, minCost
if runtimeSeconds is None:
runtimeSeconds = rospy.get_param('runtime_seconds', default=0.125)
if numRuns is None:
numRuns = rospy.get_param('num_runs', default=8)
ou.setLogLevel(ou.LOG_WARN)
# Set speedup to 1.0 during planning
speedupBeforePlan = rospy.get_param('speedup', default=1.0)
if resetSpeedupDuringPlan:
speedupDuringPlan = 1.0
rospy.loginfo(
"Setting speedup to this value during planning = {}".format(
speedupDuringPlan))
rospy.set_param('speedup', speedupDuringPlan)
# Get setup parameters from state for ompl plan()
# Convert all latlons to NE in km wrt referenceLatlon
referenceLatlon = state.globalWaypoint
start = utils.latlonToXY(state.position, referenceLatlon)
goal = utils.latlonToXY(state.globalWaypoint, referenceLatlon)
dimensions = getXYLimits(start, goal)
obstacles = obs.getObstacles(state, referenceLatlon)
stateSampler = rospy.get_param('state_sampler', default='grid')
# Run the planner multiple times and find the best one
rospy.loginfo(
"Running createLocalPathSS. runtimeSeconds: {}. numRuns: {}. Total time: {} seconds"
.format(runtimeSeconds, numRuns, runtimeSeconds * numRuns))
rospy.loginfo("Using stateSampler = {}".format(stateSampler)
if len(stateSampler) > 0 else "Using default state sampler")
# Create non-blocking plot showing the setup of the pathfinding problem.
# Useful to understand if the pathfinding problem is invalid or impossible
shouldPlot = rospy.get_param('plot_pathfinding_problem', False)
if shouldPlot:
plotPathfindingProblem(state.globalWindDirectionDegrees, dimensions,
start, goal, obstacles, state.headingDegrees)
# Take screenshot
shouldTakeScreenshot = rospy.get_param('screenshot', False)
if shouldTakeScreenshot:
utils.takeScreenshot()
# Look for solutions
validSolutions = []
invalidSolutions = []
plannerType = rospy.get_param('planner_type', 'lazyprmstar')
for i in range(numRuns):
# TODO: Incorporate globalWindSpeed into pathfinding?
rospy.loginfo("Starting path-planning run number: {}".format(i))
solution = plan(runtimeSeconds, plannerType,
state.globalWindDirectionDegrees, dimensions, start,
goal, obstacles, state.headingDegrees, stateSampler)
if isValidSolution(solution, referenceLatlon, state):
validSolutions.append(solution)
else:
invalidSolutions.append(solution)
# If no validSolutions found, re-run with larger runtime
totalRuntimeSeconds = numRuns * runtimeSeconds
while len(validSolutions) == 0:
rospy.logwarn("No valid solutions found in {} seconds runtime".format(
runtimeSeconds))
runtimeSeconds *= INCREASE_RUNTIME_FACTOR
totalRuntimeSeconds += runtimeSeconds
incrementTempInvalidSolutions()
# If valid solution can't be found for large runtime, then stop searching
if totalRuntimeSeconds >= maxAllowableRuntimeSeconds:
rospy.logerr(
"No valid solution can be found in under {} seconds. Returning None."
.format(maxAllowableRuntimeSeconds))
incrementCountInvalidSolutions()
return None
rospy.logwarn(
"Attempting to rerun with longer runtime: {} seconds".format(
runtimeSeconds))
solution = plan(runtimeSeconds, plannerType,
state.globalWindDirectionDegrees, dimensions, start,
goal, obstacles, state.headingDegrees, stateSampler)
if isValidSolution(solution, referenceLatlon, state):
validSolutions.append(solution)
else:
invalidSolutions.append(solution)
# Find best solution
bestSolution, bestSolutionPath, minCost = findBestSolution(
validSolutions, invalidSolutions)
# Close plot if it was started
plt.close()
# Reset speedup back to original value
if resetSpeedupDuringPlan:
rospy.loginfo(
"Setting speedup back to its value before planning = {}".format(
speedupBeforePlan))
rospy.set_param('speedup', speedupBeforePlan)
return Path(OMPLPath(bestSolution, bestSolutionPath, referenceLatlon))
def updateObstacles(self, state):
'''Update the obstacles of the OMPL validation checker'''
self._obstacles = obs.getObstacles(state, self._referenceLatlon)
validity_checker = ph.ValidityChecker(self._ss.getSpaceInformation(),
self._obstacles)
self._ss.setStateValidityChecker(validity_checker)