lejos.robotics.navigation
Class Navigator

java.lang.Object
  extended by lejos.robotics.navigation.Navigator
All Implemented Interfaces:
WaypointListener

public class Navigator
extends Object
implements WaypointListener

This class controls a robot to traverse a Path, a sequence of Waypoints. It's default mode for a new path is continuous movement (no stopping at waypoints) but see also singleStep(boolean). To interrupt the path traversal, call stop(). It 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 Calls its NavigationListeners when a Waypoint is reached or the robot stops. This class has only one blocking method: waitForStop() .

Author:
Roger Glassey

Constructor Summary
Navigator(MoveController pilot)
          Allocates a Navigator object, using pilot that implements the ArcMoveController interface.
Navigator(MoveController pilot, PoseProvider poseProvider)
          Allocates a Navigator object using a pilot and a custom poseProvider, rather than the default OdometryPoseProvider.
 
Method Summary
 void addNavigationListener(NavigationListener listener)
          Adds a NavigationListener that is informed when a the robot stops or reaches a WayPoint.
 void addWaypoint(float x, float y)
          Constructs an new Waypoint from the parameters and adds it to the end of the path.
 void addWaypoint(float x, float y, float heading)
          Constructs an new Waypoint from the parameters and adds it to the end of the path.
 void addWaypoint(Waypoint aWaypoint)
          Adds a Waypoint to the end of the path.
 void clearPath()
          Clears the current path.
 void followPath()
          Starts the robot traversing the current path.
 void followPath(Path path)
          Starts the robot traversing the path.
 MoveController getMoveController()
          Returns the MoveController belonging to this object.
 Path getPath()
          Gets the current path
 PoseProvider getPoseProvider()
          Returns the PoseProvider
 Waypoint getWaypoint()
          Returns the waypoint to which the robot is presently moving.
 void goTo(float x, float y)
          Starts the moving toward the destination Waypoint created from the parameters.
 void goTo(float x, float y, float heading)
          Starts the moving toward the destination Waypoint created from the parameters.
 void goTo(Waypoint destination)
          Starts the robot moving toward the destination.
 boolean isMoving()
          Returns true if the robot is moving toward a waypoint.
 boolean pathCompleted()
          Returns true if the the final waypoint has been reached
 void pathGenerated()
          Called when generation of the path is complete
 boolean rotateTo(double angle)
          Rotates the robot to a new absolute heading.
 void setPath(Path path)
          Sets the path that the Navigator will traverse.
 void setPoseProvider(PoseProvider aProvider)
          Sets the PoseProvider after construction of the Navigator
 void singleStep(boolean yes)
          Controls whether the robot stops at each Waypoint; applies to the current path only.
 void stop()
          Stops the robot.
 boolean waitForStop()
          Waits for the robot to stop for any reason ; returns true if the robot stopped at the final Waypoint of the path.
 
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Constructor Detail

Navigator

public Navigator(MoveController pilot)
Allocates a Navigator object, using pilot that implements the ArcMoveController interface.

Parameters:
pilot -

Navigator

public Navigator(MoveController pilot,
                 PoseProvider poseProvider)
Allocates a Navigator object using a pilot and a custom poseProvider, rather than the default OdometryPoseProvider.

Parameters:
pilot - the pilot
poseProvider - the custom PoseProvider
Method Detail

setPoseProvider

public void setPoseProvider(PoseProvider aProvider)
Sets the PoseProvider after construction of the Navigator

Parameters:
aProvider - the PoseProvider

addNavigationListener

public void addNavigationListener(NavigationListener listener)
Adds a NavigationListener that is informed when a the robot stops or reaches a WayPoint.

Parameters:
listener - the NavitationListener

getPoseProvider

public PoseProvider getPoseProvider()
Returns the PoseProvider

Returns:
the PoseProvider

getMoveController

public MoveController getMoveController()
Returns the MoveController belonging to this object.

Returns:
the pilot

setPath

public void setPath(Path path)
Sets the path that the Navigator will traverse. By default, the robot will not stop along the way. If the robot is moving when this method is called, it stops and the current path is replaced by the new one.

Parameters:
path - to be followed.

clearPath

public void clearPath()
Clears the current path. If the robot is moving when this method is called, it stops;


getPath

public Path getPath()
Gets the current path

Returns:
the path

followPath

public void followPath(Path path)
Starts the robot traversing the path. This method is non-blocking.

Parameters:
path - to be followed.

followPath

public void followPath()
Starts the robot traversing the current path. This method is non-blocking;


singleStep

public void singleStep(boolean yes)
Controls whether the robot stops at each Waypoint; applies to the current path only. The robot will move to the next Waypoint if you call followPath().

Parameters:
yes - if true , the robot stops at each Waypoint.

goTo

public void goTo(Waypoint destination)
Starts the robot moving toward the destination. If no path exists, a new one is created consisting of the destination, otherwise the destination is added to the path. This method is non-blocking, and is equivalent to add(Waypoint); followPath();

Parameters:
destination - the waypoint to be reached

goTo

public void goTo(float x,
                 float y)
Starts the moving toward the destination Waypoint created from the parameters. If no path exists, a new one is created, otherwise the new Waypoint is added to the path. This method is non-blocking, and is equivalent to add(float x, float y); followPath();

Parameters:
x - coordinate of the destination
y - coordinate of the destination

goTo

public void goTo(float x,
                 float y,
                 float heading)
Starts the moving toward the destination Waypoint created from the parameters. If no path exists, a new one is created, otherwise the new Waypoint is added to the path. This method is non-blocking, and is equivalent to add(float x, float y); followPath();

Parameters:
x - coordinate of the destination
y - coordinate of th destination
heading - desired robot heading at arrival

rotateTo

public boolean rotateTo(double angle)
Rotates the robot to a new absolute heading. For example, rotateTo(0) will line the robot with the x-axis, while rotateTo(90) lines it with the y-axis. If the robot is currently on the move to a coordinate, this method will not attempt to rotate and it will return false.

Parameters:
angle - The absolute heading to rotate the robot to. Value is 0 to 360.
Returns:
true if the rotation happened, false if the robot was moving while this method was called.

addWaypoint

public void addWaypoint(Waypoint aWaypoint)
Adds a Waypoint to the end of the path.

Specified by:
addWaypoint in interface WaypointListener
Parameters:
aWaypoint - to be added

addWaypoint

public void addWaypoint(float x,
                        float y)
Constructs an new Waypoint from the parameters and adds it to the end of the path.

Parameters:
x - coordinate of the waypoint
y - coordinate of the waypoint

addWaypoint

public void addWaypoint(float x,
                        float y,
                        float heading)
Constructs an new Waypoint from the parameters and adds it to the end of the path.

Parameters:
x - coordinate of the waypoint
y - coordinate of the waypoint
heading - the heading of the robot when it reaches the waypoint

stop

public void stop()
Stops the robot. The robot will resume its path traversal if you call followPath().


getWaypoint

public Waypoint getWaypoint()
Returns the waypoint to which the robot is presently moving.

Returns:
the waypoint ; null if the path is empty.

pathCompleted

public boolean pathCompleted()
Returns true if the the final waypoint has been reached

Returns:
true if the path is completed

waitForStop

public boolean waitForStop()
Waits for the robot to stop for any reason ; returns true if the robot stopped at the final Waypoint of the path.

Returns:
true if the path is completed

isMoving

public boolean isMoving()
Returns true if the robot is moving toward a waypoint.

Returns:
true if moving.

pathGenerated

public void pathGenerated()
Description copied from interface: WaypointListener
Called when generation of the path is complete

Specified by:
pathGenerated in interface WaypointListener