#include <ArLocalizationTask.h>
Inheritance diagram for ArLocalizationTask:
The localization software can be used to localize a robot in a given map using the SICK laser rangefinder. The system is meant to be used along with ARIA.
The localization system uses Monte Carlo Localization Algorithm to accurately localize the robot in the given map using its laser data. The localization task is meant to be initialized and run in a separate thread in ARIA. It can be used with a real robot or with a simulator.
In order to get the localization task thread going, the ArLocalizationTask class needs an instantiated ArRobot, ArSick and a map file of the robot's environment. The output of the localization will be reflected directly in the pose of the ArRobot. (unless explicitly set to not do so)
The basic MCL localization system has also been augmented with a Kalman filter which will fuse the encoder data with the reflections from known laser reflecting landmarks (these are constructed with special reflective plastic manufactured by SICK) in the map. To use this functionality you will need a map with reflector positions in it, and will have to configure the SICK laser to return reflectance information. ARNL will triangulate the robot position if it is set to do so, otherwise it will incrementally incorporate the reflections from the landmarks using a EKF formulation. See the main ARNL overview page for details on this extra feature, including how to enable reflectance data in the SICK.
ArLocalizationTask automatically creates and runs its background thread when constructed.
See the Localization section for an overview of localization in ARNL.
Public Types | |
| enum | LocalizationState { , LOCALIZATION_FAILED, LOCALIZATION_SUCCESS, LOCALIZATION_COMPUTING, INVALID } |
| State of the path plan, accessible using getState(). More... | |
Public Member Functions | |
| ArLocalizationTask (ArRobot *robot, ArSick *laser, char *mapName) | |
| Base constructor with all the necessary inputs. | |
| ArLocalizationTask (ArRobot *robot, ArSick *laser, ArMap *ariaMap, bool noReflectors=false) | |
| Base constructor with all the necessary inputs. | |
| ~ArLocalizationTask (void) | |
| Base destructor. | |
| bool | localizeRobotInMapInit (ArPose given, int numSamples, double stdX, double stdY, double stdT, double thresFactor, bool warn=true, bool setInitializedToFalse=true) |
| Localization function mainly for initialization of robot at given pose. | |
| bool | localizeRobotInMapMoved (int numSamples, double distFactor, double angFactor, double thresFactor) |
| Function used to do the localization after motion (normally called automatically by the background task). | |
| bool | localizeRobotAtHomeBlocking (double distSpread, double angleSpread, double probThreshold) |
| Try initial localization at each home point at the map, and set the robot pose to the point with best score, using given parameter values instead of values previously configured. | |
| bool | localizeRobotAtHomeBlocking (double spreadX, double spreadY, double angleSpread, double probThreshold) |
| Try initial localization at each home point at the map, and set the robot pose to the point with best score, using given parameter values instead of values previously configured. | |
| bool | localizeRobotAtHomeBlocking () |
| Try initial localization at each home point at the map, and set the robot pose to the point with best score. | |
| bool | localizeRobotAtHomeNonBlocking (void) |
| Request that the task later localize the robot at a map home position, then return immediately. | |
| ArPose | getHomePose (void) |
| Gets the pose that robot was localized to. | |
| void | setForceUpdateParams (int numSamples, double xStd, double yStd, double tStd) |
| Sets the force update parameters. | |
| void | forceUpdatePose (ArPose forcePose) |
| Force an update in thread instead of waiting for distance-angle trigger. | |
| void | addFailedLocalizationCB (ArFunctor1< int > *functor) |
| Adds a callback which will be called when loca fails. | |
| void | remFailedLocalizationCB (ArFunctor1< int > *functor) |
| Removes a callback. | |
| bool | setGridResolution (double res, ArMap *ariaMap) |
| Sets the occupancy grid with the new map. | |
| void | setFailedCallBack (ArFunctor1< int > *fcb) |
| Sets tracking failed Callback. | |
| LocalizationState | getState (void) |
| Gets the state of localization. | |
| bool | getIdleFlag (void) |
| Gets the idle flag. | |
| bool | getReloadingMapFlag (void) |
| Gets the map reloading flag. | |
| ArMap * | readMapFromFile (char *mapName) |
| Read the Map data from a map file. | |
| ArMap * | readAriaMap (ArMap *ariaMap) |
| Read the Map data from ArMap. | |
| bool | loadParamFile (const char *file) |
| Load parameters from the given filename into ArConfig. | |
| bool | saveParams (char *filename) |
| Saves all parameter values from ArConfig to the given file. | |
Modifiers for configuration values. | |
| void | setTriggerDelR (double) |
| Sets the motion trigger value for distance. | |
| void | setTriggerDelT (double) |
| Sets the motion trigger value for angle. | |
| void | setTriggerTimeFlag (bool) |
| Sets the flag to trigger on idle. | |
| void | setTriggerTime (double) |
| Sets the idle trigger time in millisecs. | |
| void | setTriggerTimeX (double) |
| Sets the X range when localization triggers due to idle time. | |
| void | setTriggerTimeY (double) |
| Sets the Y range when localization triggers due to idle time. | |
| void | setTriggerTimeTh (double) |
| Sets the Theta range when localization triggers due to idle time. | |
| void | setNumSamples (int numSamples) |
| Set the number of samples to use in the MCL. | |
| void | setCurrentNumSamples (int numSamples) |
| Set the variable number of samples (usually adjusted automatically). | |
| void | setPassThreshold (double r) |
| Sets the pass threshold for fraction of laser points matched to env. | |
| void | setSensorBelief (double sensorBelief) |
| Sets the sensor belief. | |
| void | setCurrentLocaPose (double x, double y, double th) |
| Sets the current pose. | |
| void | setCurrentLocaPose (ArPose p) |
| Sets the current pose. | |
| void | setVerboseFlag (bool a) |
| Sets the verbose flag value. (for debugging). | |
| void | setAngleIncrement (double i) |
| Sets the laser increments. | |
| void | setKillThreshold (double i) |
| void | setRecoverOnFailedFlag (bool f) |
| Sets the recoverOnFailed flag. | |
| void | setIdleFlag (bool f) |
| Sets the idle flag. | |
| void | setReloadingMapFlag (bool f) |
| Sets the map reloading flag. | |
| void | setFuseAllSensorsFlag (bool f) |
| Sets the flag to fuse all sensors. | |
| void | setReflectorVar (double f) |
| Sets the variance in the XY for the reflector centers. | |
| void | setQxx (double f) |
| Sets the variance in the X for the Q matrix. | |
| void | setQyy (double f) |
| Sets the variance in the Y for the Q matrix. | |
| void | setQtt (double f) |
| Sets the variance in the Theta for the Q matrix. | |
| void | setReflectorMatchDist (double f) |
| Sets the radius to search for a reflector to match a given reflection. | |
| void | setReflectorMaxRange (double f) |
| Sets the maximum distance the reflector can be seen. | |
| void | setReflectorMaxAngle (double f) |
| Sets the maximum incident angle the reflector can be seen. | |
| void | setUseReflectorCenterFlag (bool f) |
| Sets the flag to use the reflector centers instead of all reflections. | |
| void | setTriangulateFlag (bool f) |
| Sets the flag to allow triangulation with reflectors. | |
| void | setTriangulateScoreThreshold (double f) |
| Sets the value at which triangulation will be attempted. | |
| void | setBypassMCLFlag (bool f) |
| Sets the flag which decides to bypasses MCL and use reflectors. | |
Accessors for current configuration values. | |
These values are normally set via ArConfig (the "Localization" section) by loading a file or other means, or by calling the modifier functions above. | |
| void | getForceUpdateParams (ArPose &forcePose, int &numSamples, double &xStd, double &yStd, double &tStd) |
| Get the params to do the localization from the class. | |
| bool | getVerboseFlag (void) |
| Gets the verbose flag. (for debugging only). | |
| bool | getInitializedFlag (void) |
| Gets the initialized flag indicating if localization thread is on. | |
| int | getNumSamples (void) |
| Get the maximum number of samples used in the MCL. | |
| int | getCurrentNumSamples (void) |
| Get the variable number of samples if adjusted during move. | |
| ArPose | getRobotMaxProbPose (void) |
| Get the current computed best robot pose. | |
| double | getTriggerDelR (void) |
| Gets the min distance to localize. | |
| double | getTriggerDelT (void) |
| Gets the min angle to localize. | |
| bool | getTriggerTimeFlag (void) |
| Gets the flag indicating if localization should trigger on idle. | |
| double | getTriggerTime (void) |
| Gets the min time in millisecs to be idle. | |
| double | getTriggerTimeX (void) |
| Gets the X range of samples when localization triggers on idle. | |
| double | getTriggerTimeY (void) |
| Gets the Y range of samples when localization triggers on idle. | |
| double | getTriggerTimeTh (void) |
| Gets the Theta range of samples when localization triggers on idle. | |
| double | getPassThreshold (void) |
| Gets the Pass threshold for localization success. | |
| double | getLocalizationScore (void) |
| Gets the higher of MCL and Reflector based localization. | |
| double | getMCLocalizationScore (void) |
| Gets the localization score based on Monte Carlo only. | |
| double | getRefLocalizationScore (void) |
| Gets the localization score based on reflectors only. | |
| double | getSensorBelief (void) |
| Gets the sensor belief. | |
| ArPose | getCurrentLocaPose (void) |
| Gets the last successful MCLocalized pose. | |
| double | getStdX (void) const |
| double | getStdY (void) const |
| double | getStdTh (void) const |
| double | getErrorMmPerMm (void) const |
| double | getErrorDegPerDeg (void) const |
| double | getErrorDegPerMm (void) const |
| double | getSensorBelief (void) const |
| double | getPeakFactor (void) const |
| double | getOccThreshold (void) const |
| double | getGridRes (void) const |
| char * | getMapName (void) |
| double | getPeturbRangeX (void) const |
| double | getPeturbRangeY (void) const |
| double | getPeturbRangeTh (void) const |
| double | getFailedRangeX (void) const |
| double | getFailedRangeY (void) const |
| double | getFailedRangeTh (void) const |
| double | getPeakStdX (void) const |
| double | getPeakStdY (void) const |
| double | getPeakStdTh (void) const |
| double | getAngleIncrement (void) const |
| double | getKillThreshold (void) const |
| ArMap * | getAriaMap (void) |
| int | getBufferSize (void) |
| Scan Buffer size. (internal laser scan buffer). | |
| std::vector< ArPose > | getXYBuffer (void) |
| Scan Buffer XY. | |
| ArPose | getBufferPose (void) |
| Scan Buffer pose taken. (internal laser scan buffer). | |
| ArOccGrid * | getOccGridPtr (void) |
| Returns pointer to the occgrid. | |
| std::list< ArPose > | getCurrentSamplePoses (void) |
| Gets the pose samples for client. | |
| bool | getRecoverOnFailedFlag (void) |
| Gets the recoverOnFailed flag. | |
| bool | getIgnoreIllegalPoseFlag (void) |
| Gets the flag which allows poses overlaping occupied points on map. | |
| bool | getAdjustNumSamplesFlag (void) |
| Gets the flag which allows for changing numSamples with loca score. | |
| int | getMinNumSamples (void) |
| Gets the minimum no of samples the localization will drop to. | |
| double | getNumSamplesAngleFactor (void) |
| Gets the rotation factor for adjusting no of samples with score. | |
| bool | getSensorSetFlag (void) |
| Gets the value of the sensor set flag which indicates new cycle. | |
| bool | getFuseAllSensorsFlag (void) |
| Gets the flag to fuse all sensors. | |
| double | getReflectorVar (void) |
| Gets the variance in the XY for the reflector centers. | |
| double | getQxx (void) |
| Gets the variance in the X for the Q matrix. | |
| double | getQyy (void) |
| Gets the variance in the Y for the Q matrix. | |
| double | getQtt (void) |
| Gets the variance in the Theta for the Q matrix. | |
| double | getReflectorMatchDist (void) |
| Gets the variance in the XY for the reflector centers. | |
| double | getReflectorMaxRange (void) |
| Gets the maximum distance the reflector can be seen. | |
| double | getReflectorMaxAngle (void) |
| Gets the maximum incident angle the reflector can be seen. | |
| bool | getUseReflectorCenterFlag (void) |
| Gets the flag to use the reflector centers instead of all reflections. | |
| bool | getTriangulateFlag (void) |
| Gets the flag which allows triangulation. | |
| double | getTriangulateScoreThreshold (void) |
| Gets the value at which triangulation will be attempted. | |
| bool | getBypassMCLFlag (void) |
| Gets the flag which decides to bypasses MCL and use reflectors. | |
Functions used internally by ARNL | |
For internal use only.
| |
| bool | fillHistogram (double *&hist, double *&cumSum, int &numSamples) |
| Fill prob distribution histogram for debugging. | |
| bool | scanToGlobalCoords (ArPose robPose, std::vector< ArPose > &xyLrf) |
| Convert the laser data to x and y coord array. | |
| ArTime | getLastLocaTime (void) |
| Gets the last loca time to now. | |
| bool | setLocaParams (double xStd, double yStd, double tStd, double kMmPerMm, double kDegPerDeg, double kDegPerMm, double sensorBelief) |
| Set all the parameters for localization. | |
| void | setCorrectRobotFlag (bool a) |
| Sets the flag deciding whether to reflect localized pose onto the robot. | |
| void | setIgnoreIllegalPoseFlag (bool a) |
| Sets the flag which allows poses overlaping occupied points on map. | |
| void | setAdjustNumSamplesFlag (bool a) |
| Sets the flag which changes numSamples with the localization score. | |
| void | setMinNumSamples (int n) |
| Sets the minimum no of samples the localization will drop to. | |
| void | setNumSamplesAngleFactor (double d) |
| Sets the rotation factor for adjusting no of samples with score. | |
| void | setLastLocaTimeToNow (void) |
| Sets the last loca time to now. | |
| void | setSensorSetFlag (bool p) |
| Sets the value of the sensor set flag which indicates new cycle. | |
ArNetworking callback methods | |
(used by server classes in ArServerClasses.cpp) | |
| void | drawRangePoints (ArServerClient *client, ArNetPacket *packet) |
| Draws range data used for localization. | |
| void | drawReflectorRays (ArServerClient *client, ArNetPacket *packet) |
| Draw rays from the robot to the reflector. | |
| void | drawTriangulateRays (ArServerClient *client, ArNetPacket *packet) |
| Draw rays forming triagulation. | |
| void | drawSamplePoses (ArServerClient *client, ArNetPacket *packet) |
| Draws debugging objects if filled. | |
| void | drawSampleBounds (ArServerClient *client, ArNetPacket *packet) |
| Draws the sample bounds. | |
Protected Member Functions | |
| void | basicInitialization (ArRobot *robot, ArSick *laser) |
| Basic initializer (internal use only). | |
| virtual void * | runThread (void *ptr) |
| Function used to run the task as a thread. | |
| bool | getLocalizedFlag (void) |
| Gets the localized flag. (do not use). | |
| bool | getLocalizingFlag (void) |
| Gets the localizing flag value. To avoid multiple localizing threads. | |
| bool | getRobotMovedFlag (void) |
| Gets the robot moved flag value. (Moved after last localization). | |
| bool | getCorrectRobotFlag (void) |
| Gets the flag indicating if localization result will be set on robot. | |
| void | setLocalizedFlag (bool a) |
| Sets the localized flag value. (do not use). | |
| void | setInitializedFlag (bool a) |
| Sets the initialized flag value indicating robot was localized or not. | |
| void | setRobotMovedFlag (bool a) |
| Sets the robot moved flag to set off MCL. | |
| bool | initializeSamples (int numSamples) |
| Initialize the MCL samples. | |
| bool | scanLaserIntoArray (void) |
| Get the laser buffer into data structure. | |
| ArOccGrid * | initializeOccGrid (double res, ArMap *ariaMap, bool lockMap=true) |
| Set resolution of the occupancy grid and fill it. | |
| void | killBadSamples (double obsThreshold) |
| Legalize samples to eliminate illegal location such as on obstacles. | |
| void | saveSamples (bool saveFile=false) |
| Save samples for backup. | |
| int | findBestPoses (ArRobotPoseSamples *mrsp, double factor) |
| Find the best poses by looking at prob distribution peaks. | |
| int | getNumBestPoses (ArRobotPoseSamples *mrsp) |
| Get the number of peaks. | |
| bool | findAllStatistics (double &xMean, double &yMean, double &thMean, double &xStd, double &yStd, double &tStd) |
| Compute statistics of samples after localization. | |
| unsigned int | getLocaTime () |
| Return time of last good localization. | |
| void | setMotionErrorParam (int index, double value) |
| Set motion error params. | |
| double | getMotionErrorParam (int index) |
| Get motion error params. | |
| void | robotCallBack (void) |
| The sensor interpretation callback. Called every 100msec. | |
| bool | configureLaser (void) |
| Needed if the laser does not connect first. | |
| bool | reconfigureLocalization (void) |
| Needed if the params are changed or loaded again. | |
| void | setupLocalizationParams (bool noReflectors=false) |
| Setup the path planing params with values from ArConfig (mostly for internal use only). | |
| void | failedLocalization (int times) |
| Actual thing that gets called when localization failed. | |
| int | dynamicallyAdjustNumSamples (double dr, double dt) |
| Change the sample size to reflect localization score. | |
| void | mapChanged (void) |
| Function for the things to do if map changes. | |
| bool | findPoseFromLandmarks (const std::vector< ArPose > &global, const std::vector< ArPose > &local, ArPose &rPose, ArPose &stdDev) |
| Solve the triangulation eqn. | |
| bool | kalmanFilter (const std::vector< ArPose > &global, const std::vector< ArPose > &local, ArPose &pose, ArPose &deltaPose, ArPose &delta, bool failedLocalization, ArPose &mean, ArPose &stdDev, ArPose &maxInnov, double &refScore) |
| Solve the Kalman thing. | |
| int | getReflectionCoords (ArPose &poseTaken, ArPose &rPose, std::vector< ArPose > &gList, std::vector< ArPose > &lList) |
| Find the reflectors in local and global coords. | |
| int | getReflectorCenterCoords (ArPose &poseTaken, ArPose &rPose, std::vector< ArPose > &gList, std::vector< ArPose > &lList) |
| Find the reflector centers in local and global coords. | |
| void | fuseAllSensors (bool failedLocalization) |
| Fuse all the sensor information. | |
| void | setFlagsAndCallbacksOnFail (int ntimes) |
| Set the flags and call the right callbacks when loca fails ntimes. | |
|
|
State of the path plan, accessible using getState().
|
|
|
Try initial localization at each home point at the map, and set the robot pose to the point with best score. Attempt to localize the robot in the most likely home position: first it checks the robot's current pose and then checks all of the home positions in the map. The position with the highest score is then used. Call this function after the localization thread has been initialized but needs to be reset, or an initial localization (i.e. at program start up) is needed. This function blocks while the localization takes place, and returns after it either succeeds (Either localization at a home position succeeds, or all fail.)
|
|
||||||||||||||||
|
Try initial localization at each home point at the map, and set the robot pose to the point with best score, using given parameter values instead of values previously configured.
|
1.4.0