Package lejos.robotics.localization

Class MCLPoseProvider

  • All Implemented Interfaces:
    PoseProvider, MoveListener, Transmittable
    public class MCLPoseProvider
extends java.lang.Object
implements PoseProvider, MoveListener, Transmittable
    Maintains an estimate of the robot pose using sensor data. It uses Monte Carlo Localization (See section 8.3 of "Probabilistic Robotics" by Thrun et al.
    Uses a MCLParticleSet to represent the probability distribution of the estimated pose. It uses a MoveProvider to supply odometry data whenever a movement is completed, from which the Pose of each particle is updated. It then uses a RangeScanner to provide RangeReadings which are used, together with the RangeMap to calculate the probability weight of each MCLParticle .
    Author:
    Lawrie Griffiths and Roger Glassey

    • Method Summary

      All Methods Instance Methods Concrete Methods 
      Modifier and Type Method Description
      void dumpObject​(java.io.DataOutputStream dos)
      Dump the serialized estimate of pose to a data output stream
      void estimatePose()
      Estimate pose from weighted average of the particles Calculate statistics
      void generateParticles()
      Generate a new particle set, uniformly distributed within the map, and uniformly distributed heading.
      Rectangle2D getErrorRect()
      Returns the minimum rectangle enclosing all the particles
      Pose getEstimatedPose()  
      float getMaxX()
      Returns the maximum value of X in the particle set
      float getMaxY()
      Returns the maximum value of Y in the particle set;
      float getMinX()
      Returns the minimum value of X in the particle set;
      float getMinY()
      Returns the minimum value of Y in the particle set;
      MCLParticleSet getParticles()
      Returns the particle set
      Pose getPose()
      Returns the best best estimate of the current pose;
      RangeReadings getRangeReadings()
      Returns most recent range readings
      RangeReadings getReadings()
      Get the current range readings
      RangeScanner getScanner()
      Returns the range scanner
      float getSigmaHeading()
      Returns the standard deviation of the heading values in the particle set;
      float getSigmaX()
      Returns the standard deviation of the X values in the particle set;
      float getSigmaY()
      Returns the standard deviation of the Y values in the particle set;
      float getXRange()
      Returns the difference between max X and min X
      float getYRange()
      Return difference between max Y and min Y
      boolean incompleteRanges()
      returns range scanner failure status
      boolean isBusy()
      returns true if particle weights are being updated.
      boolean isLost()
      returns lost status - all particles have very low probability weights
      boolean isUpdated()
      Returns update success flag
      void loadObject​(java.io.DataInputStream dis)
      Load serialized estimated pose from a data input stream
      void moveStarted​(Move event, MoveProvider mp)
      Required by MoveListener interface; does nothing
      void moveStopped​(Move event, MoveProvider mp)
      Required by MoveListener interface.
      void setDebug​(boolean on)
      Set debugging on or off
      void setInitialPose​(Pose aPose, float radiusNoise, float headingNoise)
      Generates an initial particle set in a circular normal distribution, centered on aPose.
      void setInitialPose​(RangeReadings readings, float sigma)
      Generates an initial particle set using the range readings.
      void setMap​(RangeMap map)
      Associates a map with the MCLPoseProvider (for example a map send from the PC).
      void setParticles​(MCLParticleSet particles)
      Associate a particle set with the MCLPoseProvider (e.g.
      void setPose​(Pose aPose)
      set the initial pose cloud with radius noise 1 and heading noise 1
      boolean update()
      Calls range scanner to get range readings, calculates the probabilities of each particle from the range readings and the map and calls resample(()
      boolean update​(RangeReadings readings)
      Calculates particle weights from readings, then resamples the particle set;

      • Methods inherited from class java.lang.Object

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

    • Constructor Detail

      • MCLPoseProvider

        public MCLPoseProvider​(MoveProvider mp,
                       RangeScanner scanner,
                       RangeMap map,
                       int numParticles,
                       int border)
        Allocates a new MCLPoseProvider.
        Parameters:
        mp - - the MoveProivder
        scanner - - the RangeScanner
        map - - the RangeMap
        numParticles - number of particles
        border - of the map

      • MCLPoseProvider

        public MCLPoseProvider​(RangeMap map,
                       int numParticles,
                       int border)
        Constructor for use on PC
        Parameters:
        map - the RangeMap
        numParticles - the numbers of particles
        border - of the map

    • Method Detail

      • setMap

        public void setMap​(RangeMap map)
        Associates a map with the MCLPoseProvider (for example a map send from the PC).
        Parameters:
        map - the RangeMap

      • setInitialPose

        public void setInitialPose​(Pose aPose,
                           float radiusNoise,
                           float headingNoise)
        Generates an initial particle set in a circular normal distribution, centered on aPose.
        Parameters:
        aPose - - center of the cloud
        radiusNoise - - standard deviation of the radius of the cloud
        headingNoise - - standard deviation of the heading;

      • setInitialPose

        public void setInitialPose​(RangeReadings readings,
                           float sigma)
        Generates an initial particle set using the range readings. The particles have a significant probability weight given the readings.
        Parameters:
        readings -
        sigma - range reading noise standard deviation.

      • setDebug

        public void setDebug​(boolean on)
        Set debugging on or off
        Parameters:
        on - true = on, false = off

      • setPose

        public void setPose​(Pose aPose)
        set the initial pose cloud with radius noise 1 and heading noise 1
        Specified by:
        setPose in interface PoseProvider

      • getParticles

        public MCLParticleSet getParticles()
        Returns the particle set
        Returns:
        the particle set

      • getReadings

        public RangeReadings getReadings()
        Get the current range readings
        Returns:
        the RangeReadings object

      • setParticles

        public void setParticles​(MCLParticleSet particles)
        Associate a particle set with the MCLPoseProvider (e.g. particles sent from the PC)
        Parameters:
        particles - the particle set

      • generateParticles

        public void generateParticles()
        Generate a new particle set, uniformly distributed within the map, and uniformly distributed heading.

      • moveStarted

        public void moveStarted​(Move event,
                        MoveProvider mp)
        Required by MoveListener interface; does nothing
        Specified by:
        moveStarted in interface MoveListener
        Parameters:
        event - the movement
        mp - the movement provider

      • moveStopped

        public void moveStopped​(Move event,
                        MoveProvider mp)
        Required by MoveListener interface. The pose of each particle is updated using the odometry data of the Move object.
        Specified by:
        moveStopped in interface MoveListener
        Parameters:
        event - the move just completed
        mp - the MoveProvider

      • update

        public boolean update()
        Calls range scanner to get range readings, calculates the probabilities of each particle from the range readings and the map and calls resample(()
        Returns:
        true if update was successful

      • update

        public boolean update​(RangeReadings readings)
        Calculates particle weights from readings, then resamples the particle set;
        Parameters:
        readings -
        Returns:
        true if update was successful.

      • isUpdated

        public boolean isUpdated()
        Returns update success flag
        Returns:
        true if update is successful

      • isLost

        public boolean isLost()
        returns lost status - all particles have very low probability weights
        Returns:
        true if robot is lost

      • incompleteRanges

        public boolean incompleteRanges()
        returns range scanner failure status
        Returns:
        true if range readings are incomplete

      • getXRange

        public float getXRange()
        Returns the difference between max X and min X
        Returns:
        the difference between min and max X

      • getYRange

        public float getYRange()
        Return difference between max Y and min Y
        Returns:
        difference between max and min Y

      • getPose

        public Pose getPose()
        Returns the best best estimate of the current pose;
        Specified by:
        getPose in interface PoseProvider
        Returns:
        the estimated pose

      • getEstimatedPose

        public Pose getEstimatedPose()

      • estimatePose

        public void estimatePose()
        Estimate pose from weighted average of the particles Calculate statistics

      • getRangeReadings

        public RangeReadings getRangeReadings()
        Returns most recent range readings
        Returns:
        the range readings

      • getErrorRect

        public Rectangle2D getErrorRect()
        Returns the minimum rectangle enclosing all the particles
        Returns:
        rectangle : the minimum rectangle enclosing all the particles

      • getMaxX

        public float getMaxX()
        Returns the maximum value of X in the particle set
        Returns:
        max X

      • getMinX

        public float getMinX()
        Returns the minimum value of X in the particle set;
        Returns:
        minimum X

      • getMaxY

        public float getMaxY()
        Returns the maximum value of Y in the particle set;
        Returns:
        max y

      • getMinY

        public float getMinY()
        Returns the minimum value of Y in the particle set;
        Returns:
        minimum Y

      • getSigmaX

        public float getSigmaX()
        Returns the standard deviation of the X values in the particle set;
        Returns:
        sigma X

      • getSigmaY

        public float getSigmaY()
        Returns the standard deviation of the Y values in the particle set;
        Returns:
        sigma Y

      • getSigmaHeading

        public float getSigmaHeading()
        Returns the standard deviation of the heading values in the particle set;
        Returns:
        sigma heading

      • getScanner

        public RangeScanner getScanner()
        Returns the range scanner
        Returns:
        the range scanner

      • dumpObject

        public void dumpObject​(java.io.DataOutputStream dos)
                throws java.io.IOException
        Dump the serialized estimate of pose to a data output stream
        Specified by:
        dumpObject in interface Transmittable
        Parameters:
        dos - the data output stream
        Throws:
        java.io.IOException

      • loadObject

        public void loadObject​(java.io.DataInputStream dis)
                throws java.io.IOException
        Load serialized estimated pose from a data input stream
        Specified by:
        loadObject in interface Transmittable
        Parameters:
        dis - the data input stream
        Throws:
        java.io.IOException

      • isBusy

        public boolean isBusy()
        returns true if particle weights are being updated. The robot should not move while this is happening otherwise the prediction from odometry data may introduce errors into the updating.
        Returns:
        true if weight update is in progress.