Python example: global_localization.py

This example runs a MonteCarlo global localization algorithm for a robot from a dataset in .rawlog format. The adaptive particle filter algorithm has two implementations, for 2D and 3D poses, or SE(2) or SE(3), respectively.

  1#!/usr/bin/env python3
  2
  3# ---------------------------------------------------------------------
  4# Install python3-pymrpt, ros-$ROS_DISTRO-mrpt2, or test with a local build with:
  5# export PYTHONPATH=$HOME/code/mrpt/build-Release/:$PYTHONPATH
  6# ---------------------------------------------------------------------
  7
  8#
  9# Usage example:
 10#
 11# ./global_localization.py ../share/mrpt/config_files/pf-localization/localization_demo.ini
 12#
 13from mrpt import pymrpt
 14import os
 15import sys
 16import argparse
 17from time import sleep
 18
 19mrpt = pymrpt.mrpt  # namespace shortcut
 20
 21
 22# args
 23parser = argparse.ArgumentParser()
 24parser.add_argument('config', help='Config file.')
 25parser.add_argument(
 26    '-d', '--delay', help='Time delay in seconds. Default: 0.2')
 27parser.add_argument('-r', '--resolution',
 28                    help='Window resolution. Default: 800x600')
 29args = parser.parse_args()
 30
 31# get filenames from args
 32config_filename = args.config
 33
 34# get configuration
 35if not os.path.exists(config_filename):
 36    print('Error. Config file not found.')
 37    print('Quit.')
 38    sys.exit(1)
 39
 40config_file = mrpt.config.CConfigFile(config_filename)
 41print('Load config file {}.'.format(config_filename))
 42sec_name = 'LocalizationExperiment'
 43
 44rawlog_filename = config_file.read_string(sec_name, "rawlog_file", "")
 45map_filename = config_file.read_string(sec_name, "map_file", "")
 46
 47
 48# default filenames are relative so we need to change our dir to supplied config file dir
 49curr_dir = os.path.abspath(os.curdir)
 50config_dir = os.path.dirname(os.path.abspath(config_filename))
 51# rawlog
 52if not os.path.exists(os.path.abspath(rawlog_filename)):
 53    os.chdir(config_dir)
 54    if not os.path.exists(os.path.abspath(rawlog_filename)):
 55        print('Error. Rawlog file not found.')
 56        print('Quit.')
 57        sys.exit(1)
 58    else:
 59        rawlog_filename = os.path.abspath(rawlog_filename)
 60        os.chdir(curr_dir)
 61rawlog_file = mrpt.io.CFileGZInputStream(rawlog_filename)
 62print('Load rawlog file {}.'.format(rawlog_filename))
 63
 64# map
 65if not os.path.exists(os.path.abspath(map_filename)):
 66    os.chdir(config_dir)
 67    if not os.path.exists(os.path.abspath(map_filename)):
 68        print('Error. Map file not found.')
 69        print('Quit.')
 70        sys.exit(1)
 71    else:
 72        map_filename = os.path.abspath(map_filename)
 73        os.chdir(curr_dir)
 74
 75# Load parameters:
 76# KLD (Adapative sampling)
 77pdf_prediction_options = mrpt.slam.TMonteCarloLocalizationParams()
 78pdf_prediction_options.KLD_params.loadFromConfigFileName(
 79    config_filename, 'KLD_options')
 80pdf_prediction_options.KLD_params.dumpToConsole()
 81
 82# Particle filtering itself:
 83pf_options = mrpt.bayes.CParticleFilter.TParticleFilterOptions()
 84pf_options.loadFromConfigFileName(config_filename, 'PF_options')
 85pf_options.dumpToConsole()
 86
 87# Metric maps to build:
 88map_list = mrpt.maps.TSetOfMetricMapInitializers()
 89map_list.loadFromConfigFileName(config_filename, 'MetricMap')
 90map_list.dumpToConsole()
 91
 92# setup
 93metric_map = mrpt.maps.CMultiMetricMap()
 94metric_map.setListOfMaps(map_list)
 95
 96# load map
 97map_file = mrpt.io.CFileGZInputStream(map_filename)
 98map_arch = mrpt.serialization.archiveFrom(map_file)
 99
