#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.
Public Member Functions | |
ArSonarLocalizationTask (ArRobot *robot, ArSonarDevice *sonar, char *mapName) | |
Base constructor with all the necessary inputs. | |
ArSonarLocalizationTask (ArRobot *robot, ArSonarDevice *sonar, ArMapInterface *ariaMap) | |
Base constructor with all the necessary inputs. | |
virtual | ~ArSonarLocalizationTask (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 a given pose. | |
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. | |
virtual ArPose | getRobotHome (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. | |
virtual LocalizationState | getState (void) |
Gets the state of localization. | |
bool | getIdleFlag (void) |
Gets the idle flag. | |
bool | getReloadingMapFlag (void) |
Gets the map reloading flag. | |
ArMapInterface * | readMapFromFile (char *mapName) |
Read the Map data from a map file. | |
ArMapInterface * | readAriaMap (ArMapInterface *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. | |
Normally thes evalues are set via ArConfig (e.g.
from a loaded file or other source) in the "Localization" section, and these methods would only be used internally by ARNL. However, they can be used if you are not using ArConfig or wish to override a parameter. | |
void | setTriggerDelR (double tr) |
Sets the motion trigger value for distance. | |
void | setTriggerDelT (double tt) |
Sets the motion trigger value for angle. | |
void | setTriggerTimeFlag (bool tt) |
Sets the flag to trigger on idle. | |
void | setTriggerTime (double tt) |
Sets the idle trigger time in millisecs. | |
void | setTriggerTimeX (double tt) |
Sets the X range when localization triggers due to idle time. | |
void | setTriggerTimeY (double tt) |
Sets the Y range when localization triggers due to idle time. | |
void | setTriggerTimeTh (double tt) |
Sets the Theta range when localization triggers due to idle time. | |
void | setNumSamples (int n) |
Set the number of samples to use in the MCL. | |
void | setNumSamplesAtInit (int n) |
Set the number of samples to use during initialization. | |
void | setCurrentNumSamples (int n) |
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 f) |
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 f) |
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 | getNumSamplesAtInit (void) |
Get the number of samples used in the MCL during initialization. | |
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 | getUsingPassThreshold (void) |
Gets the Pass threshold for localization success being used right now. | |
void | setTempPassThreshold (double passThreshold) |
Sets the Pass threshold to use until it is reset to config. | |
double | getTempPassThreshold (void) |
Gets the temporary pass threshold to use until it is reset to config. | |
void | clearTempPassThreshold (void) |
Resets the Pass threshold to config. | |
virtual 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 |
ArMapInterface * | 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 | |
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 | setAdjustNumSamplesFlag (bool f) |
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. | |
bool | findMCLMeanVar (ArPose &mean, ArMatrix &Var) |
Finds the mean and var of MCL after moving to present pose. | |
virtual bool | findLocalizationMeanVar (ArPose &mean, ArMatrix &Var) |
The actual mean var calculator for the virtual in the base class. | |
virtual void | setCorrectRobotFlag (bool f) |
Sets the flag deciding whether to reflect localized pose onto the robot. | |
virtual void | setRobotPose (ArPose pose, ArPose spread=ArPose(0, 0, 0), int nSam=0) |
Used to set the robot pose usually at the start of localization. | |
virtual bool | getRobotIsLostFlag (void) |
Gets the lost flag. | |
virtual bool | localizeRobotAtHomeBlocking (double distSpread, double angleSpread) |
Localize the robot at known homes. | |
virtual void | setLocalizationIdle (bool f) |
Set the localization idle. | |
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. | |
void | drawMCLVariance (ArServerClient *client, ArNetPacket *packet) |
Draws the sample bounds. | |
Protected Member Functions | |
std::vector< ArPose > | makeMeanVarPoints (ArPose &mean, ArMatrix &Var) |
void | basicInitialization (ArRobot *robot, ArSonarDevice *sonar) |
void | setFlagsAndCallbacksOnFail (int ntimes) |
virtual void * | runThread (void *ptr) |
bool | correctPoseFromSensor (int sonarStart, int sonarEnd, double maxRange, std::vector< ArLineSegment > lines) |
bool | initializeSamples (int numSamples) |
void | killBadSamples (double obsThreshold) |
void | saveSamples (bool saveFile=false) |
int | findBestPoses (ArRobotPoseSamples *mrsp, double factor) |
bool | findAllStatistics (double &xMean, double &yMean, double &thMean, double &xStd, double &yStd, double &tStd) |
void | setMotionErrorParam (int index, double value) |
double | getMotionErrorParam (int index) |
void | robotCallBack (void) |
bool | configureSonar (void) |
bool | reconfigureLocalization (void) |
void | setupLocalizationParams (void) |
void | failedLocalization (int times) |
int | dynamicallyAdjustNumSamples (double dr, double dt) |
void | mapChanged (void) |
std::vector< ArLineSegment > | findClosestLines (ArPose robotPose, double range) |
std::vector< ArLineSegment > | getClosestLines (void) |
ArSonarLocalizationTask::ArSonarLocalizationTask | ( | ArRobot * | robot, | |
ArSonarDevice * | sonar, | |||
char * | mapname | |||
) |
Base constructor with all the necessary inputs.
Constructor.
robot,: | Robot data structure. | |
sonar,: | Sonar data structure. | |
mapname,: | Map file containing environment map. |
ArSonarLocalizationTask::ArSonarLocalizationTask | ( | ArRobot * | robot, | |
ArSonarDevice * | sonar, | |||
ArMapInterface * | ariaMap | |||
) |
Base constructor with all the necessary inputs.
Constructor.
robot,: | Robot data structure. | |
sonar,: | Sonar data structure. | |
ariaMap,: | Aria map containing environment map. |
ArSonarLocalizationTask::~ArSonarLocalizationTask | ( | void | ) | [virtual] |
Base destructor.
Destructor.
bool ArSonarLocalizationTask::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 a given pose.
This is the first call to the main localization function. This function is meant to find a robot in the map without motion. The error parameters fed to the localization are temporary and will be reset to the old values after the localization.
given,: | Pose to start localization at. | |
numSamples,: | No of samples to be used in the MCL algorithm. | |
stdX,: | Standard deviation in the X coords in mm. | |
stdY,: | Standard deviation in the Y coords in mm. | |
stdT,: | Standard deviation in the Theta coords in degs. | |
thresFactor,: | Factor deciding the threshold for a match. +ve(0-1). Generally must allow at least .1 (10%). | |
warn,: | Flag to warning if loca score is too low. |
bool ArSonarLocalizationTask::localizeRobotInMapMoved | ( | int | numSamples, | |
double | distFactor, | |||
double | angFactor, | |||
double | thresFactor | |||
) |
Function used to do the localization after motion (normally automatic).
This is the actual function doing the Monte Carlo Markov Localization during most of the time once the localization is initialized and the robot is moving. This function calls the appropriate functions in the ArRobotPoseSamples to compute the most likely pose given a current robot pose and the sensor reading.
numSamples,: | No of samples to be used in the MCL algorithm. | |
distFactor,: | Scale factor for scaling the linear std deviation.(+ve). | |
angFactor,: | Scale factor for scaling the angle std deviation.(+ve). | |
thresFactor,: | Factor deciding the threshold for a match. +ve(0-1). Generally must allow at least .1 (10%). |
bool ArSonarLocalizationTask::localizeRobotAtHomeBlocking | ( | double | distSpread, | |
double | angleSpread, | |||
double | probThreshold | |||
) | [inline] |
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 ArSonarLocalizationTask::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.
This function is specifically 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 one with the highest score is where the robot is localized (the home is set to the first home or the home the robot is at if it is at a home). This is to be called when the localization thread has been initialized and needs to be reset. This function blocks while the localization takes place.
distSpread | the distance in mm to spread x and y in search of position | |
angleSpread | the number of degrees to spread in search of position | |
locScoreThreshold | the score for localization to say is good enough |
bool ArSonarLocalizationTask::localizeRobotAtHomeBlocking | ( | ) | [inline, virtual] |
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.)
Reimplemented from ArBaseLocalizationTask.
bool ArSonarLocalizationTask::localizeRobotAtHomeNonBlocking | ( | void | ) |
Request that the task later localize the robot at a home position, and return immediately.
This function is specifically to localize the robot in the first home pose as defined in the Aria Map. This is to be called when the localization thread has been initialized and needs to be reset. This function returns immediately.
void ArSonarLocalizationTask::setForceUpdateParams | ( | int | numSamples, | |
double | xStd, | |||
double | yStd, | |||
double | tStd | |||
) |
Sets the force update parameters.
Sets the force update params. This function is neccesary to cleanly enter the localization thread without messing things up.
numSamples,: | no of samples to be used. | |
xStd,: | X axis std deviation in mm. | |
yStd,: | Y axis std deviation in mm. | |
tStd,: | Theta axis std deviation in degs. |
void ArSonarLocalizationTask::forceUpdatePose | ( | ArPose | forcePose | ) |
Force an update in thread instead of waiting for distance-angle trigger.
Forces the thread to go from move localization to set localization. This function is neccesary to cleanly enter the localization thread without messing things up.
forcePose,: | Pose to be used to center localization about. |
void ArSonarLocalizationTask::addFailedLocalizationCB | ( | ArFunctor1< int > * | functor | ) |
Adds a callback which will be called when loca fails.
After localization has failed these callbacks will be called. This function adds the a pointer to a function to this list.
functor | the functor to call. |
void ArSonarLocalizationTask::remFailedLocalizationCB | ( | ArFunctor1< int > * | functor | ) |
Removes a callback.
Removes a FailedLocalizationCB, see addFailedLocalizationCB for details.
functor,: | The pointer to the function to be taken off. |
void ArSonarLocalizationTask::getForceUpdateParams | ( | ArPose & | forcePose, | |
int & | numSamples, | |||
double & | xStd, | |||
double & | yStd, | |||
double & | tStd | |||
) |
Get the params to do the localization from the class.
Fills the 5 relevant parameters from the force update set.
forcePose,: | Pose to be used to center localization about. | |
numSamples,: | no of samples to be used. | |
xStd,: | X axis std deviation in mm. | |
yStd,: | Y axis std deviation in mm. | |
tStd,: | Theta axis std deviation in mm. |
ArPose ArSonarLocalizationTask::getRobotMaxProbPose | ( | void | ) |
Get the current computed best robot pose.
Gets the most probable pose of the robot as computed by the last localization.
double ArSonarLocalizationTask::getLocalizationScore | ( | void | ) | [virtual] |
Gets the score for the localization as fraction of matched sonar pts.
Returns the goodness of the last localization result. No of matched laser points in the environment to the total no of laser points.
Reimplemented from ArBaseLocalizationTask.
std::list< ArPose > ArSonarLocalizationTask::getCurrentSamplePoses | ( | void | ) | [virtual] |
Gets the pose samples for client.
Gets the Robot pose samples before sensor correction.
Reimplemented from ArBaseLocalizationTask.
ArMapInterface * ArSonarLocalizationTask::readMapFromFile | ( | char * | mapName | ) |
Read the Map data from a map file.
Read the map given the file name.
mapName,: | Name of the map file. |
ArMapInterface * ArSonarLocalizationTask::readAriaMap | ( | ArMapInterface * | ariaMap | ) |
Read the Map data from an ArMap object.
Read the map given the Aria map.
ariaMap,: | The Aria map class ptr. |
bool ArSonarLocalizationTask::saveParams | ( | char * | filename | ) |
Saves all default params to the param file.
Saves the params.
bool ArSonarLocalizationTask::fillHistogram | ( | double *& | hist, | |
double *& | cumSum, | |||
int & | numSamples | |||
) |
Fill prob distribution histogram for debugging.
Fill histogram arrays. For display purposes during debugging.
hist,: | Histogram array. | |
cumSum,: | Cumulative sum array reference. | |
numSamples,: | No of samples. |
bool ArSonarLocalizationTask::scanToGlobalCoords | ( | ArPose | robPose, | |
std::vector< ArPose > & | xyLrf | |||
) |
Convert the sonar data to x and y coord array.
Converts the range points into X and Y coord arrays.
robPose,: | Robots pose. | |
xLrf,: | Pointer to the X coords. | |
xLrf,: | Pointer to the Y coords. |
bool ArSonarLocalizationTask::setLocaParams | ( | double | xStd, | |
double | yStd, | |||
double | tStd, | |||
double | kMmPerMm, | |||
double | kDegPerDeg, | |||
double | kDegPerMm, | |||
double | sensorBelief | |||
) |
Set all the parameters for localization.
Sets all error parameters needed to do the MCL localization.
xStd,: | Standard deviation in the X coords in mm. | |
yStd,: | Standard deviation in the Y coords in mm. | |
tStd,: | Standard deviation in the Theta coords in deg. | |
kMmPerMm,: | Error factor for distance per distance traveled. mm/mm. | |
kDegPerDeg,: | Error factor for angle drift per heading change. deg/deg. | |
kDegPerMm,: | Error factor for angle drift per dist travelled. deg/mm. | |
sensorBelief,: | Sensor Belief. (0-1) |
bool ArSonarLocalizationTask::findMCLMeanVar | ( | ArPose & | mean, | |
ArMatrix & | Var | |||
) |
Finds the mean and var of MCL after moving to present pose.
Finds the MCL mean and variance at the current pose.
mean,: | The computed mean pose. | |
Var,: | The computed variance matrix. |
bool ArSonarLocalizationTask::findLocalizationMeanVar | ( | ArPose & | mean, | |
ArMatrix & | Var | |||
) | [virtual] |
The actual mean var calculator for the virtual in the base class.
Virtual function which will pull the current state of the loca.
mean,: | The computed mean pose. | |
Var,: | The computed variance matrix. |
Reimplemented from ArBaseLocalizationTask.
void ArSonarLocalizationTask::setRobotPose | ( | ArPose | pose, | |
ArPose | spread = ArPose(0, 0, 0) , |
|||
int | nSam = 0 | |||
) | [virtual] |
Used to set the robot pose usually at the start of localization.
This one with a spread around the set pose.
Reimplemented from ArBaseLocalizationTask.
void ArSonarLocalizationTask::drawRangePoints | ( | ArServerClient * | client, | |
ArNetPacket * | packet | |||
) |
Draws range data used for localization.
Draws the stuff.
void ArSonarLocalizationTask::drawClosestLines | ( | ArServerClient * | client, | |
ArNetPacket * | packet | |||
) |
Draws the closest map lines to robot.
Draws the closest lines.
void ArSonarLocalizationTask::drawBestRays | ( | ArServerClient * | client, | |
ArNetPacket * | packet | |||
) |
Draws the sonar rays.
Draws the closest lines.
void ArSonarLocalizationTask::drawSampleRays | ( | ArServerClient * | client, | |
ArNetPacket * | packet | |||
) |
Draws the sonar rays.
Draws the closest lines.
void ArSonarLocalizationTask::drawSonarRays | ( | ArServerClient * | client, | |
ArNetPacket * | packet | |||
) |
Draws the sonar rays.
Draws the closest lines.
void ArSonarLocalizationTask::drawIntersections | ( | ArServerClient * | client, | |
ArNetPacket * | packet | |||
) |
Draws intersection to closest lines.
Draws the intersection points.
void ArSonarLocalizationTask::drawHistogram | ( | ArServerClient * | client, | |
ArNetPacket * | packet | |||
) |
Draws the histogram.
Draws the histogram
void ArSonarLocalizationTask::drawMCLVariance | ( | ArServerClient * | client, | |
ArNetPacket * | packet | |||
) |
Draws the sample bounds.
Draws the MCL's mean pose and var position rectangle with angle cone.
std::vector< ArPose > ArSonarLocalizationTask::makeMeanVarPoints | ( | ArPose & | mean, | |
ArMatrix & | Var | |||
) | [protected] |
Makes the list of points needed to draw the variance and mean. The points will form a rectangle to frame the position and a cone to frame the angle.
mean,: | Return holder for the computed mean. | |
Var,: | Return holder for the variance matrix. |
void ArSonarLocalizationTask::basicInitialization | ( | ArRobot * | robot, | |
ArSonarDevice * | sonar | |||
) | [protected] |
Basic initializer to avoid duplication in the two constructors.
robot,: | Robot data structure. | |
laser,: | Laser data structure. |
void ArSonarLocalizationTask::setFlagsAndCallbacksOnFail | ( | int | ntimes | ) | [protected] |
One stop for all failed localization in runThread.
void * ArSonarLocalizationTask::runThread | ( | void * | ptr | ) | [protected, virtual] |
This is the function loop which is called when the localization is run as a thread.
ptr,: | Some kind of pointer needed by ARIA. |
bool ArSonarLocalizationTask::correctPoseFromSensor | ( | int | sonarStart, | |
int | sonarEnd, | |||
double | maxRange, | |||
std::vector< ArLineSegment > | lines | |||
) | [protected] |
Correct the sample poses by using the data from the sensor. This is the second of the two important steps in sonar Localization. Sensor information is correlated with the lines in the sonar FOV.
sonarStart | Sonar start number. | |
sonarEnd | Sonar end number. | |
maxRange | Maximum range to work with. | |
lines | The set of lines to work with |
bool ArSonarLocalizationTask::initializeSamples | ( | int | ns | ) | [protected] |
Initialize the robot samples for MCL to start.
ns,: | Number of samples. |
void ArSonarLocalizationTask::killBadSamples | ( | double | occThreshold | ) | [protected] |
Kill all bad samples by setting their probs to zero.
occThreshold,: | Value at which the occupancy grid is considered an obstacle. |
void ArSonarLocalizationTask::saveSamples | ( | bool | saveFile = false |
) | [protected] |
Backup samples before they are resampled. Resampling usually flattens the probability distribution to a single location.
saveFile,: | Flag to make it save it as a file for debugging. |
int ArSonarLocalizationTask::findBestPoses | ( | ArRobotPoseSamples * | mrsp, | |
double | factor | |||
) | [protected] |
Finds the most probable poses from the results of the MCL by looking at the samples and their characteristics. Uses the fact that after resampling, the probabilties must be non zero for only one or a few poses.
mrsp,: | Pointer to the robot samples. | |
factor,: | Scale factor from the peak prob to consider. |
bool ArSonarLocalizationTask::findAllStatistics | ( | double & | xMean, | |
double & | yMean, | |||
double & | thMean, | |||
double & | xStd, | |||
double & | yStd, | |||
double & | tStd | |||
) | [protected] |
Compute the statistics from the samples.
xMean,: | Reference to the mean X of the sample poses. | |
yMean,: | Reference to the mean Y of the sample poses. | |
thMean,: | Reference to the mean Theta of the sample poses. | |
xStd,: | Reference to the computed std deviation on X. | |
yStd,: | Reference to the computed std deviation on Y. | |
tStd,: | Reference to the computed std deviation on Theta. |
void ArSonarLocalizationTask::setMotionErrorParam | ( | int | index, | |
double | value | |||
) | [protected] |
Sets all error parameters needed to do the MCL localization. [Kr, Kt, Kd] treated as a vector.
index,: | Index of error param. | |
value,: | Value to set it to. |
double ArSonarLocalizationTask::getMotionErrorParam | ( | int | index | ) | [protected] |
Gets all error parameters needed to do the MCL localization. [Kr, Kt, Kd] treated as a vector.
index,: | Index of error param. |
void ArSonarLocalizationTask::robotCallBack | ( | void | ) | [protected] |
Updates the true current robot pose. Called every 100msec from ArRobot.
bool ArSonarLocalizationTask::configureSonar | ( | void | ) | [protected] |
A function to setup the sonar if it wasnt connected first time around.
bool ArSonarLocalizationTask::reconfigureLocalization | ( | void | ) | [protected] |
A function to setup the laser if it wasnt connected first time around.
void ArSonarLocalizationTask::setupLocalizationParams | ( | void | ) | [protected] |
Sets all the default parameters for the localization and sets up the ArConfig struct for handling Aria configuration.
void ArSonarLocalizationTask::failedLocalization | ( | int | times | ) | [protected] |
Things to do when localization fails.
int ArSonarLocalizationTask::dynamicallyAdjustNumSamples | ( | double | dr, | |
double | dt | |||
) | [protected] |
Dynamically reduce the number of samples when the localization score is good and increase it when the score is lower.
dr,: | Change in distance. | |
dt,: | Change in angle. |
void ArSonarLocalizationTask::mapChanged | ( | void | ) | [protected] |
Things to do if map changes.
std::vector< ArLineSegment > ArSonarLocalizationTask::findClosestLines | ( | ArPose | robotPose, | |
double | range | |||
) | [protected] |
Find the closest lines to a robot pose.
robotPose,: | The current robot pose. | |
range,: | Only those within this distance. |
std::vector< ArLineSegment > ArSonarLocalizationTask::getClosestLines | ( | void | ) | [protected] |
Gets the closest lines.