Package lejos.robotics.pathfinding

Class ShortestPathFinder

java.lang.Object

lejos.robotics.pathfinding.ShortestPathFinder

All Implemented Interfaces:

PathFinder

public class ShortestPathFinder
extends java.lang.Object
implements PathFinder

This class calculates the shortest path from a starting point to a finish point. while avoiding obstacles that are represented as a set of straight lines. The path passes through the end points of some of these lines, which is where the changes of direction occur. Since the robot is not point, the lines representing the obstacles should be lengthened so the actual robot will miss the actual obstacles. Use the lengthenLines() method to do this. Uses modification of the A* algorithm,which is a a variant of the Dijkstra shortest path algorithm. This variant adds nodes needed. It uses the Node inner class for its internal representation of points.

Nested Class Summary

Nested Classes

Modifier and Type	Class and Description
class	ShortestPathFinder.Node

Field Summary

Fields

Modifier and Type	Field and Description
private java.util.ArrayList<ShortestPathFinder.Node>	_candidate the set of nodes that are candidates for being in the shortest path, but whose distance from the start node is not yet known.
private int	_count
private boolean	_debug
private ShortestPathFinder.Node	_destination
private java.util.ArrayList<Line>	_map The map of the obstacles
private java.util.ArrayList<ShortestPathFinder.Node>	_reached the set of nodes that are candidates for being in the shortest path, and whose distance from the start node is known
private ShortestPathFinder.Node	_source
private static float	BIG
private java.util.ArrayList<WaypointListener>	listeners

Constructor Summary

Constructors

Constructor and Description
ShortestPathFinder(LineMap map)

Method Summary

Modifier and Type	Method and Description
void	addListener(WaypointListener wpl)
private Path	findPath(Point start, Point finish, java.util.ArrayList<Line> theMap) Finds the shortest path between start and finish Points while avoiding the obstacles represented by lines in the map
Path	findRoute(Pose start, Waypoint finish) Finds the shortest path from start to finish using the map (or collection of lines) in the constructor.
Path	findRoute(Pose start, Waypoint finish, LineMap theMap) Finds the shortest path from start to finish using the map ( collection of lines) in the constructor.
int	getIterationCount()
java.util.ArrayList<Line>	getMap()
int	getNodeCount()
private Path	getRoute(ShortestPathFinder.Node destination) helper method for find path() calculates the route backtracking through predecessor chain
boolean	inCandidateSet(ShortestPathFinder.Node aNode) helper method for findPath; check if aNode is in the set of candidate nodes
private void	initialize()
private boolean	inReachedSet(ShortestPathFinder.Node aNode) helper method for findPath; check if aNode is in the set of reached nodes
void	lengthenLines(float delta) lengthens all the lines in the map by delta at each end
private boolean	segmentBlocked(ShortestPathFinder.Node from, ShortestPathFinder.Node theDest) Helper method for findPath().
void	setDebug(boolean yes)
void	setMap(java.util.ArrayList<Line> theMap)
void	setMap(LineMap theMap)
void	startPathFinding(Pose start, Waypoint end)

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

listeners

private java.util.ArrayList<WaypointListener> listeners

_count

private int _count

BIG

private static final float BIG

See Also:

Constant Field Values

_destination

private ShortestPathFinder.Node _destination

_source

private ShortestPathFinder.Node _source

_candidate

private java.util.ArrayList<ShortestPathFinder.Node> _candidate

the set of nodes that are candidates for being in the shortest path, but whose distance from the start node is not yet known.

_reached

private java.util.ArrayList<ShortestPathFinder.Node> _reached

the set of nodes that are candidates for being in the shortest path, and whose distance from the start node is known

_map

private java.util.ArrayList<Line> _map

The map of the obstacles

_debug

private boolean _debug

Constructor Detail

ShortestPathFinder

public ShortestPathFinder(LineMap map)

Method Detail

findRoute

public Path findRoute(Pose start,
                      Waypoint finish)
               throws DestinationUnreachableException

Finds the shortest path from start to finish using the map (or collection of lines) in the constructor.

Specified by:

findRoute in interface PathFinder

Parameters:

start - the initial robot pose

finish - the final robot location

Returns:

the shortest route

Throws:

DestinationUnreachableException - if, for example, you nave not called setMap();

findRoute

public Path findRoute(Pose start,
                      Waypoint finish,
                      LineMap theMap)
               throws DestinationUnreachableException

Finds the shortest path from start to finish using the map ( collection of lines) in the constructor.

Parameters:

start - the initial robot pose

finish - the final robot location

theMap - the LineMap of obstacles

Returns:

the shortest route

Throws:

DestinationUnreachableException - if, for example, you nave not called setMap();

findPath

private Path findPath(Point start,
                      Point finish,
                      java.util.ArrayList<Line> theMap)
               throws DestinationUnreachableException

Finds the shortest path between start and finish Points while avoiding the obstacles represented by lines in the map

Parameters:

start - : the beginning of the path

finish - : the destination

theMap - that contains the obstacles

Returns:

an array list of waypoints. If no path exists, returns null and throws an exception.

Throws:

DestinationUnreachableException

segmentBlocked

private boolean segmentBlocked(ShortestPathFinder.Node from,
                               ShortestPathFinder.Node theDest)

Helper method for findPath(). Determines if the straight line segment crosses a line on the map. Side effect: creates nodes at the end of the blocking line and adds them to the _candidate set

Parameters:

from - the beginning of the line segment

theDest - the end of the line segment

Returns:

true if the segment is blocked

inReachedSet

private boolean inReachedSet(ShortestPathFinder.Node aNode)

helper method for findPath; check if aNode is in the set of reached nodes

Parameters:

aNode -

Returns:

true if aNode has been reached already

inCandidateSet

public boolean inCandidateSet(ShortestPathFinder.Node aNode)

helper method for findPath; check if aNode is in the set of candidate nodes

Parameters:

aNode -

Returns:

true if aNode has been reached already

getRoute

private Path getRoute(ShortestPathFinder.Node destination)

helper method for find path()
calculates the route backtracking through predecessor chain

Parameters:

destination -

Returns:

the route of the shortest path

setMap

public void setMap(java.util.ArrayList<Line> theMap)

setMap

public void setMap(LineMap theMap)

setDebug

public void setDebug(boolean yes)

lengthenLines

public void lengthenLines(float delta)

lengthens all the lines in the map by delta at each end

Parameters:

delta - added to each end of each line

initialize

private void initialize()

getMap

public java.util.ArrayList<Line> getMap()

getIterationCount

public int getIterationCount()

getNodeCount

public int getNodeCount()

addListener

public void addListener(WaypointListener wpl)

Specified by:

addListener in interface PathFinder

startPathFinding

public void startPathFinding(Pose start,
                             Waypoint end)

Specified by:

startPathFinding in interface PathFinder