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.')