#include <ArSonarLocalizationTask.h>
Inheritance diagram for ArSonarLocalizationTask:
The sonar localization task can be used to localize a robot in a given line map using the ring of sonar rangefinders.
ARNL's sonar localization uses Monte Carlo Localization algorithm to accurately localize the robot in the given line map using its sonar 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 ArSonarLocalizationTask class needs an instantiated ArRobot, ArSonarDevice and a line map of the robot's environment in an ArMap object (or loaded from file). The output of the localization will be reflected directly in the pose of the ArRobot. (unless explicitly set to not do so)
ArSonarLocalizationTask automatically creates and runs its background thread when constructed.
See the Sonar Localization section for an overview of the sonar localization task in ARNL or SONARNL.
Public Types | |
| enum | SonarLocalizationState { , LOCALIZATION_FAILED, LOCALIZATION_SUCCESS, LOCALIZATION_COMPUTING, INVALID } |
| State of the path plan, accessible using getState(). More... | |
Public Member Functions | |
| ArSonarLocalizationTask (ArRobot *robot, ArSonarDevice *sonar, char *mapName) | |
| Base constructor with all the necessary inputs. | |
| ArSonarLocalizationTask (ArRobot *robot, ArSonarDevice *sonar, ArMap *ariaMap) | |
| Base constructor with all the necessary inputs. | |
| ~ArSonarLocalizationTask (void) | |
| Base destructor. | |
| bool | localizeRobotInMapInit (ArPose given, int numSamples, double stdX, double stdY, double stdT, double thresFactor, bool warn=true, bool setInitializedToFalse=true) |
| bool | localizeRobotInMapMoved (int numSamples, double distFactor, double angFactor, double thresFactor) |
| Function used to do the localization after motion (normally automatic). | |
| bool | localizeRobotAtHomeBlocking (double distSpread, double angleSpread, double probThreshold) |
| Try initial localization at each home point in the map, 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 in the map, 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 home position, and 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. | |
| void | setFailedCallBack (ArFunctor1< int > *fcb) |
| Sets tracking failed Callback. | |
| SonarLocalizationState | 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 an ArMap object. | |
| bool | loadParamFile (const char *file) |
| load param file when Aria config sets it off. | |
| bool | saveParams (char *filename) |
| Saves all default params to the param 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 | setNumSamplesAngleFactor (double f) |
| Sets the rotation factor for adjusting no of samples with score. | |
| void | setPassThreshold (double r) |
| Sets the pass threshold for fraction of sonar 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 | 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 | setSonarMaxRange (double f) |
| Sets the sonar range. | |
Accessors for confiuration values. | |
These values are normally set via ArConfig (the "Localization" section) by loading a file or other means, or by manually 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 score for the localization as fraction of matched sonar pts. | |
| 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 | getPeakFactor (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 |
| ArMap * | getAriaMap (void) |
| int | getBufferSize (void) |
| Scan Buffer size. (internal sonar scan buffer). | |
| std::vector< ArPose > | getXYBuffer (void) |
| Scan Buffer XY. | |
| ArPose | getBufferPose (void) |
| Scan Buffer pose taken. (internal sonar scan buffer). | |
| std::list< ArPose > | getCurrentSamplePoses (void) |
| Gets the pose samples for client. | |
| bool | getRecoverOnFailedFlag (void) |
| Gets the recoverOnFailed flag. | |
| double | getNumSamplesAngleFactor (void) |
| Gets the angle factor when rotating which multiplies the no of samples. | |
| double | getSonarMaxRange (void) |
| Gets the sonar range. | |
| double | getSonarMinLineSize (void) |
| Gets the min line size;. | |
| bool | getAdjustNumSamplesFlag (void) |
| Gets the flag which allows for changing numSamples with loca score. | |
| int | getMinNumSamples (void) |
| Gets the min no of samples if they can be adjusted. | |
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 sonar data to x and y coord array. | |
| 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 | setAdjustNumSamplesFlag (bool a) |
| Sets the flag which changes numSamples with the localization score. | |
| void | setMinNumSamples (int f) |
| Sets the min no of samples if they are adjustable with loca score. | |
| void | setLastLocaTimeToNow (void) |
| Sets the last loca time to now. | |
| ArTime | getLastLocaTime (void) |
| Gets the last loca time to now. | |
ArNetworking callback methods | |
(used by server classes in ArServerClasses.cpp) | |
| void | drawRangePoints (ArServerClient *client, ArNetPacket *packet) |
| Draws range data used for localization. | |
| void | drawClosestLines (ArServerClient *client, ArNetPacket *packet) |
| Draws the closest map lines to robot. | |
| void | drawBestRays (ArServerClient *client, ArNetPacket *packet) |
| Draws the sonar rays. | |
| void | drawSampleRays (ArServerClient *client, ArNetPacket *packet) |
| Draws the sonar rays. | |
| void | drawSonarRays (ArServerClient *client, ArNetPacket *packet) |
| Draws the sonar rays. | |
| void | drawIntersections (ArServerClient *client, ArNetPacket *packet) |
| Draws intersection to closest lines. | |
| void | drawHistogram (ArServerClient *client, ArNetPacket *packet) |
| Draws the histogram. | |
| void | drawBestPoses (ArServerClient *client, ArNetPacket *packet) |
| Draws the histogram. | |
| void | drawSamplePoses (ArServerClient *client, ArNetPacket *packet) |
| Draws the samples. | |
Protected Member Functions | |
| void | basicInitialization (ArRobot *robot, ArSonarDevice *sonar) |
| Basic initializer (internal use only). | |
| void | setFlagsAndCallbacksOnFail (int ntimes) |
| Common segment to call after fail. | |
| 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. | |
| 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 | configureSonar (void) |
| Needed if the sonar does not connect first. | |
| bool | reconfigureLocalization (void) |
| Needed if the params are changed or loaded again. | |
| void | setupLocalizationParams (void) |
| Setup the path planing params with the aria config thing. | |
| 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. | |
| std::vector< ArLineSegment > | findClosestLines (ArPose robotPose, double range) |
| Find the closest lines to a robot pose. | |
| std::vector< ArLineSegment > | getClosestLines (void) |
| Gets the closest lines. | |
|
|
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 in the map, set the robot pose to the point with best score, using given parameter values instead of values previously configured.
|
|
||||||||||||||||||||||||||||||||||||
|
Localization function mainly for initialization of robot at a given pose. |
1.4.0