lejos.robotics.navigation
Class NavPathController

java.lang.Object
  extended by lejos.robotics.navigation.NavPathController
All Implemented Interfaces:
PathController

public class NavPathController
extends java.lang.Object
implements PathController

This class can cause the robot to follow a route - a sequence of WayPoint ; The way points are stored in a queue (actually, a Collection). This class uses an inner class running its own thread to issue movement commands to its MoveController, which can be either a DifferentialPilot or SteeringPilot. It also uses a PoseProvider to keep its pose updated, and calls its WayPointListener when a way point is reached.

Author:
Roger Glassey

Nested Class Summary
protected  class NavPathController.Nav
          This inner class runs the thread that processes the waypoint queue
 
Field Summary
protected  WayPoint _destination
           
protected  boolean _keepGoing
           
protected  NavPathController.Nav _nav
           
protected  MoveController _pilot
           
protected  Pose _pose
           
protected  double _radius
           
protected  java.util.ArrayList<WayPoint> _route
           
protected  java.util.ArrayList<WayPointListener> listeners
           
protected  PathFinder pathFinder
           
protected  PoseProvider poseProvider
           
protected  java.util.ArrayList<WayPointListener> targetListeners
           
 
Constructor Summary
NavPathController(MoveController pilot)
          Can use any pilot that implements the ArcMoveController interface.
NavPathController(MoveController pilot, PoseProvider poseProvider)
          Creates a PathController using a custom poseProvider, rather than the default tachometer pose provider.
NavPathController(MoveController pilot, PoseProvider poseProvider, PathFinder pathFinder)
          Creates a PathController which will navigate to a point via goTo() using a PathFinder to provide assistance to create a path.
 
Method Summary
 void addListener(WayPointListener aListener)
          Adds a WayPointListener that will be notified with the actual waypoint it reached.
 void addTargetListener(WayPointListener targetListener)
          Adds a waypoint listener that will be notified with the theoretical target waypoint it reached.
 void addWayPoint(WayPoint aWayPoint)
          Adds a WayPoint to the route.
 void flushQueue()
          Stops the robot and empties the queue of waypoints (the route)
 void followRoute(java.util.Collection<WayPoint> aRoute, boolean immediateReturn)
          Moves the robot through the sequence of waypoints contained in the route.
 MoveController getMoveController()
          Returns a reference to the MoveController.
 PathFinder getPathFinder()
           
 PoseProvider getPoseProvider()
          Get a reference to the PoseProvider being used as a localizer.
 WayPoint getWayPoint()
          Returns the waypoint to which the robot is moving
 void goTo(double x, double y)
          This method will navigate to a point.
 void goTo(WayPoint destination)
          This method will navigate to a point.
 void goTo(WayPoint destination, boolean immediateReturn)
          This method will navigate to a point.
 void interrupt()
          Stops the robot immediately; preserves the rest of the queue
 boolean isGoing()
          returns false if the the final waypoint has been reached or interrupt() has been called
 void resume()
          Resumes following the route
 void setPathFinder(PathFinder pathFinder)
           
 void setPoseProvider(PoseProvider aProvider)
          Sets a new PoseProvider for the PathController robot to use.
 void waitForDestinationReached()
           
 
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Field Detail

_nav

protected NavPathController.Nav _nav

_route

protected java.util.ArrayList<WayPoint> _route

listeners

protected java.util.ArrayList<WayPointListener> listeners

targetListeners

protected java.util.ArrayList<WayPointListener> targetListeners

_keepGoing

protected boolean _keepGoing

_pilot

protected MoveController _pilot

poseProvider

protected PoseProvider poseProvider

pathFinder

protected PathFinder pathFinder

_pose

protected Pose _pose

_destination

protected WayPoint _destination

_radius

protected double _radius
Constructor Detail

NavPathController

public NavPathController(MoveController pilot)
Can use any pilot that implements the ArcMoveController interface.

Parameters:
pilot -

NavPathController

public NavPathController(MoveController pilot,
                         PoseProvider poseProvider)
