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