100if (map_filename.endswith('.simplemap')
101        or map_filename.endswith('.simplemap.gz')):
102    simple_map = mrpt.maps.CSimpleMap()
103    map_arch.ReadObject(simple_map)
104    metric_map.loadFromProbabilisticPosesAndObservations(simple_map)
105elif (map_filename.endswith('.gridmap')
106        or map_filename.endswith('.gridmap.gz')):
107    occ_map = mrpt.maps.COccupancyGridMap2D()
108    map_arch.ReadObject(occ_map)
109    # overwrite gridmap
110    for i in range(len(metric_map.maps)):
111        if metric_map.maps[i].GetRuntimeClass().className == 'COccupancyGridMap2D':
112            metric_map.maps[i] = occ_map
113else:
114    print('Error. Can not load map from unknown extension.')
115    print('Quit.')
116    sys.exit(1)
117print('Load map file {}.'.format(map_filename))
118
119# get window resolution
120if args.resolution:
121    resolution_str = args.resolution
122else:
123    resolution_str = '800x600'
124
125# gui
126try:
127    res = resolution_str.split('x')
128    win3D = mrpt.gui.CDisplayWindow3D(
129        "pf_localization", int(res[0]), int(res[1]))
130except:
131    win3D = mrpt.gui.CDisplayWindow3D("pf_localization", 800, 600)
132
133# initial scene
134map_object = metric_map.getVisualization()
135
136scene_ptr = win3D.get3DSceneAndLock()
137scene_ptr.clear()
138scene_ptr.insert(map_object)
139win3D.unlockAccess3DScene()
140win3D.forceRepaint()
141
142# mcl
143pdf = mrpt.slam.CMonteCarloLocalization2D()
144pdf.options = pdf_prediction_options
145pdf.options.metricMap = metric_map
146
147pf = mrpt.bayes.CParticleFilter()
148pf.m_options = pf_options
149
150# initialize pdf
151pdf.resetUniformFreeSpace(metric_map.maps[0], 0.7, 40000)
152
153# Archive for reading from the file:
154rawlogArch = mrpt.serialization.archiveFrom(rawlog_file)
155
156# loop
157entry = 0
158while True:
159    # get action observation pair
160    [readOk, entry, act, sf, obs] = mrpt.obs.CRawlog.ReadFromArchive(
161        rawlogArch, entry)
162    if not readOk:
163        break
164
165    print('Processing entry: {}.'.format(entry))
166
167    # get covariance and mean
168    cov, mean = pdf.getCovarianceAndMean()
169
170    # get particles
171    particles_object = pdf.getVisualization()
172
173    # get visualization for laser scan (two ways for demonstration purposes)
174    if False:
175        # Alternative method 1:
176        # Insert the observations into a point cloud:
177        points_map = mrpt.maps.CSimplePointsMap()
178        points_map.insertObs(sf)
179
180        # And get the visualization of the point cloud:
181        glObservation = points_map.getVisualization()
182    else:
183        # Alternative 2:
184        # Using the generic obs_to_viz() method:
185        vizOpts = mrpt.obs.VisualizationParameters()
186        vizOpts.pointSize = 3
187        vizOpts.showAxis = False
188
189        glObservation = mrpt.opengl.CSetOfObjects()
190
191        mrpt.obs.obs_to_viz(sf, vizOpts, glObservation)
192
193    glObservation.setPose(mrpt.poses.CPose3D(mean))
194    glObservation.setColor(mrpt.img.TColorf(1., 0., 0.))
195
196    # update pf
197    stats = pf.executeOn(pdf, act, sf)
198
199    # update scene
200    scene_ptr = win3D.get3DSceneAndLock()
201    scene_ptr.clear()
202    scene_ptr.insert(map_object)
203    scene_ptr.insert(particles_object)
204    scene_ptr.insert(glObservation)
205    win3D.unlockAccess3DScene()
206    win3D.forceRepaint()
207
208    # sleep
209    if args.delay:
210        try:
211            sleep(float(args.delay))
212        except:
213            sleep(0.2)
214    else:
215        sleep(0.2)
216
217print()
218print('Done.')
219input('Press any key to quit.')