Python example: global_localization.py

Monte Carlo (particle filter) global localization of a robot on a known map, from a rawlog dataset.

Modules: mrpt.bayes, mrpt.config, mrpt.img, mrpt.io, mrpt.maps, mrpt.obs, mrpt.poses, mrpt.slam, mrpt.gui, mrpt.viz | Requires: gui

  1#!/usr/bin/env python3
  2r"""
  3Monte Carlo (particle filter) global localization of a robot on a known map, from a rawlog dataset.
  4
  5Usage (from the MRPT source tree):
  6
  7  . install/setup.bash
  8  ./mrpt_examples_py/global_localization.py \
  9      modules/mrpt_data/config_files/pf-localization/localization_demo.ini
 10
 11Add --no-gui to run without a 3D window, and --max-steps N to stop early.
 12"""
 13# mrpt-example: video=ZfVUydQIM5E
 14
 15import argparse
 16import os
 17import sys
 18import time
 19
 20from mrpt.bayes import CParticleFilter, TParticleFilterOptions
 21from mrpt.config import CConfigFile
 22from mrpt.img import TColorf
 23from mrpt.io import CCompressedInputStream, archiveFrom
 24from mrpt.maps import (
 25    CMultiMetricMap,
 26    COccupancyGridMap2D,
 27    CSimpleMap,
 28    TSetOfMetricMapInitializers,
 29    VisualizationParameters,
 30    obs_to_viz,
 31)
 32from mrpt.obs import CRawlog
 33from mrpt.poses import CPose3D
 34from mrpt.slam import CMonteCarloLocalization2D, TMonteCarloLocalizationParams
 35
 36DEFAULT_CONFIG = os.path.join(
 37    os.path.dirname(os.path.abspath(__file__)),
 38    "../modules/mrpt_data/config_files/pf-localization/localization_demo.ini")
 39
 40parser = argparse.ArgumentParser()
 41parser.add_argument("config", nargs="?", default=DEFAULT_CONFIG, help="Config file (.ini)")
 42parser.add_argument("-d", "--delay", type=float, default=0.2,
 43                    help="Delay between steps, in seconds (default: 0.2)")
 44parser.add_argument("-r", "--resolution", default="800x600",
 45                    help="Window resolution (default: 800x600)")
 46parser.add_argument("--no-gui", action="store_true", help="Do not open a 3D window")
 47parser.add_argument("--max-steps", type=int, default=0,
 48                    help="Stop after this many rawlog entries (default: 0 = all)")
 49args = parser.parse_args()
 50
 51config_filename = os.path.abspath(args.config)
 52if not os.path.exists(config_filename):
 53    sys.exit(f"Error: config file not found: {config_filename}")
 54config_file = CConfigFile(config_filename)
 55print(f"Loaded config file {config_filename}")
 56
 57# File names in the config are relative to the config file directory:
 58sec_name = "LocalizationExperiment"
 59config_dir = os.path.dirname(config_filename)
 60rawlog_filename = os.path.join(config_dir, config_file.read_string(sec_name, "rawlog_file", ""))
 61map_filename = os.path.join(config_dir, config_file.read_string(sec_name, "map_file", ""))
 62particles_count = config_file.read_int(sec_name, "particles_count", 10000)
 63for f in (rawlog_filename, map_filename):
 64    if not os.path.exists(f):
 65        sys.exit(f"Error: file not found: {f}")
 66
 67# Particle filter options:
 68pf_options = TParticleFilterOptions()
 69pf_options.loadFromConfigFileName(config_filename, "PF_options")
 70
 71# MCL options, including KLD-sampling (adaptive number of particles):
 72mcl_options = TMonteCarloLocalizationParams()
 73mcl_options.KLD_params.loadFromConfigFileName(config_filename, "KLD_options")
 74
 75# The metric maps used to evaluate the observation likelihoods:
 76map_list = TSetOfMetricMapInitializers()
 77map_list.loadFromConfigFileName(config_filename, "MetricMap")
 78metric_map = CMultiMetricMap(map_list)
 79
 80if map_filename.endswith((".simplemap", ".simplemap.gz")):
 81    # A view-based map (poses + observations): build the metric maps from it.
 82    simple_map = CSimpleMap()
 83    if not simple_map.loadFromFile(map_filename):
 84        sys.exit(f"Error loading {map_filename}")
 85    metric_map.loadFromSimpleMap(simple_map)
 86elif map_filename.endswith((".gridmap", ".gridmap.gz")):
 87    # A serialized occupancy grid: use it as the grid map.
 88    grid = archiveFrom(CCompressedInputStream(map_filename)).ReadObject()
 89    for i, m in enumerate(metric_map):
 90        if isinstance(m, COccupancyGridMap2D):
 91            metric_map[i] = grid
 92else:
 93    sys.exit(f"Error: unknown map file extension: {map_filename}")
 94print(f"Loaded map file {map_filename}: {metric_map}")
 95
 96grid_map = next(m for m in metric_map if isinstance(m, COccupancyGridMap2D))
 97
 98# The particle filter:
 99pdf = CMonteCarloLocalization2D()
