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.pymrpt import mrpt
18import argparse
19
20# args
21parser = argparse.ArgumentParser()
22parser.add_argument('rawlog', help='Rawlog file.')
23parser.add_argument('-c', '--config', help='Config file.')
24parser.add_argument(
25    '-o', '--output', help='Save final map as to file with this prefix name.', default='final_map')
26args = parser.parse_args()
27
28# get filenames from args
29rawlog_filename = args.rawlog
30config_filename = args.config
31output_filename = args.output
32
33# Instance SLAM app, init, and run SLAM:
34app = mrpt.apps.RBPF_SLAM_App_Rawlog()
35
36app.init(iniConfigFile=config_filename, rawlogFile=rawlog_filename)
37
38app.run()
39
40# Get last built map
41app.mapBuilder.saveCurrentMapToFile('final_map.simplemap')
42
43# Save map:
44print('saving final map to: "{}_*"'.format(output_filename))
45
46metricMap = app.mapBuilder.getCurrentlyBuiltMetricMap()
47metricMap.saveMetricMapRepresentationToFile(output_filename)