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