The lost_detector node looks at the spread of particles from amcl and tries to decide whether the robot might be confused or lost.

The lost detector creates two features based on the amcl particle filter spread. The first is the standard deviation of the longest dimension of the x-y covariance ellipse (where the longest dimension corresponds to the largest eigenvector of the covariance matrix), and the second is the standard deviation of the particles’ theta values.

Those two features are used each time amcl broadcasts its particle covariance, to evaluate whether the robot currently looks “lost” or “normal” using a Gaussian Naive Bayes (GNB) classifier. If enough time steps recently have looked “lost,” then the lost_detector will declare the robot confused or lost.

In particular, `/lost_detector/status`

outputs one of the following:

“confused” (tier 1): we’ve seen at least `lost_detector/queue_min`

detections and this reading was labeled lost by the GNB classifier

“lost” (tier 2): the ratio of lost to normal readings in the last
`lost_detector/window_meta_tier2_lost`

readings is higher than
`lost_detector/ratio_thresh_detections_tier2_lost`

“normal” (tier 0): neither confused nor lost

Seeing `lost_detector/window_meta_tier0_normal`

“normal” readings will
reset the lost detector entirely.

- amcl
- lots of other things brought in by
`gizmo`

`/lost_detector/reset`

std_msgs::Empty

Used to reset the lost detector (empty queue, set status to normal)

`/laser_amcl_pose`

geometry_msgs::PoseWithCovarianceStamped

Used to get the covariance matrix from amcl

`/lost_detector/status`

std_msgs/String

Outputs “lost,” “confused,” or “normal” depending on the state of the
lost_detector

Parameters can be found in `gizmo/app/configs/lost_detector.yaml`

.

In namespace modified_prob_classifier (params for the GNB classifier):

`modified_prob_classifier/tier0_normal_prior`

double

The prior on whether we are lost

`modified_prob_classifier/prob_thresh`

double

Classify this single set of features as “lost” if the prob is > prob_thresh
that it belongs to the lost-gaussian

`modified_prob_classifier/slice_mu`

4-vector of doubles

mu (mean) vector is: [xy-not-lost, theta-not-lost, xy-lost, theta-lost]

units are meters, radians

`modified_prob_classifier/slice_sigma`

4-vector of doubles

sigma vector is: [xy-std-not-lost, theta-std-not-lost, xy-std-lost,
theta-std-lost]

units are meters, radians

In namespace lost_detector:

`lost_detector/window_meta_tier2_lost`

int

how many lost detections to track when deciding whether we’re tier2 “lost”

`lost_detector/window_meta_tier0_normal`

int

if we haven’t looked lost this many times in a row, declare that we’re no
longer lost (reset queue and set to detector to normal)

`lost_detector/queue_min`

int

min length of detection queue before we will declare even tier1 “confused”

`lost_detector/ratio_thresh_detections_tier2_lost`

double

if the ratio of lost detections in the queue is this high, we are tier2 “lost”

`lost_detector pylaunch`