From e139a4ca0bf790010fe0fad3fe3b9cccc1c91f18 Mon Sep 17 00:00:00 2001
From: Holger Rapp <HolgerRapp@gmx.net>
Date: Wed, 5 Jul 2017 17:27:00 +0200
Subject: [PATCH] Add pure localization demo. (#410)

Adds configuration files and launch files for 2D backpack pure
localization.

PAIR=wohe
---
 .../backpack_2d_localization.lua              | 20 ++++++++++
 .../demo_backpack_2d_localization.launch      | 39 +++++++++++++++++++
 docs/source/index.rst                         | 12 ++++++
 3 files changed, 71 insertions(+)
 create mode 100644 cartographer_ros/configuration_files/backpack_2d_localization.lua
 create mode 100644 cartographer_ros/launch/demo_backpack_2d_localization.launch

diff --git a/cartographer_ros/configuration_files/backpack_2d_localization.lua b/cartographer_ros/configuration_files/backpack_2d_localization.lua
new file mode 100644
index 0000000..a2888b9
--- /dev/null
+++ b/cartographer_ros/configuration_files/backpack_2d_localization.lua
@@ -0,0 +1,20 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "backpack_2d.lua"
+
+TRAJECTORY_BUILDER.pure_localization = true
+SPARSE_POSE_GRAPH.optimize_every_n_scans = 20
+
+return options
diff --git a/cartographer_ros/launch/demo_backpack_2d_localization.launch b/cartographer_ros/launch/demo_backpack_2d_localization.launch
new file mode 100644
index 0000000..2c9c55e
--- /dev/null
+++ b/cartographer_ros/launch/demo_backpack_2d_localization.launch
@@ -0,0 +1,39 @@
+<!--
+  Copyright 2016 The Cartographer Authors
+
+  Licensed under the Apache License, Version 2.0 (the "License");
+  you may not use this file except in compliance with the License.
+  You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+  Unless required by applicable law or agreed to in writing, software
+  distributed under the License is distributed on an "AS IS" BASIS,
+  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  See the License for the specific language governing permissions and
+  limitations under the License.
+-->
+
+<launch>
+  <param name="/use_sim_time" value="true" />
+
+  <param name="robot_description"
+    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+
+  <node name="cartographer_node" pkg="cartographer_ros"
+      type="cartographer_node" args="
+          -configuration_directory $(find cartographer_ros)/configuration_files
+          -configuration_basename backpack_2d_localization.lua
+          -map_filename $(arg map_filename)"
+      output="screen">
+    <remap from="echoes" to="horizontal_laser_2d" />
+  </node>
+
+  <node name="rviz" pkg="rviz" type="rviz" required="true"
+      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
+  <node name="playbag" pkg="rosbag" type="play"
+      args="--clock $(arg bag_filename)" />
+</launch>
diff --git a/docs/source/index.rst b/docs/source/index.rst
index 0e0dfde..cbb61a2 100644
--- a/docs/source/index.rst
+++ b/docs/source/index.rst
@@ -94,6 +94,18 @@ the demo:
     # Launch the 2D backpack demo.
     roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag
 
+    # Pure localization demo: We use 2 different 2D bags from the Deutsche
+    # Museum. The first one is used to generate the map, the second to run
+    # pure localization.
+    wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
+    wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag
+    # Generate the map: Run the next command, wait until cartographer_offline_node finishes.
+    roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag
+    # Run pure localization:
+    roslaunch cartographer_ros demo_backpack_2d_localization.launch \
+       bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag \
+       map_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream
+
     # Download the 3D backpack example bag.
     wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-14-14-00.bag