Creates a PathController using a custom poseProvider, rather than the default tachometer pose provider.

Parameters:
pilot -
poseProvider -

NavPathController

public NavPathController(MoveController pilot,
                         PoseProvider poseProvider,
                         PathFinder pathFinder)
Creates a PathController which will navigate to a point via goTo() using a PathFinder to provide assistance to create a path.

Parameters:
pilot -
poseProvider -
pathFinder -
Method Detail

setPathFinder

public void setPathFinder(PathFinder pathFinder)

getPathFinder

public PathFinder getPathFinder()

isGoing

public boolean isGoing()
returns false if the the final waypoint has been reached or interrupt() has been called


followRoute

public void followRoute(java.util.Collection<WayPoint> aRoute,
                        boolean immediateReturn)
Description copied from interface: PathController
Moves the robot through the sequence of waypoints contained in the route. Informs its listeners of each waypoint reached. The waypoint is removed from the route when it is reached.

Specified by:
followRoute in interface PathController
immediateReturn - ; if false the method returns when the route is empty

goTo

public void goTo(WayPoint destination,
                 boolean immediateReturn)
This method will navigate to a point. If a PathFinder was used in the constructor, it will rely on it to calculate a series of waypoints to get to the destination.

Specified by:
goTo in interface PathController
Parameters:
destination - the destination MoveController
immediateReturn - true for non-blocking, false for blocking

goTo

public void goTo(WayPoint destination)
This method will navigate to a point. If a PathFinder was used in the constructor, it will rely on it to calculate a series of waypoints to get to the destination.

Specified by:
goTo in interface PathController
Parameters:
destination - the destination waypoint

goTo

public void goTo(double x,
                 double y)
This method will navigate to a point. If a PathFinder was used in the constructor, it will rely on it to calculate a series of waypoints to get to the destination.

Specified by:
goTo in interface PathController
Parameters:
x - The x coordinate
y - The y coordinate

addListener

public void addListener(WayPointListener aListener)
Description copied from interface: PathController
Adds a WayPointListener that will be notified with the actual waypoint it reached.

Specified by:
addListener in interface PathController

addTargetListener

public void addTargetListener(WayPointListener targetListener)
Description copied from interface: PathController
Adds a waypoint listener that will be notified with the theoretical target waypoint it reached.

Specified by:
addTargetListener in interface PathController

getMoveController

public MoveController getMoveController()
Returns a reference to the MoveController. The Navigator pose will be automatically updated as a result of methods executed on the MoveController.

Specified by:
getMoveController in interface PathController
Returns:
reference to the MoveController

addWayPoint

public void addWayPoint(WayPoint aWayPoint)
Description copied from interface: PathController
Adds a WayPoint to the route. If the route was empty, the robot immediately starts moving toward the Waypoint

Specified by:
addWayPoint in interface PathController

interrupt

public void interrupt()
Description copied from interface: PathController
Stops the robot immediately; preserves the rest of the queue

Specified by:
interrupt in interface PathController

resume

public void resume()
Description copied from interface: PathController
Resumes following the route

Specified by:
resume in interface PathController

flushQueue

public void flushQueue()
Description copied from interface: PathController
Stops the robot and empties the queue of waypoints (the route)

Specified by:
flushQueue in interface PathController

getWayPoint

public WayPoint getWayPoint()
Returns the waypoint to which the robot is moving

Returns:
the waypoint to which the robot is moving

setPoseProvider

public void setPoseProvider(PoseProvider aProvider)
Description copied from interface: PathController
Sets a new PoseProvider for the PathController robot to use. Example: If the robot moves from one environment (indoors) to another environment (outdoors) it might want to change to another method of localization if a change in environment is detected.

Specified by:
setPoseProvider in interface PathController
Parameters:
aProvider - the new PoseProvider

getPoseProvider

public PoseProvider getPoseProvider()
Description copied from interface: PathController
Get a reference to the PoseProvider being used as a localizer.

Specified by:
getPoseProvider in interface PathController
Returns:
the pose provider

waitForDestinationReached

public void waitForDestinationReached()