Package lejos.robotics.pathfinding

Class DijkstraPathFinder

java.lang.Object

lejos.robotics.pathfinding.DijkstraPathFinder

All Implemented Interfaces:

PathFinder

public class DijkstraPathFinder
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 the A* algorithm, a variant of the Dijkstra shortest path algorithm It uses the Node inner class for its internal representation of points.

Nested Class Summary

Nested Classes

Modifier and Type	Class and Description
protected class	DijkstraPathFinder.Node

Field Summary

Fields

Modifier and Type	Field and Description
protected boolean	_blocked set by segmentBlocked() used by findPath()
protected java.util.ArrayList<DijkstraPathFinder.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
protected int	_count
protected java.util.ArrayList<Line>	_map The map of the obstacles
protected java.util.ArrayList<DijkstraPathFinder.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 static float	BIG
private java.util.ArrayList<WaypointListener>	listeners

Constructor Summary

Constructors

Constructor and Description
DijkstraPathFinder(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 whild avoiding the obstacles 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 (or collection of lines) in the constructor.
protected DijkstraPathFinder.Node	getBest(DijkstraPathFinder.Node currentDestination) Helper method for findPath() returns the node in the Reached set, whose distance from the start node plus its straight line distance to the destination is the minimum.
int	getIterationCount()
protected java.util.ArrayList<Line>	getMap()
int	getNodeCount()
protected Path	getRoute(DijkstraPathFinder.Node destination) helper method for find path() calculates the route backtracking through predecessor chain
protected boolean	inCandidateSet(DijkstraPathFinder.Node aNode) helper method for findPath; check if aNode is in the set of candidate nodes
protected void	initialize()
protected boolean	inReachedSet(DijkstraPathFinder.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
protected boolean	segmentBlocked(DijkstraPathFinder.Node from, DijkstraPathFinder.Node theDest) helper method for findPath().
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

protected int _count

_blocked

protected boolean _blocked

set by segmentBlocked() used by findPath()

BIG

private static final float BIG

See Also:

Constant Field Values

_candidate

protected java.util.ArrayList<DijkstraPathFinder.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

protected java.util.ArrayList<DijkstraPathFinder.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

protected java.util.ArrayList<Line> _map

The map of the obstacles

Constructor Detail

DijkstraPathFinder

public DijkstraPathFinder(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 (or 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 whild avoiding the obstacles 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

Throws:

DestinationUnreachableException

setMap

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

setMap

public void setMap(LineMap theMap)

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

protected void initialize()

segmentBlocked

protected boolean segmentBlocked(DijkstraPathFinder.Node from,
                                 DijkstraPathFinder.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

getBest

protected DijkstraPathFinder.Node getBest(DijkstraPathFinder.Node currentDestination)

Helper method for findPath()
returns the node in the Reached set, whose distance from the start node plus its straight line distance to the destination is the minimum.

Parameters:

currentDestination - : the current destination node, (in the Candidate set)

Returns:

the node the node which could be the last node in the shortest path

inReachedSet

protected boolean inReachedSet(DijkstraPathFinder.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

protected boolean inCandidateSet(DijkstraPathFinder.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

protected Path getRoute(DijkstraPathFinder.Node destination)

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

Parameters:

destination -

Returns:

the route of the shortest path

getMap

protected 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