100pdf.options = mcl_options
101pdf.options.metricMap = metric_map
102pdf.resetUniformFreeSpace(grid_map, 0.7, particles_count)
103
104pf = CParticleFilter()
105pf.options = pf_options
106
107# 3D view:
108win3D = None
109if not args.no_gui:
110    from mrpt.gui import CDisplayWindowGUI
111    from mrpt.viz import Scene
112
113    w, h = (int(v) for v in args.resolution.split("x"))
114    win3D = CDisplayWindowGUI("pf_localization", w, h)
115    win3D.camera().setZoomDistance(40.0)
116    scene = Scene()
117    win3D.background_scene = scene
118    map_object = metric_map.getVisualization()
119
120
121def spin_gui(seconds):
122    """Keeps the window responsive (events, repaints) for a while."""
123    t_end = time.monotonic() + seconds
124    while True:
125        if not win3D.spinOnce():
126            return False
127        if time.monotonic() >= t_end:
128            return True
129        time.sleep(0.01)
130
131viz_options = VisualizationParameters()
132viz_options.pointSize = 3
133viz_options.showAxis = False
134
135# Read the rawlog as a stream, one (action, sensory frame) pair at a time:
136rawlog_arch = archiveFrom(CCompressedInputStream(rawlog_filename))
137entry = 0
138steps = 0
139while True:
140    read_ok, entry, actions, sf, obs = CRawlog.ReadFromArchive(rawlog_arch, entry)
141    if not read_ok:
142        break
143    if actions is None or sf is None:
144        continue  # this example needs rawlogs in the (action, sensory frame) format
145
146    stats = pf.executeOn(pdf, actions, sf)
147    cov, mean = pdf.getCovarianceAndMean()
148    print(f"Entry {entry}: {len(pdf)} particles, mean={mean}, ESS={stats.ESS_beforeResample:.3f}")
149
150    if win3D is not None:
151        gl_obs = obs_to_viz(sf, viz_options)
152        gl_obs.setPose(CPose3D(mean))
153        gl_obs.setColor(TColorf(1.0, 0.0, 0.0))
154
155        scene.clear()
156        scene.insert(map_object)
157        scene.insert(pdf.getVisualization())
158        scene.insert(gl_obs)
159        win3D.camera().setCameraPointing(mean.x, mean.y, 0.0)
160        if not spin_gui(args.delay):
161            break  # window closed
162
163    steps += 1
164    if args.max_steps and steps >= args.max_steps:
165        break
166
167print(f"\nDone. Final estimate: {pdf.getMean()}")
168if win3D is not None and win3D.isOpen() and not args.max_steps:
169    print("Close the window to quit.")
170    while win3D.spinOnce():
171        time.sleep(0.02)