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 obstacleOnPath(self,
state,
numLookAheadWaypoints=None,
showWarnings=False):
'''Checks if there any obstacles on the path starting from state.position to the next numLookAheadWaypoints
Args:
state (BoatState): state of the boat, which defines the boat position and obstacle positions
numLookAheadWaypoints (int): number of waypoints to look ahead at from state.position
(ignores obstacles very far ahead)
showWarnings (bool): display ros warnings if obstacles are detected
Returns:
bool True iff there exists an obstacle on the path within the first numLookAheadWaypoints
starting from state.position
'''
# Default behavior when numLookAheadWaypoints is not given OR bad input: set to max
numLookAheadWaypoints = self._cleanNumLookAheadWaypoints(
numLookAheadWaypoints)
# Check if path will hit objects
positionXY = utils.latlonToXY(state.position,
self.getReferenceLatlon())
self.updateObstacles(state)
waypointIndexWithObstacle = indexOfObstacleOnPath(
positionXY, self.getNextWaypointIndex(), numLookAheadWaypoints,
self._omplPath)
if waypointIndexWithObstacle != -1:
if showWarnings:
rospy.logwarn(
"Obstacle on path. waypointIndexWithObstacle: {}".format(
waypointIndexWithObstacle))
return True
return False
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 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_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)
rospy.Subscriber("localPath", path, localPathCallback)
rospy.Subscriber("nextLocalWaypoint", latlon, nextLocalWaypointCallback)
rospy.Subscriber("previousLocalWaypoint", latlon, previousLocalWaypointCallback)
rospy.Subscriber("nextGlobalWaypoint", latlon, nextGlobalWaypointCallback)
rospy.Subscriber("previousGlobalWaypoint", latlon, previousGlobalWaypointCallback)
r = rospy.Rate(1.0 / VISUALIZER_UPDATE_PERIOD_SECONDS)
sailbot.waitForFirstSensorDataAndGlobalPath()
rospy.loginfo("ROS message received. Starting visualization")
state = sailbot.getCurrentState()
referenceLatlon = nextGlobalWaypoint
# Convert values from latlon to XY, relative to the referenceLatlon
if nextGlobalWaypoint: # check that nextGlobalWaypoint is available
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)
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 upwindOrDownwindOnPath(self,
state,
numLookAheadWaypoints=None,
showWarnings=False):
'''Checks if there exists any upwind or downwind sailing on the path starting
from state.position to the next numLookAheadWaypoints
Note: Requires that upwind/downwind sailing be detected consistently for a certain time interval to return True
This prevents upwind/downwind to be detected from wind sensor noise or poor measurements while turning
But will still catch real upwind/downwind sailing when called repeatedly
Args:
state (BoatState): state of the boat, which defines the boat position and the global wind
numLookAheadWaypoints (int): number of waypoints to look ahead at from state.position
(ignores upwind/downwind very far ahead)
showWarnings (bool): display ros warnings if upwind/downwind is detected
Returns:
bool True iff this method detects upwind/downwind sailing on the path within the first numLookAheadWaypoints
starting from state.position CONSISTENTLY for a certain time interval
'''
# Default behavior when numLookAheadWaypoints is not given OR bad input: set to max
numLookAheadWaypoints = self._cleanNumLookAheadWaypoints(
numLookAheadWaypoints)
# Calculate global wind from measured wind and boat state
# Get relevant waypoints (boat position first, then the next
# numLookAheadWaypoints startin from nextLocalWaypoint onwards)
relevantWaypoints = []
relevantWaypoints.append(
utils.latlonToXY(state.position, self.getReferenceLatlon()))
for waypointIndex in range(
self.getNextWaypointIndex(),
self.getNextWaypointIndex() + numLookAheadWaypoints):
waypoint = self._omplPath.getSolutionPath().getState(waypointIndex)
relevantWaypoints.append([waypoint.getX(), waypoint.getY()])
# Check relevantWaypoints for upwind or downwind sailing
upwindOrDownwind = False
for waypointIndex in range(1, len(relevantWaypoints)):
# Calculate required heading between waypoints
waypoint = relevantWaypoints[waypointIndex]
prevWaypoint = relevantWaypoints[waypointIndex - 1]
requiredHeadingDegrees = math.degrees(
math.atan2(waypoint[1] - prevWaypoint[1],
waypoint[0] - prevWaypoint[0]))
if ph.isDownwind(math.radians(state.globalWindDirectionDegrees),
math.radians(requiredHeadingDegrees)):
if showWarnings:
rospy.loginfo(
"Downwind sailing on path detected. globalWindDirectionDegrees: {}. "
"requiredHeadingDegrees: {}. waypointIndex: {}".format(
state.globalWindDirectionDegrees,
requiredHeadingDegrees, waypointIndex))
upwindOrDownwind = True
break
elif ph.isUpwind(math.radians(state.globalWindDirectionDegrees),
math.radians(requiredHeadingDegrees)):
if showWarnings:
rospy.loginfo(
"Upwind sailing on path detected. globalWindDirectionDegrees: {}. "
"requiredHeadingDegrees: {}. waypointIndex: {}".format(
state.globalWindDirectionDegrees,
requiredHeadingDegrees, waypointIndex))
upwindOrDownwind = True
break
# Set counter to 0 on first use
try:
self.lastTimeNotUpwindOrDownwind
except AttributeError:
rospy.loginfo(
"Handling first time case in upwindOrDownwindOnPath()")
self.lastTimeNotUpwindOrDownwind = time.time()
# Reset last time not upwind/downwind
if not upwindOrDownwind:
self.lastTimeNotUpwindOrDownwind = time.time()
return False
# Return true only if upwindOrDownwind for enough time
consecutiveUpwindOrDownwindTimeSeconds = time.time(
) - self.lastTimeNotUpwindOrDownwind
if consecutiveUpwindOrDownwindTimeSeconds >= UPWIND_DOWNWIND_TIME_LIMIT_SECONDS:
if showWarnings:
rospy.logwarn(
"Upwind/downwind sailing detected for {} seconds consecutively, which is greater than"
"the {} second limit. This officially counts as upwind/downwind"
.format(consecutiveUpwindOrDownwindTimeSeconds,
UPWIND_DOWNWIND_TIME_LIMIT_SECONDS))
self.lastTimeNotUpwindOrDownwind = time.time()
return True
else:
if showWarnings:
rospy.loginfo(
"Upwind/downwind sailing detected for only {} seconds consecutively, which is less than"
"the {} second limit. This does not count as upwind/downwind sailing yet."
.format(consecutiveUpwindOrDownwindTimeSeconds,
UPWIND_DOWNWIND_TIME_LIMIT_SECONDS))
return False
def waypointReached(self,
positionLatlon,
previousWaypointLatlon,
nextWaypointLatlon,
isGlobal=False):
'''Check if the given positionLatlon has reached the next waypoint.
This is done by drawing a line from the waypoint at (self._nextWaypointIndex - 1) to (self._nextWaypointIndex)
Then drawing a line perpendicular to the previous line that
is dist in front of the waypoint at self._nextWaypointIndex.
Then checks if the positionLatlon is past the perpendicular line or not
Examples: B is boat. 0 is (self._nextWaypointIndex - 1). 1 is (self._nextWaypointIndex).
Example 1 Waypoint not reached:
|
|
0 B 1
|
|
Example 2 Waypoint reached:
|
|
0 1
|
|B
Args:
positionLatlon (sailbot_msg.msg._latlon.latlon): Latlon of the current boat position
Returns:
bool True iff the positionLatlon has reached the next waypoint
'''
# Convert from latlons to XY
refLatlon = self._omplPath.getReferenceLatlon()
positionX, positionY = utils.latlonToXY(positionLatlon, refLatlon)
previousWaypointXY = utils.latlonToXY(previousWaypointLatlon,
refLatlon)
nextWaypointXY = utils.latlonToXY(nextWaypointLatlon, refLatlon)
isStartNorth, isStartEast, normalSlope, b = getPerpLine(
previousWaypointXY, nextWaypointXY, isGlobal)
dist = GLOBAL_WAYPOINT_REACHED_DISTANCE if isGlobal else WAYPOINT_REACHED_DISTANCE
# Handle edge cases where waypoints have the same x or y component
if normalSlope == 0:
if isStartNorth:
return positionY <= b
else:
return positionY >= b
elif not normalSlope:
if isStartEast:
return positionX <= nextWaypointXY[0] + dist
else:
return positionX >= nextWaypointXY[0] - dist
def y(x):
return normalSlope * x + b
def x(y):
return (y - b) / normalSlope
# import numpy as np
# plt.xlim(-20, 20)
# plt.ylim(-20, 20)
# plt.plot([0], [0], marker='o', markersize=10, color="black")
# plt.plot([positionX], [positionY], marker='o', markersize=10, color="blue")
# plt.plot([previousWaypointX], [previousWaypointY], marker='o', markersize=10, color="green")
# plt.plot([nextWaypointX], [nextWaypointY], marker="o", markersize=10, color="red")
# x_plot = np.linspace(-200, 200, 100)
# plt.plot(x_plot, y(x_plot), '-r')
# plt.show()
# Check if the line has been crossed
if isStartNorth:
if positionY < y(positionX):
return True
elif positionY > y(positionX):
return True
if isStartEast:
if positionX < x(positionY):
return True
elif positionX > x(positionY):
return True
return False
def removePastWaypoints(self, state):
"""Removes waypoints that the boat has already passed.
Note: Best used when waypointReached() == True, not every loop
Reason: keepAfter() method implementation assumes all waypoints are equally spaced
keepAfter() algorithm:
1. Find index of waypoint closest to state, call that closestWaypoint
2. Check dist(waypoint before closestWaypoint, state) and dist(waypoint after closestWaypoint, state)
* If closer to waypoint BEFORE closestWaypoint: remove all waypoints before closestWaypoint
EXCLUDING closestWaypoint
* If closer to waypoint AFTER closestWaypoint: remove all waypoints before closestWaypoint
INCLUDING closestWaypoint
This answers the question: should we remove the closestWaypoint?
Eg. Are we past the closestWaypoint or behind it?
Algorithm works when all waypoints have about the same distance apart.
Doesn't always work when waypoints are not all equally close.
Let B = Boat and numbers be waypoint numbers
Boat is always aiming for index = 1
Will replace the boat with index 0 after operation
Example 1 (regular behavior)
------------------------
Before:
0 B 1 2 3 (B is closest to 1. Thus it compares dist(0, B) and dist(2, B). Decides to keep 1.)
KeepAfter:
B 0 1 2
Example 2 (cases where we overshoot the waypoint, so we skip a waypoint
------------------------
Before:
0 1B 2 3 (B is closest to 1. Thus it compares dist(0, B) and dist(2, B). Decides to remove 1.)
KeepAfter:
B 0 1
Example 3 (edge case where the boat's position must be prepended to the path to avoid out of bounds)
------------------------
Before:
0 1 2 B 3 (B is closest to 2. Thus it compares dist(1, B) and dist(3, B). Decides to remove 2.)
KeepAfter:
B 0
After add in boat position as first position:
0 1
Example 4 (strange case with irregular spacing)
------------------------
Before:
0 1B 2 3 (B is closest to 1. Thus it compares dist(0, B) and dist(2, B). Decides to keep 1.)
KeepAfter:
0B 1 2
Example 5
------------------------
Before:
0 B 1 2 3 (B is closest to 0. For this edge case, it just keeps everything.)
KeepAfter:
0 B 1 2 3
Therefore, in this method, we will have an edge case check after the keepAfter operation.
If lengthAfter == 1, then add position B as a starting waypoint.
"""
# Get current position in xy coordinates
x, y = utils.latlonToXY(state.position, self._referenceLatlon)
positionXY = self._ss.getSpaceInformation().allocState()
positionXY.setXY(x, y)
# Keep waypoints only after your positionXY
lengthBefore = self._solutionPath.getStateCount()
self._solutionPath.keepAfter(positionXY)
lengthAfter = self._solutionPath.getStateCount()
rospy.loginfo("lengthBefore = {}. lengthAfter = {}".format(
lengthBefore, lengthAfter))
if lengthAfter == 0:
raise RuntimeError(
"lengthAfter == 0, can't perform pathfinding. keepAfter() won't do this"
)
if lengthAfter == 1:
rospy.loginfo(
"lengthAfter == 1, prepending boat position to path so path length is at least 2"
)
self._solutionPath.prepend(positionXY)