Python example: rbpf_slam.py

This example shows how to build a metric map using the RBPF-SLAM algorithm:

 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# RBPF-SLAM: builds a gridmap from a 2D lidar dataset.
 9#
10# Usage example:
11#
12# ./rbpf_slam.py --help
13#
14# ./rbpf_slam.py -c ../share/mrpt/config_files/rbpf-slam/gridmapping_optimal_sampling.ini  ../share/mrpt/datasets/2006-01ENE-21-SENA_Telecom\ Faculty_one_loop_only.rawlog
15#
16
17from mrpt import pymrpt
18import argparse
19
20mrpt = pymrpt.mrpt
21
22# args
23parser = argparse.ArgumentParser()
24parser.add_argument('rawlog', help='Rawlog file.')
25parser.add_argument('-c', '--config', help='Config file.')
26parser.add_argument(
27    '-o', '--output', help='Save final map as to file with this prefix name.', default='final_map')
28args = parser.parse_args()
29
30# get filenames from args
31rawlog_filename = args.rawlog
32config_filename = args.config
33output_filename = args.output
34
35# Instance SLAM app, init, and run SLAM:
36app = mrpt.apps.RBPF_SLAM_App_Rawlog()
37
38app.init(iniConfigFile=config_filename, rawlogFile=rawlog_filename)
39
40app.run()
41
42# Get last built map
43app.mapBuilder.saveCurrentMapToFile('final_map.simplemap')
44
45# Save map:
46print('saving final map to: "{}_*"'.format(output_filename))
47
48metricMap = app.mapBuilder.getCurrentlyBuiltMetricMap()
49metricMap.saveMetricMapRepresentationToFile(output_filename)