WIP update python examples

python-examples/global_localization.py

@@ -1,16 +1,26 @@
 #!/usr/bin/env python3
 
+# ---------------------------------------------------------------------
+# Install python3-mrpt, or test with a local build with:
+# export PYTHONPATH=$HOME/code/mrpt/build-Release/:$PYTHONPATH
+# ---------------------------------------------------------------------
+
+from mrpt import pymrpt
 import os
 import sys
-import pymrpt
 import argparse
 from time import sleep
 
+mrpt = pymrpt.mrpt  # namespace shortcut
+
+
 # args
 parser = argparse.ArgumentParser()
 parser.add_argument('config', help='Config file.')
-parser.add_argument('-d', '--delay', help='Time delay in seconds. Default: 0.2')
-parser.add_argument('-r', '--resolution', help='Window resolution. Default: 800x600')
+parser.add_argument(
+    '-d', '--delay', help='Time delay in seconds. Default: 0.2')
+parser.add_argument('-r', '--resolution',
+                    help='Window resolution. Default: 800x600')
 args = parser.parse_args()
 
 # get filenames from args
@@ -22,7 +32,7 @@
     print('Quit.')
     sys.exit(1)
 
-config_file = pymrpt.utils.CConfigFile(config_filename)
+config_file = mrpt.config.CConfigFile(config_filename)
 print('Load config file {}.'.format(config_filename))
 sec_name = 'LocalizationExperiment'
 
@@ -43,7 +53,7 @@
     else:
         rawlog_filename = os.path.abspath(rawlog_filename)
         os.chdir(curr_dir)
-rawlog_file = pymrpt.utils.CFileGZInputStream(rawlog_filename)
+rawlog_file = mrpt.io.CFileGZInputStream(rawlog_filename)
 print('Load rawlog file {}.'.format(rawlog_filename))
 
 # map
@@ -59,34 +69,35 @@
 
 # options
 # kld
-pdf_prediction_options = pymrpt.slam.TMonteCarloLocalizationParams()
-pdf_prediction_options.KLD_params.loadFromConfigFileName(config_filename, 'KLD_options')
+pdf_prediction_options = mrpt.slam.TMonteCarloLocalizationParams()
+pdf_prediction_options.KLD_params.loadFromConfigFileName(
+    config_filename, 'KLD_options')
 pdf_prediction_options.KLD_params.dumpToConsole()
 # pf
-pf_options = pymrpt.bayes.CParticleFilter.TParticleFilterOptions()
+pf_options = mrpt.bayes.CParticleFilter.TParticleFilterOptions()
 pf_options.loadFromConfigFileName(config_filename, 'PF_options')
 pf_options.dumpToConsole()
 # maps
-map_list = pymrpt.maps.TSetOfMetricMapInitializers()
+map_list = mrpt.maps.TSetOfMetricMapInitializers()
 map_list.loadFromConfigFileName(config_filename, 'MetricMap')
 map_list.dumpToConsole()
 
-## setup
-metric_map = pymrpt.maps.CMultiMetricMap()
+# setup
+metric_map = mrpt.maps.CMultiMetricMap()
 metric_map.setListOfMaps(map_list)
 
 
 # load map
 if (map_filename.endswith('.simplemap')
         or map_filename.endswith('.simplemap.gz')):
-    simple_map = pymrpt.maps.CSimpleMap.Create()
-    map_file = pymrpt.utils.CFileGZInputStream(map_filename)
+    simple_map = mrpt.maps.CSimpleMap()
+    map_file = mrpt.io.CFileGZInputStream(map_filename)
     map_file.ReadObject(simple_map)
     metric_map.loadFromProbabilisticPosesAndObservations(simple_map.ctx())
 elif (map_filename.endswith('.gridmap')
         or map_filename.endswith('.gridmap.gz')):
-    occ_map = pymrpt.maps.COccupancyGridMap2D.Create()
-    map_file = pymrpt.utils.CFileGZInputStream(map_filename)
+    occ_map = mrpt.maps.COccupancyGridMap2D()
+    map_file = mrpt.utils.CFileGZInputStream(map_filename)
     map_file.ReadObject(occ_map)
     # overwrite gridmap
     for i in range(len(metric_map.maps)):
@@ -100,14 +111,17 @@
 
 
 # get window resolution
-if args.resolution: resolution_str = args.resolution
-else: resolution_str = '800x600'
+if args.resolution:
+    resolution_str = args.resolution
+else:
+    resolution_str = '800x600'
 
 # gui
 try:
-    win3D = pymrpt.gui.CDisplayWindow3D("pf_localization", int(resolution_str.split('x')[0]), int(resolution_str.split('x')[1]))
+    win3D = mrpt.gui.CDisplayWindow3D("pf_localization", int(
+        resolution_str.split('x')[0]), int(resolution_str.split('x')[1]))
 except:
-    win3D = pymrpt.gui.CDisplayWindow3D("pf_localization", 800, 600)
+    win3D = mrpt.gui.CDisplayWindow3D("pf_localization", 800, 600)
 
 # initial scene
 map_object = metric_map.maps[0].ctx().getAs3DObject()
@@ -119,11 +133,11 @@
 win3D.forceRepaint()
 
 # mcl
-pdf = pymrpt.slam.CMonteCarloLocalization2D()
+pdf = mrpt.slam.CMonteCarloLocalization2D()
 pdf.options = pdf_prediction_options
 pdf.options.metricMap = metric_map
 
-pf = pymrpt.bayes.CParticleFilter()
+pf = mrpt.bayes.CParticleFilter()
 pf.m_options = pf_options
 
 # initialize pdf
@@ -133,9 +147,12 @@
 entry = 0
 while True:
     # get action observation pair
-    next_entry, act, obs, entry = pymrpt.obs.CRawlog.readActionObservationPair(rawlog_file, entry)
-    if not next_entry: break
-    else: entry += 1
+    next_entry, act, obs, entry = mrpt.obs.CRawlog.readActionObservationPair(
+        rawlog_file, entry)
+    if not next_entry:
+        break
+    else:
+        entry += 1
     print('Processing entry: {}.'.format(entry))
 
     # get covariance and mean
@@ -147,13 +164,13 @@
     # get laserscan
     points_map = obs.buildAuxPointsMap()
     laserscan_object = points_map.getAs3DObject()
-    laserscan_object.ctx().setPose(pymrpt.poses.CPose3D(mean))
-    laserscan_object.ctx().setColor(pymrpt.utils.TColorf(1.,0.,0.))
+    laserscan_object.ctx().setPose(mrpt.poses.CPose3D(mean))
+    laserscan_object.ctx().setColor(mrpt.utils.TColorf(1., 0., 0.))
 
     # update pf
-    act_ptr = pymrpt.obs.CActionCollection.Create()
+    act_ptr = mrpt.obs.CActionCollection()
     act_ptr.ctx(act)
-    obs_ptr = pymrpt.obs.CSensoryFrame.Create()
+    obs_ptr = mrpt.obs.CSensoryFrame()
     obs_ptr.ctx(obs)
     stats = pf.executeOn(pdf, act_ptr, obs_ptr)
 
@@ -172,7 +189,8 @@
             sleep(float(args.delay))
         except:
             sleep(0.2)
-    else: sleep(0.2)
+    else:
+        sleep(0.2)
 
 print()
 print('Done.')