Localization Monitor

Detecting abnormal system states and taking corrective action help ensure stable system performance and a minimum of deviation from expected behavior. To this end, Isaac SDK features a monitoring framework that can be equipped with a variety of system observation components. This framework currently contains components specific to monitoring the current state of a robot’s localization. With its modular approach, it can be customized easily for individual scenarios and use cases. It consists of multiple components that work in tandem:

• A central Monitor component that collects system performance grades from individual evaluator components.
• Evaluator components that regularly send performance grades about specific parts of the system to one or more connected Monitor components. The following evaluators are part of this release:
  • flatscan_localization::MonitorEvaluatorFrequency: Observes the frequency with which messages are received on a specified FlatscanProto channel and reports deviations from an expected frequency to the Monitor.
  • flatscan_localization::MonitorEvaluatorMapscan: Continuously compares received FlatscanProto laser scan messages with predicted laser scans for the currently estimated robot pose. Reports the degree of agreement to the Monitor.
  • flatscan_localization::MonitorEvaluatorSvio: Observes the drift between two different pose estimation frames and reports the deviation between both over time. This can be used, for example, to detect drift between a laser-scan based localization and a Stereo Visual Inertial Odometry based localization.
• An EvaluatorGradeFusion component that is used by the Monitor to combine multiple received performance grades to form an overall grade that can be used for deriving action. In this release, one type of EvaluatorGradeFusion is implemented:
  • MaxValueGradeFusion: Returns the highest (worst) grade value of all currently received evaluator grades.

Multiple Monitor instances can be present in the same system, and evaluators can report their data to multiple Monitor instances in parallel by connecting their grade edge to any unique input channel of a Monitor instance. The following parameters are available in the Monitor component:

• evaluator_max_age: Should, at any point, any evaluator stop sending messages for more than the duration specified in this parameter (in seconds), a failure is reported.
• fused_grade_threshold: When the EvaluatorGradeFusion instance currently used by the Monitor instance returns a value larger than this parameter, a failure is reported.
• settle_time: The time (in seconds) to wait after starting this codelet to start evaluation. This gives the overall system time to settle after an initial ramp-up phase and prevents action based on data that is not representative of a running system state.

The general localization subgraph includes a Localization Monitor setup that makes use of the first two evaluators. Should a failure be reported by the Monitor instance, global localization is restarted. The base configuration can be changed via the following configuration keys:

            

            
navigation.localization.localization_monitor.Monitor -> evaluator_max_age
navigation.localization.localization_monitor.Monitor -> fused_grade_threshold
navigation.localization.localization_monitor.Monitor -> settle_time

navigation.localization.evaluator_frequency.Evaluator -> threshold
navigation.localization.evaluator_frequency.Evaluator -> expected_frequency

navigation.localization.evaluator_mapscan.Evaluator -> map
navigation.localization.evaluator_mapscan.Evaluator -> range_scan_model
navigation.localization.evaluator_mapscan.Evaluator -> flatscan_frames
navigation.localization.evaluator_mapscan.Evaluator -> robot_frame
navigation.localization.evaluator_mapscan.Evaluator -> beam_distance_threshold
navigation.localization.evaluator_mapscan.Evaluator -> good_beams_threshold
        

Depending on the scenario the Localization Monitor is deployed in, the expected_frequency parameter needs to be adapted to the actual expected frequency of the monitored flatscan source. If in doubt, use a lower expected frequency, as a higher than expected frequency does not negatively influence the Monitor behavior. The threshold parameter can be adjusted if the amplitude of change in frequency is comparatively high to prevent spurious failure reports from the Monitor.

Should the ramp-up time of the system be more than the default value for the Monitor (3 seconds), the settle_time parameter can be adjusted accordingly. In cases where Monitor decisions seem too conservative, e.g. when failures are reported too early because of very noisy sensor input, the fused_grade_threshold can be increased to accomodate for this.

If the occupancy grid map used for localization changes often does not exactly represent the actual environment, the beam_distance_threshold and good_beams_threshold (ranges from 0.0 to 1.0) can be increased.

If you wish to implement additional evaluators or grade fusions, note the following design guidance:

• Evaluators must send messages of type CompositeProto to a unique input channel of a Monitor instance. These messages must contain the field grade, which should be 0.0 if the expected performance result was reached. Positive values (ideally normalized to 1.0) indicate that performance is worse than expected, and negative values (normalized to -1.0) indicate that performance is better than expected.
• Grade Fusion components must derive from the component ::isaac::monitor::EvaluatorGradeFusion and must implement its interface. The values returned by fuseGrades should follow the metric for evaluator grades described above.

The current state of the Monitor and the evaluator components can be visualized using Isaac Sight. This includes the overall grade of the Monitor, as well as the individual evaluator grades and their specific values (such as the frequency measured by MonitorEvaluatorFrequency.

In Sight, activate the following message channels to visualize the Monitor and evaluator grades (their location may vary based on the actual application setup):

            

            
navigation.localization.evaluator_frequency.Evaluator -> grade
navigation.localization.evaluator_mapscap.Evaluator   -> grade
navigation.localization.localization_monitor.Monitor  -> grade
        

These channels can then be visualized in a 2D plot to analyze the weak points of localization in a given scenario:

The internal states of the individual evaluators can also be visualized: