public class BeaconPoseProvider extends java.lang.Object implements PoseProvider, MoveListener
A PoseProvider that uses beacon triangulation to pinpoint the pose (x, y, heading) of a robot. The starting position of the robot must be such that it roughly points in the direction of beacon 2, with beacon 1 to the right of the robot, and beacon 3 to the left. This ensures that when it performs the first scan, the beacons are scanned in order 1, 2, 3. After the first scan, the robot can move about on its own and will be able to scan them in any order.
After it does a scan, you will either hear a beep or a buzz. A beep means the calculation worked. A buzz means something went wrong with the calculation. This could be the beacon ordering is wrong, or it didn't scan three beacons in, or the calculated beacon pose deviated significantly from the estimated odometry pose. In this case, the results are thrown out, the odometry pose is used, and it will try another scan after the next move. If you hear a lot of buzzes in a row, it probably means it will not recover.
The class uses an OdometryPoseProvider internally for three reasons:
The downside of using an OdometryPoseProvider to assist with detecting pose is that the robot needs to be placed with an approximate heading prior to first scan. Also, if the robot is moved at any time (lifted and placed with a different heading), it probably will no longer be able to identify the beacons and become hopelessly lost. As long as the wheels are always on the ground, this should not happen.
Constructor and Description |
---|
BeaconPoseProvider(BeaconLocator bl,
BeaconTriangle bt,
MoveProvider mp)
Creates a PoseProvider using beacons.
|
BeaconPoseProvider(BeaconLocator bl,
BeaconTriangle bt,
MoveProvider mp,
int scanInterval)
Creates a PoseProvider using beacons.
|
Modifier and Type | Method and Description |
---|---|
static int |
getLargestIndex(java.util.ArrayList<java.lang.Double> angles) |
Pose |
getPose() |
void |
moveStarted(Move move,
MoveProvider mp)
Called when a Move Provider starts a move
|
void |
moveStopped(Move move,
MoveProvider mp)
Called by the movement provider when a move stops
|
void |
setPose(Pose aPose) |
public BeaconPoseProvider(BeaconLocator bl, BeaconTriangle bt, MoveProvider mp)
Creates a PoseProvider using beacons. The BeaconLocator must be capable of scanning the angles to 3 beacons. The BeaconTriangle contains the coordinates of the three beacons. The MoveProvider is simply a Pilot, such as DifferentialPilot, which is used to keep track of odometry.
By default, this class takes a scan every 5 moves of the robot.
bl
- a BeaconLocatorbt
- a BeaconTriangle containing the three beacon coordinatesmp
- a MoveProvider (Pilot) such as DifferentialPilot. Used for odometry.public BeaconPoseProvider(BeaconLocator bl, BeaconTriangle bt, MoveProvider mp, int scanInterval)
Creates a PoseProvider using beacons. The BeaconLocator must be capable of scanning the angles to 3 beacons. The BeaconTriangle contains the coordinates of the three beacons. The MoveProvider is simply a Pilot, such as DifferentialPilot, which is used to keep track of odometry.
bl
- a BeaconLocatorbt
- a BeaconTriangle containing the three beacon coordinatesmp
- a MoveProvider (Pilot) such as DifferentialPilot. Used for odometry.scanInterval
- The number of moves to make between each physical scan.public Pose getPose()
getPose
in interface PoseProvider
public static int getLargestIndex(java.util.ArrayList<java.lang.Double> angles)
public void setPose(Pose aPose)
setPose
in interface PoseProvider
public void moveStarted(Move move, MoveProvider mp)
MoveListener
moveStarted
in interface MoveListener
move
- the movementmp
- the movement providerpublic void moveStopped(Move move, MoveProvider mp)
MoveListener
moveStopped
in interface MoveListener
move
- the movementmp
- movement provider