carto, grapher, movebase, rbx1, scanICP, sendGoal, sicktim, p&f

.gitignore

@@ -0,0 +1,52 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+.vscode/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

cartographer_ros/.dockerignore

@@ -0,0 +1,4 @@
+**/Dockerfile*
+**/.dockerignore
+**/.git
+**/.travis.yml

cartographer_ros/.github/ISSUE_TEMPLATE

@@ -0,0 +1,25 @@
+Thank you for filing an issue!
+
+It would help us tremendously if you run through the following checklist. This
+ensures that we have the most information to quickly understand, analyze,
+reproduce and eventually resolve your issue.
+
+Please
+
+1. run `rosbag_validate` which does some checks on your sensor data. This
+   tool often finds issues that can explain poor performance and must be fixed
+   at recording time. Please post the full output of the tool into a
+   GitHub Gist at https://gist.github.com/, then link it in the issue even if
+   it does not report anything. You can run the tool like this:
+
+   rosrun cartographer_ros cartographer_rosbag_validate -bag_filename <bag filename>
+
+2. post a link to a Git repository containing a branch of
+   `cartographer_ros` containing all the configuration, launch, and URDF files
+   required to reproduce your issue.
+3. post a link to a bag file we can use to reproduce your issue. Put it on
+   Google Drive, Dropbox, any webserver or wherever it is publicly
+   downloadable.
+4. remove this boilerplate text before submitting your issue.
+
+We will likely be unable to help you without all of these points addressed.

cartographer_ros/.travis.yml

@@ -0,0 +1,39 @@
+# Copyright 2018 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.
+
+sudo: required
+services: docker
+
+# Cache intermediate Docker layers. For a description of how this works, see:
+# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html
+cache:
+  directories:
+    - /home/travis/docker/
+
+env:
+  - ROS_RELEASE=indigo DOCKER_CACHE_FILE=/home/travis/docker/indigo-cache.tar.gz
+  - ROS_RELEASE=kinetic DOCKER_CACHE_FILE=/home/travis/docker/kinetic-cache.tar.gz
+  - ROS_RELEASE=lunar DOCKER_CACHE_FILE=/home/travis/docker/lunar-cache.tar.gz
+  - ROS_RELEASE=melodic DOCKER_CACHE_FILE=/home/travis/docker/melodic-cache.tar.gz
+
+before_install:
+  # $GITHUB_TOKEN must be a valid GitHub access token without access rights (https://github.com/settings/tokens).
+  # Either add your token to the 'env' section above or add it as an unencrypted variable in the Travis settings.
+  - scripts/check_access_token.sh $GITHUB_TOKEN
+  - scripts/load_docker_cache.sh
+
+install: true
+script:
+  - docker build ${TRAVIS_BUILD_DIR} -t cartographer_ros:${ROS_RELEASE} -f Dockerfile.${ROS_RELEASE} --build-arg github_token=${GITHUB_TOKEN}
+  - scripts/save_docker_cache.sh

cartographer_ros/AUTHORS

@@ -0,0 +1,7 @@
+# This is the list of Cartographer authors for copyright purposes.
+#
+# This does not necessarily list everyone who has contributed code, since in
+# some cases, their employer may be the copyright holder.  To see the full list
+# of contributors, see the revision history in source control.
+Google Inc.
+and other contributors

cartographer_ros/CONTRIBUTING.md

@@ -0,0 +1,27 @@
+Want to contribute? Great! First, read this page (including the small print at the end).
+
+### Before you contribute
+Before we can use your code, you must sign the
+[Google Individual Contributor License Agreement]
+(https://cla.developers.google.com/about/google-individual)
+(CLA), which you can do online. The CLA is necessary mainly because you own the
+copyright to your changes, even after your contribution becomes part of our
+codebase, so we need your permission to use and distribute your code. We also
+need to be sure of various other things—for instance that you'll tell us if you
+know that your code infringes on other people's patents. You don't have to sign
+the CLA until after you've submitted your code for review and a member has
+approved it, but you must do it before we can put your code into our codebase.
+Before you start working on a larger contribution, you should get in touch with
+us first through the issue tracker with your idea so that we can help out and
+possibly guide you. Coordinating up front makes it much easier to avoid
+frustration later on.
+
+### Code reviews
+All submissions, including submissions by project members, require review. We
+use Github pull requests for this purpose.
+
+### The small print
+Contributions made by corporations are covered by a different agreement than
+the one above, the
+[Software Grant and Corporate Contributor License Agreement]
+(https://cla.developers.google.com/about/google-corporate).

cartographer_ros/Dockerfile.base.melodic

@@ -0,0 +1,43 @@
+# Copyright 2018 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.
+
+FROM ros:melodic
+
+ARG CARTOGRAPHER_VERSION=master
+
+# Bionic's base image doesn't ship with sudo.
+RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
+
+# First, we invalidate the entire cache if googlecartographer/cartographer has
+# changed. This file's content changes whenever master changes. See:
+# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
+ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master \
+    cartographer_ros/cartographer_version.json
+
+# wstool needs the updated rosinstall file to clone the correct repos.
+COPY cartographer_ros.rosinstall cartographer_ros/
+COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
+RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
+    cartographer_ros/scripts/prepare_catkin_workspace.sh
+
+# rosdep needs the updated package.xml files to install the correct debs.
+COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
+COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
+COPY scripts/install_debs.sh cartographer_ros/scripts/
+RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
+
+# Install proto3.
+RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
+

cartographer_ros/Dockerfile.indigo

@@ -0,0 +1,76 @@
+# 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.
+
+FROM ros:indigo
+
+ARG CARTOGRAPHER_VERSION=master
+
+# We require a GitHub access token to be passed.
+ARG github_token
+
+# First, we invalidate the entire cache if googlecartographer/cartographer has
+# changed. This file's content changes whenever master changes. See:
+# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
+ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
+    cartographer_ros/cartographer_version.json
+
+# wstool needs the updated rosinstall file to clone the correct repos.
+COPY cartographer_ros.rosinstall cartographer_ros/
+COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
+RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
+    cartographer_ros/scripts/prepare_catkin_workspace.sh
+
+# rosdep needs the updated package.xml files to install the correct debs.
+COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
+COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
+COPY scripts/install_debs.sh cartographer_ros/scripts/
+RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
+
+# Install proto3.
+RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
+
+# Build, install, and test all packages individually to allow caching. The
+# ordering of these steps must match the topological package ordering as
+# determined by Catkin.
+COPY scripts/install.sh cartographer_ros/scripts/
+COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
+
+COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
+
+RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
+
+RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
+    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
+
+COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
+
+COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
+
+COPY scripts/ros_entrypoint.sh /
+# A BTRFS bug may prevent us from cleaning up these directories.
+# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
+RUN rm -rf cartographer_ros catkin_ws || true

cartographer_ros/Dockerfile.kinetic

@@ -0,0 +1,79 @@
+# 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.
+
+FROM ros:kinetic
+
+ARG CARTOGRAPHER_VERSION=master
+
+# We require a GitHub access token to be passed.
+ARG github_token
+
+# Xenial's base image doesn't ship with sudo.
+RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
+
+# First, we invalidate the entire cache if googlecartographer/cartographer has
+# changed. This file's content changes whenever master changes. See:
+# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
+ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
+    cartographer_ros/cartographer_version.json
+
+# wstool needs the updated rosinstall file to clone the correct repos.
+COPY cartographer_ros.rosinstall cartographer_ros/
+COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
+RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
+    cartographer_ros/scripts/prepare_catkin_workspace.sh
+
+# rosdep needs the updated package.xml files to install the correct debs.
+COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
+COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
+COPY scripts/install_debs.sh cartographer_ros/scripts/
+RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
+
+# Install proto3.
+RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
+
+# Build, install, and test all packages individually to allow caching. The
+# ordering of these steps must match the topological package ordering as
+# determined by Catkin.
+COPY scripts/install.sh cartographer_ros/scripts/
+COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
+
+COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
+
+RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
+
+RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
+    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
+
+COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
+
+COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
+
+COPY scripts/ros_entrypoint.sh /
+# A BTRFS bug may prevent us from cleaning up these directories.
+# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
+RUN rm -rf cartographer_ros catkin_ws || true

cartographer_ros/Dockerfile.lunar

@@ -0,0 +1,79 @@
+# 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.
+
+FROM ros:lunar
+
+ARG CARTOGRAPHER_VERSION=master
+
+# We require a GitHub access token to be passed.
+ARG github_token
+
+# Xenial's base image doesn't ship with sudo.
+RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
+
+# First, we invalidate the entire cache if googlecartographer/cartographer has
+# changed. This file's content changes whenever master changes. See:
+# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
+ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
+    cartographer_ros/cartographer_version.json
+
+# wstool needs the updated rosinstall file to clone the correct repos.
+COPY cartographer_ros.rosinstall cartographer_ros/
+COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
+RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
+    cartographer_ros/scripts/prepare_catkin_workspace.sh
+
+# rosdep needs the updated package.xml files to install the correct debs.
+COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
+COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
+COPY scripts/install_debs.sh cartographer_ros/scripts/
+RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
+
+# Install proto3.
+RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
+
+# Build, install, and test all packages individually to allow caching. The
+# ordering of these steps must match the topological package ordering as
+# determined by Catkin.
+COPY scripts/install.sh cartographer_ros/scripts/
+COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
+
+COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
+
+RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
+
+RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
+    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
+
+COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
+
+COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
+
+COPY scripts/ros_entrypoint.sh /
+# A BTRFS bug may prevent us from cleaning up these directories.
+# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
+RUN rm -rf cartographer_ros catkin_ws || true

cartographer_ros/Dockerfile.melodic

@@ -0,0 +1,76 @@
+# Copyright 2018 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.
+
+FROM eu.gcr.io/cartographer-141408/cartographer_ros_melodic_base
+
+ARG CARTOGRAPHER_VERSION=master
+
+# We require a GitHub access token to be passed.
+ARG github_token
+
+# Bionic's base image doesn't ship with sudo.
+RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
+
+# First, we invalidate the entire cache if googlecartographer/cartographer has
+# changed. This file's content changes whenever master changes. See:
+# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
+ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
+    cartographer_ros/cartographer_version.json
+
+# wstool needs the updated rosinstall file to clone the correct repos.
+COPY cartographer_ros.rosinstall cartographer_ros/
+COPY scripts/update_catkin_workspace.sh cartographer_ros/scripts/
+RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
+    cartographer_ros/scripts/update_catkin_workspace.sh
+
+# rosdep needs the updated package.xml files to install the correct debs.
+COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
+COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
+COPY scripts/install_debs.sh cartographer_ros/scripts/
+RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
+
+# Build, install, and test all packages individually to allow caching. The
+# ordering of these steps must match the topological package ordering as
+# determined by Catkin.
+COPY scripts/install.sh cartographer_ros/scripts/
+COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
+
+COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
+
+RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
+
+RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
+    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
+
+COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
+
+COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
+RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
+    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
+        --catkin-make-args run_tests && \
+    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
+
+COPY scripts/ros_entrypoint.sh /
+# A BTRFS bug may prevent us from cleaning up these directories.
+# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
+RUN rm -rf cartographer_ros catkin_ws || true

cartographer_ros/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   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.

cartographer_ros/README.rst

@@ -0,0 +1,60 @@
+.. 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.
+
+============================
+Cartographer ROS Integration
+============================
+
+|build| |docs| |license|
+
+Purpose
+=======
+
+`Cartographer`_ is a system that provides real-time simultaneous localization
+and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor
+configurations. This project provides Cartographer's ROS integration.
+
+.. _Cartographer: https://github.com/googlecartographer/cartographer
+.. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping
+
+Getting started
+===============
+
+* Learn to use Cartographer with ROS at `our Read the Docs site`_.
+* You can ask a question by `creating an issue`_.
+
+.. _our Read the Docs site: https://google-cartographer-ros.readthedocs.io
+.. _creating an issue: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question
+
+Contributing
+============
+
+You can find information about contributing to Cartographer's ROS integration
+at `our Contribution page`_.
+
+.. _our Contribution page: https://github.com/googlecartographer/cartographer_ros/blob/master/CONTRIBUTING.md
+
+.. |build| image:: https://travis-ci.org/googlecartographer/cartographer_ros.svg?branch=master
+    :alt: Build Status
+    :scale: 100%
+    :target: https://travis-ci.org/googlecartographer/cartographer_ros
+.. |docs| image:: https://readthedocs.org/projects/google-cartographer-ros/badge/?version=latest
+    :alt: Documentation Status
+    :scale: 100%
+    :target: https://google-cartographer-ros.readthedocs.io/en/latest/?badge=latest
+.. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg
+     :alt: Apache 2 license.
+     :scale: 100%
+     :target: https://github.com/googlecartographer/cartographer_ros/blob/master/LICENSE
+

cartographer_ros/azure-pipelines.yml

@@ -0,0 +1,70 @@
+# Copyright 2018 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.
+
+jobs:
+- job: Build
+  pool:
+    vmImage: 'vs2017-win2016'
+  timeoutInMinutes: 360
+  steps:
+  - script: |
+      choco sources add -n=roswin -s https://roswin.azurewebsites.net/api/v2/ --priority 1
+      rem Azure VM runs out of space on C:, so use D: for ros and rosdeps
+      mkdir D:\opt && mklink /J C:\opt D:\opt
+      choco upgrade %ROS_METAPACKAGE% -y
+      choco upgrade ros-melodic-perception -y
+      robocopy "." ".\src\cartographer_ros" /E /MOVE /XD "src" > NUL
+      git clone https://github.com/googlecartographer/cartographer src\cartographer
+      call "C:\opt\ros\melodic\x64\env.bat" rosdep install --from-paths src --ignore-src -r -y
+    env:
+      ROS_METAPACKAGE: 'ros-melodic-desktop'
+    displayName: Install prerequisites
+
+  - script: |
+      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
+      call "C:\opt\ros\melodic\x64\setup.bat"
+      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
+      catkin_make_isolated --use-ninja --install --cmake-args -DCMAKE_BUILD_TYPE=Release
+    displayName: Build
+
+  - script: |
+      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
+      call "C:\opt\ros\melodic\x64\setup.bat"
+      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
+      cd build_isolated\cartographer\install && ctest --no-compress-output -T Test
+    displayName: Run cartographer tests
+
+  - script: |
+      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
+      call "C:\opt\ros\melodic\x64\setup.bat"
+      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
+      cd build_isolated\cartographer_ros && ninja tests && ctest --no-compress-output -T Test
+    displayName: Build and run cartographer_ros tests
+    condition: always()
+
+  - script: |
+      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
+      call "C:\opt\ros\melodic\x64\setup.bat"
+      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
+      python src\cartographer\scripts\ctest_to_junit.py build_isolated\cartographer_ros
+    displayName: Convert tests to jUnit
+    condition: always()
+
+  - task: PublishTestResults@2
+    displayName: Publish test results
+    inputs:
+      testRunner: 'jUnit'
+      testResultsFiles: '**\jUnit.xml'
+      searchFolder: '$(Build.SourcesDirectory)\build_isolated\cartographer_ros\Testing'
+    condition: always()

cartographer_ros/cartographer_ros.files

@@ -0,0 +1,65 @@
+cartographer_ros/cartographer_ros/assets_writer.cc
+cartographer_ros/cartographer_ros/assets_writer.h
+cartographer_ros/cartographer_ros/assets_writer_main.cc
+cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc
+cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc
+cartographer_ros/cartographer_ros/configuration_files_test.cc
+cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc
+cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc
+cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc
+cartographer_ros/cartographer_ros/map_builder_bridge.cc
+cartographer_ros/cartographer_ros/map_builder_bridge.h
+cartographer_ros/cartographer_ros/metrics/family_factory.cc
+cartographer_ros/cartographer_ros/metrics/family_factory.h
+cartographer_ros/cartographer_ros/metrics/internal/counter.h
+cartographer_ros/cartographer_ros/metrics/internal/family.cc
+cartographer_ros/cartographer_ros/metrics/internal/family.h
+cartographer_ros/cartographer_ros/metrics/internal/gauge.h
+cartographer_ros/cartographer_ros/metrics/internal/histogram.cc
+cartographer_ros/cartographer_ros/metrics/internal/histogram.h
+cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc
+cartographer_ros/cartographer_ros/msg_conversion.cc
+cartographer_ros/cartographer_ros/msg_conversion.h
+cartographer_ros/cartographer_ros/msg_conversion_test.cc
+cartographer_ros/cartographer_ros/node.cc
+cartographer_ros/cartographer_ros/node.h
+cartographer_ros/cartographer_ros/node_constants.cc
+cartographer_ros/cartographer_ros/node_constants.h
+cartographer_ros/cartographer_ros/node_main.cc
+cartographer_ros/cartographer_ros/node_options.cc
+cartographer_ros/cartographer_ros/node_options.h
+cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
+cartographer_ros/cartographer_ros/offline_node.cc
+cartographer_ros/cartographer_ros/offline_node.h
+cartographer_ros/cartographer_ros/offline_node_main.cc
+cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
+cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
+cartographer_ros/cartographer_ros/playable_bag.cc
+cartographer_ros/cartographer_ros/playable_bag.h
+cartographer_ros/cartographer_ros/ros_log_sink.cc
+cartographer_ros/cartographer_ros/ros_log_sink.h
+cartographer_ros/cartographer_ros/ros_map.cc
+cartographer_ros/cartographer_ros/ros_map.h
+cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
+cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h
+cartographer_ros/cartographer_ros/rosbag_validate_main.cc
+cartographer_ros/cartographer_ros/sensor_bridge.cc
+cartographer_ros/cartographer_ros/sensor_bridge.h
+cartographer_ros/cartographer_ros/start_trajectory_main.cc
+cartographer_ros/cartographer_ros/submap.cc
+cartographer_ros/cartographer_ros/submap.h
+cartographer_ros/cartographer_ros/tf_bridge.cc
+cartographer_ros/cartographer_ros/tf_bridge.h
+cartographer_ros/cartographer_ros/time_conversion.cc
+cartographer_ros/cartographer_ros/time_conversion.h
+cartographer_ros/cartographer_ros/time_conversion_test.cc
+cartographer_ros/cartographer_ros/trajectory_options.cc
+cartographer_ros/cartographer_ros/trajectory_options.h
+cartographer_ros/cartographer_ros/urdf_reader.cc
+cartographer_ros/cartographer_ros/urdf_reader.h
+cartographer_rviz/cartographer_rviz/drawable_submap.cc
+cartographer_rviz/cartographer_rviz/drawable_submap.h
+cartographer_rviz/cartographer_rviz/ogre_slice.cc
+cartographer_rviz/cartographer_rviz/ogre_slice.h
+cartographer_rviz/cartographer_rviz/submaps_display.cc
+cartographer_rviz/cartographer_rviz/submaps_display.h

cartographer_ros/cartographer_ros.rosinstall

@@ -0,0 +1,3 @@
+- git: {local-name: cartographer, uri: 'https://github.com/googlecartographer/cartographer.git', version: '1.0.0'}
+- git: {local-name: cartographer_ros, uri: 'https://github.com/googlecartographer/cartographer_ros.git', version: '1.0.0'}
+- git: {local-name: ceres-solver, uri: 'https://ceres-solver.googlesource.com/ceres-solver.git', version: '1.13.0'}

cartographer_ros/cartographer_ros/CHANGELOG.rst

@@ -0,0 +1,19 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package cartographer_ros
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.0 (2018-06-01)
+----------------------
+* https://github.com/googlecartographer/cartographer_ros/compare/0.3.0...1.0.0
+
+0.3.0 (2017-11-23)
+------------------
+* https://github.com/googlecartographer/cartographer_ros/compare/0.2.0...0.3.0
+
+0.2.0 (2017-06-19)
+------------------
+* https://github.com/googlecartographer/cartographer_ros/compare/0.1.0...0.2.0
+
+0.1.0 (2017-05-18)
+------------------
+* First unstable development release

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -0,0 +1,196 @@
+# 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.
+
+cmake_minimum_required(VERSION 2.8.12)  # Ships with Ubuntu 14.04 (Trusty)
+
+project(cartographer_ros)
+
+set(PACKAGE_DEPENDENCIES
+  cartographer_ros_msgs
+  eigen_conversions
+  geometry_msgs
+  message_runtime
+  nav_msgs
+  pcl_conversions
+  rosbag
+  roscpp
+  roslib
+  sensor_msgs
+  std_msgs
+  tf2
+  tf2_eigen
+  tf2_ros
+  urdf
+  visualization_msgs
+)
+
+if(WIN32)
+  set(Boost_USE_STATIC_LIBS FALSE)
+endif()
+# For yet unknown reason, if Boost is find_packaged() after find_package(cartographer),
+# some Boost libraries including Thread are set to static, despite Boost_USE_STATIC_LIBS,
+# which causes linking problems on windows due to shared/static Thread clashing.
+# PCL also finds Boost. Work around by moving before find_package(cartographer).
+find_package(Boost REQUIRED COMPONENTS system iostreams)
+find_package(PCL REQUIRED COMPONENTS common)
+
+find_package(cartographer REQUIRED)
+include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake")
+option(BUILD_GRPC "build features that require Cartographer gRPC support" false)
+google_initialize_cartographer_project()
+google_enable_testing()
+
+find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
+
+include(FindPkgConfig)
+
+find_package(Abseil REQUIRED)
+find_package(LuaGoogle REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+find_package(urdfdom_headers REQUIRED)
+if(DEFINED urdfdom_headers_VERSION)
+  if(${urdfdom_headers_VERSION} GREATER 0.4.1)
+    add_definitions(-DURDFDOM_HEADERS_HAS_SHARED_PTR_DEFS)
+  endif()
+endif()
+
+include_directories(
+  ${urdfdom_headers_INCLUDE_DIRS}
+)
+
+# Override Catkin's GTest configuration to use GMock.
+set(GTEST_FOUND TRUE)
+set(GTEST_INCLUDE_DIRS ${GMOCK_INCLUDE_DIRS})
+set(GTEST_LIBRARIES ${GMOCK_LIBRARIES})
+
+catkin_package(
+  CATKIN_DEPENDS
+    ${PACKAGE_DEPENDENCIES}
+  DEPENDS
+    # TODO(damonkohler): This should be here but causes Catkin to abort because
+    # protobuf specifies a library '-lpthread' instead of just 'pthread'.
+    # CARTOGRAPHER
+    PCL
+    EIGEN3
+    Boost
+    urdfdom_headers
+  INCLUDE_DIRS "."
+  LIBRARIES ${PROJECT_NAME}
+)
+
+file(GLOB_RECURSE ALL_SRCS "cartographer_ros/*.cc" "cartographer_ros/*.h")
+file(GLOB_RECURSE ALL_TESTS "cartographer_ros/*_test.cc")
+file(GLOB_RECURSE ALL_EXECUTABLES "cartographer_ros/*_main.cc")
+file(GLOB_RECURSE ALL_GRPC_FILES "cartographer_ros/cartographer_grpc/*")
+list(REMOVE_ITEM ALL_SRCS ${ALL_TESTS})
+list(REMOVE_ITEM ALL_SRCS ${ALL_EXECUTABLES})
+if (NOT ${BUILD_GRPC})
+  list(REMOVE_ITEM ALL_SRCS ${ALL_GRPC_FILES})
+  list(REMOVE_ITEM ALL_TESTS ${ALL_GRPC_FILES})
+  list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_GRPC_FILES})
+endif()
+add_library(${PROJECT_NAME} STATIC ${ALL_SRCS})
+add_subdirectory("cartographer_ros")
+
+target_link_libraries(${PROJECT_NAME} PUBLIC cartographer)
+
+# Lua
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR})
+
+# PCL
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME} PUBLIC ${PCL_LIBRARIES})
+set(BLACKLISTED_PCL_DEFINITIONS " -march=native -msse4.2 -mfpmath=sse ")
+foreach(DEFINITION ${PCL_DEFINITIONS})
+  list (FIND BLACKLISTED_PCL_DEFINITIONS "${DEFINITION}" DEFINITIONS_INDEX)
+  if (${DEFINITIONS_INDEX} GREATER -1)
+    continue()
+  endif()
+  set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${DEFINITION}")
+endforeach()
+
+# Eigen
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
+  "${EIGEN3_INCLUDE_DIR}")
+
+# Boost
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
+  "${Boost_INCLUDE_DIRS}")
+target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES})
+
+# Catkin
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES})
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
+
+# Add the binary directory first, so that port.h is included after it has
+# been generated.
+target_include_directories(${PROJECT_NAME} PUBLIC
+    $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}>
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}>
+    $<INSTALL_INTERFACE:include>
+)
+
+set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}")
+set_target_properties(${PROJECT_NAME} PROPERTIES
+  COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
+
+if (CATKIN_ENABLE_TESTING)
+  foreach(TEST_SOURCE_FILENAME ${ALL_TESTS})
+    get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE)
+    catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME})
+    # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to
+    # target_link_libraries. That forces us to do the same.
+    target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR})
+    target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES})
+    target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
+    target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES})
+    add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS})
+    target_link_libraries(${TEST_NAME} cartographer)
+    target_link_libraries(${TEST_NAME} ${PROJECT_NAME})
+    set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
+    # Needed for dynamically linked GTest on Windows.
+    if (WIN32)
+      target_compile_definitions(${TEST_NAME} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY)
+    endif()
+  endforeach()
+endif()
+
+install(DIRECTORY launch urdf configuration_files
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(PROGRAMS scripts/tf_remove_frames.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+# Install source headers.
+file(GLOB_RECURSE HDRS "cartographer_ros/*.h")
+foreach(HDR ${HDRS})
+  file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR})
+  get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY)
+  install(
+    FILES
+      ${HDR}
+    DESTINATION
+      include/${INSTALL_DIR}
+  )
+endforeach()

cartographer_ros/cartographer_ros/cartographer_ros/.clang-format

@@ -0,0 +1,2 @@
+BasedOnStyle: Google
+DerivePointerAlignment: false

cartographer_ros/cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -0,0 +1,166 @@
+# 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.
+
+google_binary(cartographer_assets_writer
+  SRCS
+    assets_writer_main.cc
+    ros_map_writing_points_processor.h
+    ros_map_writing_points_processor.cc
+)
+
+install(TARGETS cartographer_assets_writer
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_node
+  SRCS
+    node_main.cc
+)
+
+install(TARGETS cartographer_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_offline_node
+  SRCS
+    offline_node_main.cc
+)
+
+install(TARGETS cartographer_offline_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_start_trajectory
+  SRCS
+    start_trajectory_main.cc
+)
+
+install(TARGETS cartographer_start_trajectory
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_occupancy_grid_node
+  SRCS
+    occupancy_grid_node_main.cc
+)
+
+install(TARGETS cartographer_occupancy_grid_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_rosbag_validate
+  SRCS
+    rosbag_validate_main.cc
+)
+
+install(TARGETS cartographer_rosbag_validate
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_pbstream_to_ros_map
+  SRCS
+    pbstream_to_ros_map_main.cc
+)
+
+install(TARGETS cartographer_pbstream_to_ros_map
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_pbstream_map_publisher
+  SRCS
+    pbstream_map_publisher_main.cc
+)
+
+install(TARGETS cartographer_pbstream_map_publisher
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_dev_pbstream_trajectories_to_rosbag
+  SRCS
+  dev/pbstream_trajectories_to_rosbag_main.cc
+)
+
+install(TARGETS cartographer_dev_pbstream_trajectories_to_rosbag
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_dev_rosbag_publisher
+  SRCS
+  dev/rosbag_publisher_main.cc
+)
+
+install(TARGETS cartographer_dev_rosbag_publisher
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+google_binary(cartographer_dev_trajectory_comparison
+  SRCS
+  dev/trajectory_comparison_main.cc
+)
+
+install(TARGETS cartographer_dev_trajectory_comparison
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# TODO(cschuet): Add support for shared library case.
+if (${BUILD_GRPC})
+  google_binary(cartographer_grpc_node
+    SRCS
+      cartographer_grpc/node_grpc_main.cc
+  )
+
+  install(TARGETS cartographer_grpc_node
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+  google_binary(cartographer_grpc_offline_node
+    SRCS
+      cartographer_grpc/offline_node_grpc_main.cc
+  )
+
+  install(TARGETS cartographer_grpc_offline_node
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+  install(PROGRAMS
+    ../scripts/cartographer_grpc_server.sh
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+endif()

cartographer_ros/cartographer_ros/cartographer_ros/assets_writer.cc

@@ -0,0 +1,272 @@
+/*
+ * 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 "cartographer_ros/assets_writer.h"
+
+#include <algorithm>
+#include <fstream>
+#include <iostream>
+
+#include "absl/memory/memory.h"
+#include "cartographer/common/configuration_file_resolver.h"
+#include "cartographer/common/math.h"
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/points_processor.h"
+#include "cartographer/io/points_processor_pipeline_builder.h"
+#include "cartographer/io/proto_stream.h"
+#include "cartographer/io/proto_stream_deserializer.h"
+#include "cartographer/mapping/proto/pose_graph.pb.h"
+#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/sensor/range_data.h"
+#include "cartographer/transform/transform_interpolation_buffer.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/ros_map_writing_points_processor.h"
+#include "cartographer_ros/time_conversion.h"
+#include "cartographer_ros/urdf_reader.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "ros/ros.h"
+#include "ros/time.h"
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+#include "tf2_eigen/tf2_eigen.h"
+#include "tf2_msgs/TFMessage.h"
+#include "tf2_ros/buffer.h"
+#include "urdf/model.h"
+
+namespace cartographer_ros {
+namespace {
+
+constexpr char kTfStaticTopic[] = "/tf_static";
+namespace carto = ::cartographer;
+
+std::unique_ptr<carto::io::PointsProcessorPipelineBuilder>
+CreatePipelineBuilder(
+    const std::vector<carto::mapping::proto::Trajectory>& trajectories,
+    const std::string file_prefix) {
+  const auto file_writer_factory =
+      AssetsWriter::CreateFileWriterFactory(file_prefix);
+  auto builder = absl::make_unique<carto::io::PointsProcessorPipelineBuilder>();
+  carto::io::RegisterBuiltInPointsProcessors(trajectories, file_writer_factory,
+                                             builder.get());
+  builder->Register(RosMapWritingPointsProcessor::kConfigurationFileActionName,
+                    [file_writer_factory](
+                        carto::common::LuaParameterDictionary* const dictionary,
+                        carto::io::PointsProcessor* const next)
+                        -> std::unique_ptr<carto::io::PointsProcessor> {
+                      return RosMapWritingPointsProcessor::FromDictionary(
+                          file_writer_factory, dictionary, next);
+                    });
+  return builder;
+}
+
+std::unique_ptr<carto::common::LuaParameterDictionary> LoadLuaDictionary(
+    const std::string& configuration_directory,
+    const std::string& configuration_basename) {
+  auto file_resolver =
+      absl::make_unique<carto::common::ConfigurationFileResolver>(
+          std::vector<std::string>{configuration_directory});
+
+  const std::string code =
+      file_resolver->GetFileContentOrDie(configuration_basename);
+  auto lua_parameter_dictionary =
+      absl::make_unique<carto::common::LuaParameterDictionary>(
+          code, std::move(file_resolver));
+  return lua_parameter_dictionary;
+}
+
+template <typename T>
+std::unique_ptr<carto::io::PointsBatch> HandleMessage(
+    const T& message, const std::string& tracking_frame,
+    const tf2_ros::Buffer& tf_buffer,
+    const carto::transform::TransformInterpolationBuffer&
+        transform_interpolation_buffer) {
+  const carto::common::Time start_time = FromRos(message.header.stamp);
+
+  auto points_batch = absl::make_unique<carto::io::PointsBatch>();
+  points_batch->start_time = start_time;
+  points_batch->frame_id = message.header.frame_id;
+
+  carto::sensor::PointCloudWithIntensities point_cloud;
+  carto::common::Time point_cloud_time;
+  std::tie(point_cloud, point_cloud_time) =
+      ToPointCloudWithIntensities(message);
+  CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size());
+
+  for (size_t i = 0; i < point_cloud.points.size(); ++i) {
+    const carto::common::Time time =
+        point_cloud_time +
+        carto::common::FromSeconds(point_cloud.points[i].time);
+    if (!transform_interpolation_buffer.Has(time)) {
+      continue;
+    }
+    const carto::transform::Rigid3d tracking_to_map =
+        transform_interpolation_buffer.Lookup(time);
+    const carto::transform::Rigid3d sensor_to_tracking =
+        ToRigid3d(tf_buffer.lookupTransform(
+            tracking_frame, message.header.frame_id, ToRos(time)));
+    const carto::transform::Rigid3f sensor_to_map =
+        (tracking_to_map * sensor_to_tracking).cast<float>();
+    points_batch->points.push_back(
+        sensor_to_map *
+        carto::sensor::ToRangefinderPoint(point_cloud.points[i]));
+    points_batch->intensities.push_back(point_cloud.intensities[i]);
+    // We use the last transform for the origin, which is approximately correct.
+    points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
+  }
+  if (points_batch->points.empty()) {
+    return nullptr;
+  }
+  return points_batch;
+}
+
+}  // namespace
+
+AssetsWriter::AssetsWriter(const std::string& pose_graph_filename,
+                           const std::vector<std::string>& bag_filenames,
+                           const std::string& output_file_prefix)
+    : bag_filenames_(bag_filenames),
+      pose_graph_(
+          carto::io::DeserializePoseGraphFromFile(pose_graph_filename)) {
+  CHECK_EQ(pose_graph_.trajectory_size(), bag_filenames_.size())
+      << "Pose graphs contains " << pose_graph_.trajectory_size()
+      << " trajectories while " << bag_filenames_.size()
+      << " bags were provided. This tool requires one bag for each "
+         "trajectory in the same order as the correponding trajectories in the "
+         "pose graph proto.";
+
+  // This vector must outlive the pipeline.
+  all_trajectories_ = std::vector<::cartographer::mapping::proto::Trajectory>(
+      pose_graph_.trajectory().begin(), pose_graph_.trajectory().end());
+
+  const std::string file_prefix = !output_file_prefix.empty()
+                                      ? output_file_prefix
+                                      : bag_filenames_.front() + "_";
+  point_pipeline_builder_ =
+      CreatePipelineBuilder(all_trajectories_, file_prefix);
+}
+
+void AssetsWriter::RegisterPointsProcessor(
+    const std::string& name,
+    cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction factory) {
+  point_pipeline_builder_->Register(name, factory);
+}
+
+void AssetsWriter::Run(const std::string& configuration_directory,
+                       const std::string& configuration_basename,
+                       const std::string& urdf_filename,
+                       const bool use_bag_transforms) {
+  const auto lua_parameter_dictionary =
+      LoadLuaDictionary(configuration_directory, configuration_basename);
+
+  std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
+      point_pipeline_builder_->CreatePipeline(
+          lua_parameter_dictionary->GetDictionary("pipeline").get());
+  const std::string tracking_frame =
+      lua_parameter_dictionary->GetString("tracking_frame");
+
+  do {
+    for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size();
+         ++trajectory_id) {
+      const carto::mapping::proto::Trajectory& trajectory_proto =
+          pose_graph_.trajectory(trajectory_id);
+      const std::string& bag_filename = bag_filenames_[trajectory_id];
+      LOG(INFO) << "Processing " << bag_filename << "...";
+      if (trajectory_proto.node_size() == 0) {
+        continue;
+      }
+      tf2_ros::Buffer tf_buffer;
+      if (!urdf_filename.empty()) {
+        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
+      }
+
+      const carto::transform::TransformInterpolationBuffer
+          transform_interpolation_buffer(trajectory_proto);
+      rosbag::Bag bag;
+      bag.open(bag_filename, rosbag::bagmode::Read);
+      rosbag::View view(bag);
+      const ::ros::Time begin_time = view.getBeginTime();
+      const double duration_in_seconds =
+          (view.getEndTime() - begin_time).toSec();
+
+      // We need to keep 'tf_buffer' small because it becomes very inefficient
+      // otherwise. We make sure that tf_messages are published before any data
+      // messages, so that tf lookups always work.
+      std::deque<rosbag::MessageInstance> delayed_messages;
+      // We publish tf messages one second earlier than other messages. Under
+      // the assumption of higher frequency tf this should ensure that tf can
+      // always interpolate.
+      const ::ros::Duration kDelay(1.);
+      for (const rosbag::MessageInstance& message : view) {
+        if (use_bag_transforms && message.isType<tf2_msgs::TFMessage>()) {
+          auto tf_message = message.instantiate<tf2_msgs::TFMessage>();
+          for (const auto& transform : tf_message->transforms) {
+            try {
+              tf_buffer.setTransform(transform, "unused_authority",
+                                     message.getTopic() == kTfStaticTopic);
+            } catch (const tf2::TransformException& ex) {
+              LOG(WARNING) << ex.what();
+            }
+          }
+        }
+
+        while (!delayed_messages.empty() && delayed_messages.front().getTime() <
+                                                message.getTime() - kDelay) {
+          const rosbag::MessageInstance& delayed_message =
+              delayed_messages.front();
+
+          std::unique_ptr<carto::io::PointsBatch> points_batch;
+          if (delayed_message.isType<sensor_msgs::PointCloud2>()) {
+            points_batch = HandleMessage(
+                *delayed_message.instantiate<sensor_msgs::PointCloud2>(),
+                tracking_frame, tf_buffer, transform_interpolation_buffer);
+          } else if (delayed_message
+                         .isType<sensor_msgs::MultiEchoLaserScan>()) {
+            points_batch = HandleMessage(
+                *delayed_message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
+                tracking_frame, tf_buffer, transform_interpolation_buffer);
+          } else if (delayed_message.isType<sensor_msgs::LaserScan>()) {
+            points_batch = HandleMessage(
+                *delayed_message.instantiate<sensor_msgs::LaserScan>(),
+                tracking_frame, tf_buffer, transform_interpolation_buffer);
+          }
+          if (points_batch != nullptr) {
+            points_batch->trajectory_id = trajectory_id;
+            pipeline.back()->Process(std::move(points_batch));
+          }
+          delayed_messages.pop_front();
+        }
+        delayed_messages.push_back(message);
+        LOG_EVERY_N(INFO, 10000)
+            << "Processed " << (message.getTime() - begin_time).toSec()
+            << " of " << duration_in_seconds << " bag time seconds...";
+      }
+      bag.close();
+    }
+  } while (pipeline.back()->Flush() ==
+           carto::io::PointsProcessor::FlushResult::kRestartStream);
+}
+
+::cartographer::io::FileWriterFactory AssetsWriter::CreateFileWriterFactory(
+    const std::string& file_path) {
+  const auto file_writer_factory = [file_path](const std::string& filename) {
+    return absl::make_unique<carto::io::StreamFileWriter>(file_path + filename);
+  };
+  return file_writer_factory;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/assets_writer.h

@@ -0,0 +1,63 @@
+/*
+ * 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 <string>
+#include <vector>
+
+#include "cartographer/common/configuration_file_resolver.h"
+#include "cartographer/io/points_processor_pipeline_builder.h"
+#include "cartographer/mapping/proto/pose_graph.pb.h"
+#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H
+
+namespace cartographer_ros {
+
+class AssetsWriter {
+ public:
+  AssetsWriter(const std::string& pose_graph_filename,
+               const std::vector<std::string>& bag_filenames,
+               const std::string& output_file_prefix);
+
+  // Registers a new PointsProcessor type uniquly identified by 'name' which
+  // will be created using 'factory'.
+  void RegisterPointsProcessor(
+      const std::string& name,
+      cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction
+          factory);
+
+  // Configures a points processing pipeline and pushes the points from the
+  // bag through the pipeline.
+  void Run(const std::string& configuration_directory,
+           const std::string& configuration_basename,
+           const std::string& urdf_filename, bool use_bag_transforms);
+
+  // Creates a FileWriterFactory which creates a FileWriter for storing assets.
+  static ::cartographer::io::FileWriterFactory CreateFileWriterFactory(
+      const std::string& file_path);
+
+ private:
+  std::vector<std::string> bag_filenames_;
+  std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories_;
+  ::cartographer::mapping::proto::PoseGraph pose_graph_;
+  std::unique_ptr<::cartographer::io::PointsProcessorPipelineBuilder>
+      point_pipeline_builder_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H

cartographer_ros/cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -0,0 +1,64 @@
+/*
+ * 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 "absl/strings/str_split.h"
+#include "cartographer_ros/assets_writer.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_string(configuration_directory, "",
+              "First directory in which configuration files are searched, "
+              "second is always the Cartographer installation to allow "
+              "including files from there.");
+DEFINE_string(configuration_basename, "",
+              "Basename, i.e. not containing any directory prefix, of the "
+              "configuration file.");
+DEFINE_string(
+    urdf_filename, "",
+    "URDF file that contains static links for your sensor configuration.");
+DEFINE_string(bag_filenames, "",
+              "Bags to process, must be in the same order as the trajectories "
+              "in 'pose_graph_filename'.");
+DEFINE_string(pose_graph_filename, "",
+              "Proto stream file containing the pose graph.");
+DEFINE_bool(use_bag_transforms, true,
+            "Whether to read and use the transforms from the bag.");
+DEFINE_string(output_file_prefix, "",
+              "Will be prefixed to all output file names and can be used to "
+              "define the output directory. If empty, the first bag filename "
+              "will be used.");
+
+int main(int argc, char** argv) {
+  FLAGS_alsologtostderr = true;
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_configuration_directory.empty())
+      << "-configuration_directory is missing.";
+  CHECK(!FLAGS_configuration_basename.empty())
+      << "-configuration_basename is missing.";
+  CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
+  CHECK(!FLAGS_pose_graph_filename.empty())
+      << "-pose_graph_filename is missing.";
+
+  ::cartographer_ros::AssetsWriter asset_writer(
+      FLAGS_pose_graph_filename,
+      absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()),
+      FLAGS_output_file_prefix);
+
+  asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename,
+                   FLAGS_urdf_filename, FLAGS_use_bag_transforms);
+}

cartographer_ros/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc

@@ -0,0 +1,117 @@
+/*
+ * Copyright 2017 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 "absl/memory/memory.h"
+#include "cartographer/cloud/client/map_builder_stub.h"
+#include "cartographer_ros/node.h"
+#include "cartographer_ros/node_options.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "gflags/gflags.h"
+#include "tf2_ros/transform_listener.h"
+
+DEFINE_bool(collect_metrics, false,
+            "Activates the collection of runtime metrics. If activated, the "
+            "metrics can be accessed via a ROS service.");
+DEFINE_string(configuration_directory, "",
+              "First directory in which configuration files are searched, "
+              "second is always the Cartographer installation to allow "
+              "including files from there.");
+DEFINE_string(configuration_basename, "",
+              "Basename, i.e. not containing any directory prefix, of the "
+              "configuration file.");
+DEFINE_string(server_address, "localhost:50051",
+              "gRPC server address to stream the sensor data to.");
+DEFINE_bool(
+    start_trajectory_with_default_topics, true,
+    "Enable to immediately start the first trajectory with default topics.");
+DEFINE_string(
+    save_map_filename, "",
+    "If non-empty, serialize state and write it to disk before shutting down.");
+DEFINE_string(load_state_filename, "",
+              "If non-empty, filename of a .pbstream file "
+              "to load, containing a saved SLAM state. "
+              "Unless --upload_load_state_file is set, the filepath refers "
+              "to the gRPC server's file system.");
+DEFINE_bool(load_frozen_state, true,
+            "Load the saved state as frozen (non-optimized) trajectories.");
+DEFINE_bool(upload_load_state_file, false,
+            "Upload the .pbstream file from a local path to the (remote) gRPC "
+            "server instead of loading it from the server file system.");
+DEFINE_string(client_id, "",
+              "Cartographer client ID to use when connecting to the server.");
+
+namespace cartographer_ros {
+namespace {
+
+void Run() {
+  constexpr double kTfBufferCacheTimeInSeconds = 10.;
+  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
+  tf2_ros::TransformListener tf(tf_buffer);
+  NodeOptions node_options;
+  TrajectoryOptions trajectory_options;
+  std::tie(node_options, trajectory_options) =
+      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
+
+  auto map_builder = absl::make_unique<::cartographer::cloud::MapBuilderStub>(
+      FLAGS_server_address, FLAGS_client_id);
+
+  if (!FLAGS_load_state_filename.empty() && !FLAGS_upload_load_state_file) {
+    map_builder->LoadStateFromFile(FLAGS_load_state_filename,
+                                   FLAGS_load_frozen_state);
+  }
+
+  Node node(node_options, std::move(map_builder), &tf_buffer,
+            FLAGS_collect_metrics);
+
+  if (!FLAGS_load_state_filename.empty() && FLAGS_upload_load_state_file) {
+    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
+  }
+
+  if (FLAGS_start_trajectory_with_default_topics) {
+    node.StartTrajectoryWithDefaultTopics(trajectory_options);
+  }
+
+  ::ros::spin();
+
+  node.FinishAllTrajectories();
+  node.RunFinalOptimization();
+
+  if (!FLAGS_save_map_filename.empty()) {
+    node.SerializeState(FLAGS_save_map_filename,
+                        false /* include_unfinished_submaps */);
+  }
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_configuration_directory.empty())
+      << "-configuration_directory is missing.";
+  CHECK(!FLAGS_configuration_basename.empty())
+      << "-configuration_basename is missing.";
+  CHECK(!FLAGS_client_id.empty()) << "-client_id is missing.";
+
+  ::ros::init(argc, argv, "cartographer_grpc_node");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+  cartographer_ros::Run();
+  ::ros::shutdown();
+}

cartographer_ros/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc

@@ -0,0 +1,49 @@
+/*
+ * Copyright 2018 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 "cartographer/cloud/client/map_builder_stub.h"
+#include "cartographer_ros/offline_node.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "gflags/gflags.h"
+#include "ros/ros.h"
+
+DEFINE_string(server_address, "localhost:50051",
+              "gRPC server address to "
+              "stream the sensor data to.");
+DEFINE_string(client_id, "",
+              "Cartographer client ID to use when connecting to the server.");
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_client_id.empty()) << "-client_id is missing.";
+
+  ::ros::init(argc, argv, "cartographer_grpc_offline_node");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+
+  const cartographer_ros::MapBuilderFactory map_builder_factory =
+      [](const ::cartographer::mapping::proto::MapBuilderOptions&) {
+        return absl::make_unique< ::cartographer::cloud::MapBuilderStub>(
+            FLAGS_server_address, FLAGS_client_id);
+      };
+
+  cartographer_ros::RunOfflineNode(map_builder_factory);
+
+  ::ros::shutdown();
+}

cartographer_ros/cartographer_ros/cartographer_ros/configuration_files_test.cc

@@ -0,0 +1,44 @@
+/*
+ * 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 <string>
+#include <vector>
+
+#include "cartographer_ros/node_options.h"
+#include "gtest/gtest.h"
+#include "ros/package.h"
+
+namespace cartographer_ros {
+namespace {
+
+class ConfigurationFilesTest : public ::testing::TestWithParam<const char*> {};
+
+TEST_P(ConfigurationFilesTest, ValidateNodeOptions) {
+  EXPECT_NO_FATAL_FAILURE({
+    LoadOptions(
+        ::ros::package::getPath("cartographer_ros") + "/configuration_files",
+        GetParam());
+  });
+}
+
+INSTANTIATE_TEST_CASE_P(
+    ValidateAllNodeOptions, ConfigurationFilesTest,
+    ::testing::Values("backpack_2d.lua", "backpack_2d_localization.lua",
+                      "backpack_3d.lua", "backpack_3d_localization.lua",
+                      "pr2.lua", "revo_lds.lua", "taurob_tracker.lua"));
+
+}  // namespace
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc

@@ -0,0 +1,95 @@
+/*
+ * Copyright 2019 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 <iostream>
+#include <string>
+#include <vector>
+
+#include "absl/strings/str_cat.h"
+#include "cartographer/transform/transform.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/time_conversion.h"
+#include "geometry_msgs/TransformStamped.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "rosbag/bag.h"
+#include "tf2_msgs/TFMessage.h"
+
+DEFINE_string(input, "", "pbstream file to process");
+DEFINE_string(output, "", "Bag file to write to.");
+DEFINE_string(parent_frame, "map", "Frame id to use as parent frame.");
+
+namespace cartographer_ros {
+namespace {
+
+geometry_msgs::TransformStamped ToTransformStamped(
+    int64_t timestamp_uts, const std::string& parent_frame_id,
+    const std::string& child_frame_id,
+    const cartographer::transform::proto::Rigid3d& parent_T_child) {
+  static int64_t seq = 0;
+  geometry_msgs::TransformStamped xfm;
+  xfm.header.seq = ++seq;
+  xfm.header.frame_id = parent_frame_id;
+  xfm.header.stamp = cartographer_ros::ToRos(
+      ::cartographer::common::FromUniversal(timestamp_uts));
+  xfm.child_frame_id = child_frame_id;
+  xfm.transform = cartographer_ros::ToGeometryMsgTransform(
+      ::cartographer::transform::ToRigid3(parent_T_child));
+  return xfm;
+}
+
+void pbstream_trajectories_to_bag(const std::string& pbstream_filename,
+                                  const std::string& output_bag_filename,
+                                  const std::string& parent_frame_id) {
+  const auto pose_graph =
+      cartographer::io::DeserializePoseGraphFromFile(FLAGS_input);
+
+  rosbag::Bag bag(output_bag_filename, rosbag::bagmode::Write);
+  for (const auto trajectory : pose_graph.trajectory()) {
+    const auto child_frame_id =
+        absl::StrCat("trajectory_", trajectory.trajectory_id());
+    LOG(INFO) << "Writing tf for trajectory id " << trajectory.trajectory_id()
+              << " with " << trajectory.node_size() << " nodes.";
+    for (const auto& node : trajectory.node()) {
+      tf2_msgs::TFMessage tf_msg;
+      tf_msg.transforms.push_back(ToTransformStamped(
+          node.timestamp(), parent_frame_id, child_frame_id, node.pose()));
+      bag.write("/tf", tf_msg.transforms[0].header.stamp, tf_msg);
+    }
+  }
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char* argv[]) {
+  FLAGS_alsologtostderr = true;
+  google::InitGoogleLogging(argv[0]);
+  google::SetUsageMessage(
+      "\n\n"
+      "Extracts all trajectories from the pbstream and creates a bag file with "
+      "the trajectory poses stored in /tf.\nFor each trajectory, the tool will "
+      "write transforms with the tf parent_frame_id set according to the "
+      "`parent_frame` commandline flag and child_frame_id to `trajectory_i`, "
+      "with `i` corresponding to the `trajectory_id`.");
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  CHECK(!FLAGS_input.empty()) << "-input pbstream is missing.";
+  CHECK(!FLAGS_output.empty()) << "-output is missing.";
+
+  cartographer_ros::pbstream_trajectories_to_bag(FLAGS_input, FLAGS_output,
+                                                 FLAGS_parent_frame);
+  return 0;
+}

cartographer_ros/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc

@@ -0,0 +1,148 @@
+/*
+ * Copyright 2018 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 "cartographer/common/time.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "cartographer_ros/time_conversion.h"
+#include "gflags/gflags.h"
+#include "nav_msgs/Odometry.h"
+#include "ros/ros.h"
+#include "ros/time.h"
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "tf2_msgs/TFMessage.h"
+
+DEFINE_string(bag_filename, "", "Bag to publish.");
+
+const int kQueueSize = 1;
+
+template <typename MessagePtrType>
+void PublishWithModifiedTimestamp(MessagePtrType message,
+                                  const ros::Publisher& publisher,
+                                  ros::Duration bag_to_current) {
+  ros::Time& stamp = message->header.stamp;
+  stamp += bag_to_current;
+  publisher.publish(message);
+}
+
+template <>
+void PublishWithModifiedTimestamp<tf2_msgs::TFMessage::Ptr>(
+    tf2_msgs::TFMessage::Ptr message, const ros::Publisher& publisher,
+    ros::Duration bag_to_current) {
+  for (const auto& transform : message->transforms) {
+    ros::Time& stamp = const_cast<ros::Time&>(transform.header.stamp);
+    stamp += bag_to_current;
+  }
+  publisher.publish(message);
+}
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::SetUsageMessage(
+      "\n\n"
+      "This replays and publishes messages from a given bag file, modifying "
+      "their header timestamps to match current ROS time.\n\n"
+      "Messages are published in the same sequence and with the same delay "
+      "they were recorded."
+      "Contrary to rosbag play, it does not publish a clock, so time is"
+      "hopefully smoother and it should be possible to reproduce timing"
+      "issues.\n"
+      "It only plays message types related to Cartographer.\n");
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
+
+  ros::init(argc, argv, "rosbag_publisher");
+  ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+
+  rosbag::Bag bag;
+  bag.open(FLAGS_bag_filename, rosbag::bagmode::Read);
+  rosbag::View view(bag);
+  ros::NodeHandle node_handle;
+  bool use_sim_time;
+  node_handle.getParam("/use_sim_time", use_sim_time);
+  if (use_sim_time) {
+    LOG(ERROR) << "use_sim_time is true but not supported. Expect conflicting "
+                  "ros::Time and message header times or weird behavior.";
+  }
+  std::map<std::string, ros::Publisher> topic_to_publisher;
+  for (const rosbag::ConnectionInfo* c : view.getConnections()) {
+    const std::string& topic = c->topic;
+    if (topic_to_publisher.count(topic) == 0) {
+      ros::AdvertiseOptions options(c->topic, kQueueSize, c->md5sum,
+                                    c->datatype, c->msg_def);
+      topic_to_publisher[topic] = node_handle.advertise(options);
+    }
+  }
+  ros::Duration(1).sleep();
+  CHECK(ros::ok());
+
+  ros::Time current_start = ros::Time::now();
+  ros::Time bag_start = view.getBeginTime();
+  ros::Duration bag_to_current = current_start - bag_start;
+  for (const rosbag::MessageInstance& message : view) {
+    ros::Duration after_bag_start = message.getTime() - bag_start;
+    if (!::ros::ok()) {
+      break;
+    }
+    ros::Time planned_publish_time = current_start + after_bag_start;
+    ros::Time::sleepUntil(planned_publish_time);
+
+    ros::Publisher& publisher = topic_to_publisher.at(message.getTopic());
+    if (message.isType<sensor_msgs::PointCloud2>()) {
+      PublishWithModifiedTimestamp(
+          message.instantiate<sensor_msgs::PointCloud2>(), publisher,
+          bag_to_current);
+    } else if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
+      PublishWithModifiedTimestamp(
+          message.instantiate<sensor_msgs::MultiEchoLaserScan>(), publisher,
+          bag_to_current);
+    } else if (message.isType<sensor_msgs::LaserScan>()) {
+      PublishWithModifiedTimestamp(
+          message.instantiate<sensor_msgs::LaserScan>(), publisher,
+          bag_to_current);
+    } else if (message.isType<sensor_msgs::Imu>()) {
+      PublishWithModifiedTimestamp(message.instantiate<sensor_msgs::Imu>(),
+                                   publisher, bag_to_current);
+    } else if (message.isType<nav_msgs::Odometry>()) {
+      PublishWithModifiedTimestamp(message.instantiate<nav_msgs::Odometry>(),
+                                   publisher, bag_to_current);
+    } else if (message.isType<tf2_msgs::TFMessage>()) {
+      PublishWithModifiedTimestamp(message.instantiate<tf2_msgs::TFMessage>(),
+                                   publisher, bag_to_current);
+    } else {
+      LOG(WARNING) << "Skipping message with type " << message.getDataType();
+    }
+
+    ros::Time current_time = ros::Time::now();
+    double simulation_delay = cartographer::common::ToSeconds(
+        cartographer_ros::FromRos(current_time) -
+        cartographer_ros::FromRos(planned_publish_time));
+    if (std::abs(simulation_delay) > 0.001) {
+      LOG(WARNING) << "Playback delayed by " << simulation_delay
+                   << " s. planned_publish_time: " << planned_publish_time
+                   << " current_time: " << current_time;
+    }
+  }
+  bag.close();
+
+  ros::shutdown();
+}

cartographer_ros/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc

@@ -0,0 +1,143 @@
+/*
+ * Copyright 2018 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 <algorithm>
+#include <fstream>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "cartographer/common/math.h"
+#include "cartographer/io/proto_stream_deserializer.h"
+#include "cartographer/mapping/proto/pose_graph.pb.h"
+#include "cartographer/transform/transform_interpolation_buffer.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/time_conversion.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "ros/ros.h"
+#include "ros/time.h"
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+#include "tf2_eigen/tf2_eigen.h"
+#include "tf2_msgs/TFMessage.h"
+
+DEFINE_string(bag_filename, "",
+              "Bag file containing TF messages of the trajectory that will be "
+              "compared against the trajectory in the .pbstream file.");
+DEFINE_string(tf_parent_frame, "map",
+              "The parent frame ID of the TF trajectory from the bag file.");
+DEFINE_string(tf_child_frame, "base_link",
+              "The child frame ID of the TF trajectory from the bag file.");
+DEFINE_string(pbstream_filename, "",
+              "Proto stream file containing the pose graph. The last "
+              "trajectory will be used for comparison.");
+
+namespace cartographer_ros {
+namespace {
+
+double FractionSmallerThan(const std::vector<double>& v, double x) {
+  return static_cast<double>(std::count_if(
+             v.begin(), v.end(), [=](double value) { return value < x; })) /
+         v.size();
+}
+
+std::string QuantilesToString(std::vector<double>* v) {
+  if (v->empty()) return "(empty vector)";
+  std::sort(v->begin(), v->end());
+  std::stringstream result;
+  const int kNumQuantiles = 10;
+  for (int i = 0; i < kNumQuantiles; ++i) {
+    auto value = v->at(v->size() * i / kNumQuantiles);
+    auto percentage = 100 * i / kNumQuantiles;
+    result << percentage << "%: " << value << "\n";
+  }
+  result << "100%: " << v->back() << "\n";
+  return result.str();
+}
+
+void Run(const std::string& pbstream_filename,
+         const std::string& bag_filename) {
+  cartographer::mapping::proto::PoseGraph pose_graph_proto =
+      cartographer::io::DeserializePoseGraphFromFile(pbstream_filename);
+  const cartographer::mapping::proto::Trajectory& last_trajectory_proto =
+      *pose_graph_proto.mutable_trajectory()->rbegin();
+  const cartographer::transform::TransformInterpolationBuffer
+      transform_interpolation_buffer(last_trajectory_proto);
+
+  rosbag::Bag bag;
+  bag.open(bag_filename, rosbag::bagmode::Read);
+  rosbag::View view(bag);
+  std::vector<double> deviation_translation, deviation_rotation;
+  const double signal_maximum = std::numeric_limits<double>::max();
+  for (const rosbag::MessageInstance& message : view) {
+    if (!message.isType<tf2_msgs::TFMessage>()) {
+      continue;
+    }
+    auto tf_message = message.instantiate<tf2_msgs::TFMessage>();
+    for (const auto& transform : tf_message->transforms) {
+      if (transform.header.frame_id != FLAGS_tf_parent_frame ||
+          transform.child_frame_id != FLAGS_tf_child_frame) {
+        continue;
+      }
+      const cartographer::common::Time transform_time =
+          FromRos(message.getTime());
+      if (!transform_interpolation_buffer.Has(transform_time)) {
+        deviation_translation.push_back(signal_maximum);
+        deviation_rotation.push_back(signal_maximum);
+        continue;
+      }
+      auto optimized_transform =
+          transform_interpolation_buffer.Lookup(transform_time);
+      auto published_transform = ToRigid3d(transform);
+      deviation_translation.push_back((published_transform.translation() -
+                                       optimized_transform.translation())
+                                          .norm());
+      deviation_rotation.push_back(
+          published_transform.rotation().angularDistance(
+              optimized_transform.rotation()));
+    }
+  }
+  bag.close();
+  LOG(INFO) << "Distribution of translation difference:\n"
+            << QuantilesToString(&deviation_translation);
+  LOG(INFO) << "Distribution of rotation difference:\n"
+            << QuantilesToString(&deviation_rotation);
+  LOG(INFO) << "Fraction of translation difference smaller than 1m: "
+            << FractionSmallerThan(deviation_translation, 1);
+  LOG(INFO) << "Fraction of translation difference smaller than 0.1m: "
+            << FractionSmallerThan(deviation_translation, 0.1);
+  LOG(INFO) << "Fraction of translation difference smaller than 0.05m: "
+            << FractionSmallerThan(deviation_translation, 0.05);
+  LOG(INFO) << "Fraction of translation difference smaller than 0.01m: "
+            << FractionSmallerThan(deviation_translation, 0.01);
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  FLAGS_alsologtostderr = true;
+  google::InitGoogleLogging(argv[0]);
+  google::SetUsageMessage(
+      "\n\n"
+      "This compares a trajectory from a bag file against the "
+      "last trajectory in a pbstream file.\n");
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
+  CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
+  ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_bag_filename);
+}

cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -0,0 +1,513 @@
+/*
+ * 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 "cartographer_ros/map_builder_bridge.h"
+
+#include "absl/memory/memory.h"
+#include "cartographer/io/color.h"
+#include "cartographer/io/proto_stream.h"
+#include "cartographer/mapping/pose_graph.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros_msgs/StatusCode.h"
+#include "cartographer_ros_msgs/StatusResponse.h"
+
+namespace cartographer_ros {
+namespace {
+
+using ::cartographer::transform::Rigid3d;
+
+constexpr double kTrajectoryLineStripMarkerScale = 0.07;
+constexpr double kLandmarkMarkerScale = 0.2;
+constexpr double kConstraintMarkerScale = 0.025;
+
+::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
+  ::std_msgs::ColorRGBA result;
+  result.r = color[0];
+  result.g = color[1];
+  result.b = color[2];
+  result.a = 1.f;
+  return result;
+}
+
+visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
+                                                  const std::string& frame_id) {
+  visualization_msgs::Marker marker;
+  marker.ns = "Trajectory " + std::to_string(trajectory_id);
+  marker.id = 0;
+  marker.type = visualization_msgs::Marker::LINE_STRIP;
+  marker.header.stamp = ::ros::Time::now();
+  marker.header.frame_id = frame_id;
+  marker.color = ToMessage(cartographer::io::GetColor(trajectory_id));
+  marker.scale.x = kTrajectoryLineStripMarkerScale;
+  marker.pose.orientation.w = 1.;
+  marker.pose.position.z = 0.05;
+  return marker;
+}
+
+int GetLandmarkIndex(
+    const std::string& landmark_id,
+    std::unordered_map<std::string, int>* landmark_id_to_index) {
+  auto it = landmark_id_to_index->find(landmark_id);
+  if (it == landmark_id_to_index->end()) {
+    const int new_index = landmark_id_to_index->size();
+    landmark_id_to_index->emplace(landmark_id, new_index);
+    return new_index;
+  }
+  return it->second;
+}
+
+visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
+                                                const Rigid3d& landmark_pose,
+                                                const std::string& frame_id) {
+  visualization_msgs::Marker marker;
+  marker.ns = "Landmarks";
+  marker.id = landmark_index;
+  marker.type = visualization_msgs::Marker::SPHERE;
+  marker.header.stamp = ::ros::Time::now();
+  marker.header.frame_id = frame_id;
+  marker.scale.x = kLandmarkMarkerScale;
+  marker.scale.y = kLandmarkMarkerScale;
+  marker.scale.z = kLandmarkMarkerScale;
+  marker.color = ToMessage(cartographer::io::GetColor(landmark_index));
+  marker.pose = ToGeometryMsgPose(landmark_pose);
+  return marker;
+}
+
+void PushAndResetLineMarker(visualization_msgs::Marker* marker,
+                            std::vector<visualization_msgs::Marker>* markers) {
+  markers->push_back(*marker);
+  ++marker->id;
+  marker->points.clear();
+}
+
+}  // namespace
+
+MapBuilderBridge::MapBuilderBridge(
+    const NodeOptions& node_options,
+    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
+    tf2_ros::Buffer* const tf_buffer)
+    : node_options_(node_options),
+      map_builder_(std::move(map_builder)),
+      tf_buffer_(tf_buffer) {}
+
+void MapBuilderBridge::LoadState(const std::string& state_filename,
+                                 bool load_frozen_state) {
+  // Check if suffix of the state file is ".pbstream".
+  const std::string suffix = ".pbstream";
+  CHECK_EQ(state_filename.substr(
+               std::max<int>(state_filename.size() - suffix.size(), 0)),
+           suffix)
+      << "The file containing the state to be loaded must be a "
+         ".pbstream file.";
+  LOG(INFO) << "Loading saved state '" << state_filename << "'...";
+  cartographer::io::ProtoStreamReader stream(state_filename);
+  map_builder_->LoadState(&stream, load_frozen_state);
+}
+
+int MapBuilderBridge::AddTrajectory(const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids,
+    const TrajectoryOptions& trajectory_options) 
+{
+    ///添加新轨迹
+  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
+      expected_sensor_ids, trajectory_options.trajectory_builder_options,
+      [this](const int trajectory_id, const ::cartographer::common::Time time,
+             const Rigid3d local_pose,
+             ::cartographer::sensor::RangeData range_data_in_local,
+             const std::unique_ptr<
+                 const ::cartographer::mapping::TrajectoryBuilderInterface::
+                     InsertionResult>) {
+        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
+      });
+  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
+
+  // Make sure there is no trajectory with 'trajectory_id' yet.
+  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
+  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
+      trajectory_options.num_subdivisions_per_laser_scan,
+      trajectory_options.tracking_frame,
+      node_options_.lookup_transform_timeout_sec, tf_buffer_,
+      map_builder_->GetTrajectoryBuilder(trajectory_id));
+  auto emplace_result =
+      trajectory_options_.emplace(trajectory_id, trajectory_options);
+  CHECK(emplace_result.second == true);
+  return trajectory_id;
+}
+
+void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
+  LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
+
+  // Make sure there is a trajectory with 'trajectory_id'.
+  CHECK(GetTrajectoryStates().count(trajectory_id));
+  map_builder_->FinishTrajectory(trajectory_id);
+  sensor_bridges_.erase(trajectory_id);
+}
+
+void MapBuilderBridge::RunFinalOptimization() {
+  LOG(INFO) << "Running final trajectory optimization...";
+  map_builder_->pose_graph()->RunFinalOptimization();
+}
+
+bool MapBuilderBridge::SerializeState(const std::string& filename,
+                                      const bool include_unfinished_submaps) {
+  return map_builder_->SerializeStateToFile(include_unfinished_submaps,
+                                            filename);
+}
+
+void MapBuilderBridge::HandleSubmapQuery(
+    cartographer_ros_msgs::SubmapQuery::Request& request,
+    cartographer_ros_msgs::SubmapQuery::Response& response) {
+  cartographer::mapping::proto::SubmapQuery::Response response_proto;
+  cartographer::mapping::SubmapId submap_id{request.trajectory_id,
+                                            request.submap_index};
+  const std::string error =
+      map_builder_->SubmapToProto(submap_id, &response_proto);
+  if (!error.empty()) {
+    LOG(ERROR) << error;
+    response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
+    response.status.message = error;
+    return;
+  }
+
+  response.submap_version = response_proto.submap_version();
+  for (const auto& texture_proto : response_proto.textures()) {
+    response.textures.emplace_back();
+    auto& texture = response.textures.back();
+    texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(),
+                         texture_proto.cells().end());
+    texture.width = texture_proto.width();
+    texture.height = texture_proto.height();
+    texture.resolution = texture_proto.resolution();
+    texture.slice_pose = ToGeometryMsgPose(
+        cartographer::transform::ToRigid3(texture_proto.slice_pose()));
+  }
+  response.status.message = "Success.";
+  response.status.code = cartographer_ros_msgs::StatusCode::OK;
+}
+
+std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
+MapBuilderBridge::GetTrajectoryStates() {
+  auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates();
+  // Add active trajectories that are not yet in the pose graph, but are e.g.
+  // waiting for input sensor data and thus already have a sensor bridge.
+  for (const auto& sensor_bridge : sensor_bridges_) {
+    trajectory_states.insert(std::make_pair(
+        sensor_bridge.first,
+        ::cartographer::mapping::PoseGraph::TrajectoryState::ACTIVE));
+  }
+  return trajectory_states;
+}
+
+cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
+  cartographer_ros_msgs::SubmapList submap_list;
+  submap_list.header.stamp = ::ros::Time::now();
+  submap_list.header.frame_id = node_options_.map_frame;
+  for (const auto& submap_id_pose :
+       map_builder_->pose_graph()->GetAllSubmapPoses()) {
+    cartographer_ros_msgs::SubmapEntry submap_entry;
+    submap_entry.is_frozen = map_builder_->pose_graph()->IsTrajectoryFrozen(
+        submap_id_pose.id.trajectory_id);
+    submap_entry.trajectory_id = submap_id_pose.id.trajectory_id;
+    submap_entry.submap_index = submap_id_pose.id.submap_index;
+    submap_entry.submap_version = submap_id_pose.data.version;
+    submap_entry.pose = ToGeometryMsgPose(submap_id_pose.data.pose);
+    submap_list.submap.push_back(submap_entry);
+  }
+  return submap_list;
+}
+
+std::unordered_map<int, MapBuilderBridge::LocalTrajectoryData>
+MapBuilderBridge::GetLocalTrajectoryData() {
+  std::unordered_map<int, LocalTrajectoryData> local_trajectory_data;
+  for (const auto& entry : sensor_bridges_) {
+    const int trajectory_id = entry.first;
+    const SensorBridge& sensor_bridge = *entry.second;
+
+    std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data;
+    {
+      absl::MutexLock lock(&mutex_);
+      if (local_slam_data_.count(trajectory_id) == 0) {
+        continue;
+      }
+      local_slam_data = local_slam_data_.at(trajectory_id);
+    }
+
+    // Make sure there is a trajectory with 'trajectory_id'.
+    CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
+    local_trajectory_data[trajectory_id] = {
+        local_slam_data,
+        map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
+        sensor_bridge.tf_bridge().LookupToTracking(
+            local_slam_data->time,
+            trajectory_options_[trajectory_id].published_frame),
+        trajectory_options_[trajectory_id]};
+  }
+  return local_trajectory_data;
+}
+
+visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
+  visualization_msgs::MarkerArray trajectory_node_list;
+  const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
+  // Find the last node indices for each trajectory that have either
+  // inter-submap or inter-trajectory constraints.
+  std::map<int, int /* node_index */>
+      trajectory_to_last_inter_submap_constrained_node;
+  std::map<int, int /* node_index */>
+      trajectory_to_last_inter_trajectory_constrained_node;
+  for (const int trajectory_id : node_poses.trajectory_ids()) {
+    trajectory_to_last_inter_submap_constrained_node[trajectory_id] = 0;
+    trajectory_to_last_inter_trajectory_constrained_node[trajectory_id] = 0;
+  }
+  const auto constraints = map_builder_->pose_graph()->constraints();
+  for (const auto& constraint : constraints) {
+    if (constraint.tag ==
+        cartographer::mapping::PoseGraph::Constraint::INTER_SUBMAP) {
+      if (constraint.node_id.trajectory_id ==
+          constraint.submap_id.trajectory_id) {
+        trajectory_to_last_inter_submap_constrained_node[constraint.node_id
+                                                             .trajectory_id] =
+            std::max(trajectory_to_last_inter_submap_constrained_node.at(
+                         constraint.node_id.trajectory_id),
+                     constraint.node_id.node_index);
+      } else {
+        trajectory_to_last_inter_trajectory_constrained_node
+            [constraint.node_id.trajectory_id] =
+                std::max(trajectory_to_last_inter_submap_constrained_node.at(
+                             constraint.node_id.trajectory_id),
+                         constraint.node_id.node_index);
+      }
+    }
+  }
+
+  for (const int trajectory_id : node_poses.trajectory_ids()) {
+    visualization_msgs::Marker marker =
+        CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);
+    int last_inter_submap_constrained_node = std::max(
+        node_poses.trajectory(trajectory_id).begin()->id.node_index,
+        trajectory_to_last_inter_submap_constrained_node.at(trajectory_id));
+    int last_inter_trajectory_constrained_node = std::max(
+        node_poses.trajectory(trajectory_id).begin()->id.node_index,
+        trajectory_to_last_inter_trajectory_constrained_node.at(trajectory_id));
+    last_inter_submap_constrained_node =
+        std::max(last_inter_submap_constrained_node,
+                 last_inter_trajectory_constrained_node);
+
+    if (map_builder_->pose_graph()->IsTrajectoryFrozen(trajectory_id)) {
+      last_inter_submap_constrained_node =
+          (--node_poses.trajectory(trajectory_id).end())->id.node_index;
+      last_inter_trajectory_constrained_node =
+          last_inter_submap_constrained_node;
+    }
+
+    marker.color.a = 1.0;
+    for (const auto& node_id_data : node_poses.trajectory(trajectory_id)) {
+      if (!node_id_data.data.constant_pose_data.has_value()) {
+        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
+        continue;
+      }
+      const ::geometry_msgs::Point node_point =
+          ToGeometryMsgPoint(node_id_data.data.global_pose.translation());
+      marker.points.push_back(node_point);
+
+      if (node_id_data.id.node_index ==
+          last_inter_trajectory_constrained_node) {
+        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
+        marker.points.push_back(node_point);
+        marker.color.a = 0.5;
+      }
+      if (node_id_data.id.node_index == last_inter_submap_constrained_node) {
+        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
+        marker.points.push_back(node_point);
+        marker.color.a = 0.25;
+      }
+      // Work around the 16384 point limit in RViz by splitting the
+      // trajectory into multiple markers.
+      if (marker.points.size() == 16384) {
+        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
+        // Push back the last point, so the two markers appear connected.
+        marker.points.push_back(node_point);
+      }
+    }
+    PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
+    size_t current_last_marker_id = static_cast<size_t>(marker.id - 1);
+    if (trajectory_to_highest_marker_id_.count(trajectory_id) == 0) {
+      trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id;
+    } else {
+      marker.action = visualization_msgs::Marker::DELETE;
+      while (static_cast<size_t>(marker.id) <=
+             trajectory_to_highest_marker_id_[trajectory_id]) {
+        trajectory_node_list.markers.push_back(marker);
+        ++marker.id;
+      }
+      trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id;
+    }
+  }
+  return trajectory_node_list;
+}
+
+visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() {
+  visualization_msgs::MarkerArray landmark_poses_list;
+  const std::map<std::string, Rigid3d> landmark_poses =
+      map_builder_->pose_graph()->GetLandmarkPoses();
+  for (const auto& id_to_pose : landmark_poses) {
+    landmark_poses_list.markers.push_back(CreateLandmarkMarker(
+        GetLandmarkIndex(id_to_pose.first, &landmark_to_index_),
+        id_to_pose.second, node_options_.map_frame));
+  }
+  return landmark_poses_list;
+}
+
+visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
+  visualization_msgs::MarkerArray constraint_list;
+  int marker_id = 0;
+  visualization_msgs::Marker constraint_intra_marker;
+  constraint_intra_marker.id = marker_id++;
+  constraint_intra_marker.ns = "Intra constraints";
+  constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST;
+  constraint_intra_marker.header.stamp = ros::Time::now();
+  constraint_intra_marker.header.frame_id = node_options_.map_frame;
+  constraint_intra_marker.scale.x = kConstraintMarkerScale;
+  constraint_intra_marker.pose.orientation.w = 1.0;
+
+  visualization_msgs::Marker residual_intra_marker = constraint_intra_marker;
+  residual_intra_marker.id = marker_id++;
+  residual_intra_marker.ns = "Intra residuals";
+  // This and other markers which are less numerous are set to be slightly
+  // above the intra constraints marker in order to ensure that they are
+  // visible.
+  residual_intra_marker.pose.position.z = 0.1;
+
+  visualization_msgs::Marker constraint_inter_same_trajectory_marker =
+      constraint_intra_marker;
+  constraint_inter_same_trajectory_marker.id = marker_id++;
+  constraint_inter_same_trajectory_marker.ns =
+      "Inter constraints, same trajectory";
+  constraint_inter_same_trajectory_marker.pose.position.z = 0.1;
+
+  visualization_msgs::Marker residual_inter_same_trajectory_marker =
+      constraint_intra_marker;
+  residual_inter_same_trajectory_marker.id = marker_id++;
+  residual_inter_same_trajectory_marker.ns = "Inter residuals, same trajectory";
+  residual_inter_same_trajectory_marker.pose.position.z = 0.1;
+
+  visualization_msgs::Marker constraint_inter_diff_trajectory_marker =
+      constraint_intra_marker;
+  constraint_inter_diff_trajectory_marker.id = marker_id++;
+  constraint_inter_diff_trajectory_marker.ns =
+      "Inter constraints, different trajectories";
+  constraint_inter_diff_trajectory_marker.pose.position.z = 0.1;
+
+  visualization_msgs::Marker residual_inter_diff_trajectory_marker =
+      constraint_intra_marker;
+  residual_inter_diff_trajectory_marker.id = marker_id++;
+  residual_inter_diff_trajectory_marker.ns =
+      "Inter residuals, different trajectories";
+  residual_inter_diff_trajectory_marker.pose.position.z = 0.1;
+
+  const auto trajectory_node_poses =
+      map_builder_->pose_graph()->GetTrajectoryNodePoses();
+  const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
+  const auto constraints = map_builder_->pose_graph()->constraints();
+
+  for (const auto& constraint : constraints) {
+    visualization_msgs::Marker *constraint_marker, *residual_marker;
+    std_msgs::ColorRGBA color_constraint, color_residual;
+    if (constraint.tag ==
+        cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
+      constraint_marker = &constraint_intra_marker;
+      residual_marker = &residual_intra_marker;
+      // Color mapping for submaps of various trajectories - add trajectory id
+      // to ensure different starting colors. Also add a fixed offset of 25
+      // to avoid having identical colors as trajectories.
+      color_constraint = ToMessage(
+          cartographer::io::GetColor(constraint.submap_id.submap_index +
+                                     constraint.submap_id.trajectory_id + 25));
+      color_residual.a = 1.0;
+      color_residual.r = 1.0;
+    } else {
+      if (constraint.node_id.trajectory_id ==
+          constraint.submap_id.trajectory_id) {
+        constraint_marker = &constraint_inter_same_trajectory_marker;
+        residual_marker = &residual_inter_same_trajectory_marker;
+        // Bright yellow
+        color_constraint.a = 1.0;
+        color_constraint.r = color_constraint.g = 1.0;
+      } else {
+        constraint_marker = &constraint_inter_diff_trajectory_marker;
+        residual_marker = &residual_inter_diff_trajectory_marker;
+        // Bright orange
+        color_constraint.a = 1.0;
+        color_constraint.r = 1.0;
+        color_constraint.g = 165. / 255.;
+      }
+      // Bright cyan
+      color_residual.a = 1.0;
+      color_residual.b = color_residual.g = 1.0;
+    }
+
+    for (int i = 0; i < 2; ++i) {
+      constraint_marker->colors.push_back(color_constraint);
+      residual_marker->colors.push_back(color_residual);
+    }
+
+    const auto submap_it = submap_poses.find(constraint.submap_id);
+    if (submap_it == submap_poses.end()) {
+      continue;
+    }
+    const auto& submap_pose = submap_it->data.pose;
+    const auto node_it = trajectory_node_poses.find(constraint.node_id);
+    if (node_it == trajectory_node_poses.end()) {
+      continue;
+    }
+    const auto& trajectory_node_pose = node_it->data.global_pose;
+    const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;
+
+    constraint_marker->points.push_back(
+        ToGeometryMsgPoint(submap_pose.translation()));
+    constraint_marker->points.push_back(
+        ToGeometryMsgPoint(constraint_pose.translation()));
+
+    residual_marker->points.push_back(
+        ToGeometryMsgPoint(constraint_pose.translation()));
+    residual_marker->points.push_back(
+        ToGeometryMsgPoint(trajectory_node_pose.translation()));
+  }
+
+  constraint_list.markers.push_back(constraint_intra_marker);
+  constraint_list.markers.push_back(residual_intra_marker);
+  constraint_list.markers.push_back(constraint_inter_same_trajectory_marker);
+  constraint_list.markers.push_back(residual_inter_same_trajectory_marker);
+  constraint_list.markers.push_back(constraint_inter_diff_trajectory_marker);
+  constraint_list.markers.push_back(residual_inter_diff_trajectory_marker);
+  return constraint_list;
+}
+
+SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
+  return sensor_bridges_.at(trajectory_id).get();
+}
+
+void MapBuilderBridge::OnLocalSlamResult(
+    const int trajectory_id, const ::cartographer::common::Time time,
+    const Rigid3d local_pose,
+    ::cartographer::sensor::RangeData range_data_in_local) {
+  std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
+      std::make_shared<LocalTrajectoryData::LocalSlamData>(
+          LocalTrajectoryData::LocalSlamData{time, local_pose,
+                                             std::move(range_data_in_local)});
+  absl::MutexLock lock(&mutex_);
+  local_slam_data_[trajectory_id] = std::move(local_slam_data);
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -0,0 +1,124 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
+
+#include <memory>
+#include <set>
+#include <string>
+#include <unordered_map>
+
+#include "absl/synchronization/mutex.h"
+#include "cartographer/mapping/map_builder_interface.h"
+#include "cartographer/mapping/pose_graph_interface.h"
+#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+#include "cartographer/mapping/trajectory_builder_interface.h"
+#include "cartographer_ros/node_options.h"
+#include "cartographer_ros/sensor_bridge.h"
+#include "cartographer_ros/tf_bridge.h"
+#include "cartographer_ros/trajectory_options.h"
+#include "cartographer_ros_msgs/SubmapEntry.h"
+#include "cartographer_ros_msgs/SubmapList.h"
+#include "cartographer_ros_msgs/SubmapQuery.h"
+#include "nav_msgs/OccupancyGrid.h"
+
+// Abseil unfortunately pulls in winnt.h, which #defines DELETE.
+// Clean up to unbreak visualization_msgs::Marker::DELETE.
+#ifdef DELETE
+#undef DELETE
+#endif
+#include "visualization_msgs/MarkerArray.h"
+
+namespace cartographer_ros {
+
+class MapBuilderBridge {
+ public:
+  struct LocalTrajectoryData 
+  {
+    // Contains the trajectory data received from local SLAM, after
+    // it had processed accumulated 'range_data_in_local' and estimated
+    // current 'local_pose' at 'time'.
+    struct LocalSlamData
+    {
+      ::cartographer::common::Time time;
+      ::cartographer::transform::Rigid3d local_pose;
+      ::cartographer::sensor::RangeData range_data_in_local;
+    };
+    std::shared_ptr<const LocalSlamData> local_slam_data;
+    cartographer::transform::Rigid3d local_to_map;
+    std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
+    TrajectoryOptions trajectory_options;
+  };
+
+  MapBuilderBridge(
+      const NodeOptions& node_options,
+      std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
+      tf2_ros::Buffer* tf_buffer);
+
+  MapBuilderBridge(const MapBuilderBridge&) = delete;
+  MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
+
+  void LoadState(const std::string& state_filename, bool load_frozen_state);
+  int AddTrajectory(const std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids, 
+		    const TrajectoryOptions& trajectory_options);
+  void FinishTrajectory(int trajectory_id);
+  void RunFinalOptimization();
+  bool SerializeState(const std::string& filename,
+                      const bool include_unfinished_submaps);
+
+  void HandleSubmapQuery(
+      cartographer_ros_msgs::SubmapQuery::Request& request,
+      cartographer_ros_msgs::SubmapQuery::Response& response);
+
+  std::map<int /* trajectory_id */,
+           ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
+  GetTrajectoryStates();
+  cartographer_ros_msgs::SubmapList GetSubmapList();
+  std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
+      LOCKS_EXCLUDED(mutex_);
+  visualization_msgs::MarkerArray GetTrajectoryNodeList();
+  visualization_msgs::MarkerArray GetLandmarkPosesList();
+  visualization_msgs::MarkerArray GetConstraintList();
+
+  SensorBridge* sensor_bridge(int trajectory_id);
+
+ private:
+  void OnLocalSlamResult(const int trajectory_id,
+                         const ::cartographer::common::Time time,
+                         const ::cartographer::transform::Rigid3d local_pose,
+                         ::cartographer::sensor::RangeData range_data_in_local)
+      LOCKS_EXCLUDED(mutex_);
+
+  absl::Mutex mutex_;
+  const NodeOptions node_options_;
+  std::unordered_map<int,
+                     std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
+      local_slam_data_ GUARDED_BY(mutex_);
+  std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
+  tf2_ros::Buffer* const tf_buffer_;
+
+  std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
+
+  // These are keyed with 'trajectory_id'.
+  std::unordered_map<int, TrajectoryOptions> trajectory_options_;
+  std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
+  std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/family_factory.cc

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2018 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 "cartographer_ros/metrics/family_factory.h"
+
+#include "absl/memory/memory.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
+
+::cartographer::metrics::Family<::cartographer::metrics::Counter>*
+FamilyFactory::NewCounterFamily(const std::string& name,
+                                const std::string& description) {
+  auto wrapper = absl::make_unique<CounterFamily>(name, description);
+  auto* ptr = wrapper.get();
+  counter_families_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+::cartographer::metrics::Family<::cartographer::metrics::Gauge>*
+FamilyFactory::NewGaugeFamily(const std::string& name,
+                              const std::string& description) {
+  auto wrapper = absl::make_unique<GaugeFamily>(name, description);
+  auto* ptr = wrapper.get();
+  gauge_families_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+::cartographer::metrics::Family<::cartographer::metrics::Histogram>*
+FamilyFactory::NewHistogramFamily(const std::string& name,
+                                  const std::string& description,
+                                  const BucketBoundaries& boundaries) {
+  auto wrapper =
+      absl::make_unique<HistogramFamily>(name, description, boundaries);
+  auto* ptr = wrapper.get();
+  histogram_families_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+void FamilyFactory::ReadMetrics(
+    ::cartographer_ros_msgs::ReadMetrics::Response* response) const {
+  for (const auto& counter_family : counter_families_) {
+    response->metric_families.push_back(counter_family->ToRosMessage());
+  }
+  for (const auto& gauge_family : gauge_families_) {
+    response->metric_families.push_back(gauge_family->ToRosMessage());
+  }
+  for (const auto& histogram_family : histogram_families_) {
+    response->metric_families.push_back(histogram_family->ToRosMessage());
+  }
+}
+
+}  // namespace metrics
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/metrics/family_factory.h

@@ -0,0 +1,61 @@
+/*
+ * Copyright 2018 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H
+#define CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H
+
+#include <memory>
+#include <string>
+
+#include "cartographer/metrics/family_factory.h"
+#include "cartographer_ros/metrics/internal/counter.h"
+#include "cartographer_ros/metrics/internal/family.h"
+#include "cartographer_ros/metrics/internal/gauge.h"
+#include "cartographer_ros/metrics/internal/histogram.h"
+#include "cartographer_ros_msgs/ReadMetrics.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+// Realizes the factory / registry interface for the metrics in libcartographer
+// and provides a wrapper to collect ROS messages from the metrics it owns.
+class FamilyFactory : public ::cartographer::metrics::FamilyFactory {
+ public:
+  ::cartographer::metrics::Family<::cartographer::metrics::Counter>*
+
+  NewCounterFamily(const std::string& name,
+                   const std::string& description) override;
+  ::cartographer::metrics::Family<::cartographer::metrics::Gauge>*
+  NewGaugeFamily(const std::string& name,
+                 const std::string& description) override;
+  ::cartographer::metrics::Family<::cartographer::metrics::Histogram>*
+  NewHistogramFamily(const std::string& name, const std::string& description,
+                     const ::cartographer::metrics::Histogram::BucketBoundaries&
+                         boundaries) override;
+
+  void ReadMetrics(
+      ::cartographer_ros_msgs::ReadMetrics::Response* response) const;
+
+ private:
+  std::vector<std::unique_ptr<CounterFamily>> counter_families_;
+  std::vector<std::unique_ptr<GaugeFamily>> gauge_families_;
+  std::vector<std::unique_ptr<HistogramFamily>> histogram_families_;
+};
+
+}  // namespace metrics
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/counter.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright 2018 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H
+#define CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H
+
+#include "cartographer/metrics/counter.h"
+#include "cartographer_ros/metrics/internal/gauge.h"
+#include "cartographer_ros_msgs/Metric.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+class Counter : public ::cartographer::metrics::Counter {
+ public:
+  explicit Counter(const std::map<std::string, std::string>& labels)
+      : gauge_(labels) {}
+
+  void Increment(const double value) override { gauge_.Increment(value); }
+
+  void Increment() override { gauge_.Increment(); }
+
+  double Value() { return gauge_.Value(); }
+
+  cartographer_ros_msgs::Metric ToRosMessage() {
+    cartographer_ros_msgs::Metric msg = gauge_.ToRosMessage();
+    msg.type = cartographer_ros_msgs::Metric::TYPE_COUNTER;
+    return msg;
+  }
+
+ private:
+  Gauge gauge_;
+};
+
+}  // namespace metrics
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/family.cc

@@ -0,0 +1,82 @@
+/*
+ * Copyright 2018 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 "cartographer_ros/metrics/internal/family.h"
+
+#include "absl/memory/memory.h"
+#include "cartographer_ros/metrics/internal/counter.h"
+#include "cartographer_ros/metrics/internal/gauge.h"
+#include "cartographer_ros/metrics/internal/histogram.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
+
+Counter* CounterFamily::Add(const std::map<std::string, std::string>& labels) {
+  auto wrapper = absl::make_unique<Counter>(labels);
+  auto* ptr = wrapper.get();
+  wrappers_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+cartographer_ros_msgs::MetricFamily CounterFamily::ToRosMessage() {
+  cartographer_ros_msgs::MetricFamily family_msg;
+  family_msg.name = name_;
+  family_msg.description = description_;
+  for (const auto& wrapper : wrappers_) {
+    family_msg.metrics.push_back(wrapper->ToRosMessage());
+  }
+  return family_msg;
+}
+
+Gauge* GaugeFamily::Add(const std::map<std::string, std::string>& labels) {
+  auto wrapper = absl::make_unique<Gauge>(labels);
+  auto* ptr = wrapper.get();
+  wrappers_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+cartographer_ros_msgs::MetricFamily GaugeFamily::ToRosMessage() {
+  cartographer_ros_msgs::MetricFamily family_msg;
+  family_msg.name = name_;
+  family_msg.description = description_;
+  for (const auto& wrapper : wrappers_) {
+    family_msg.metrics.push_back(wrapper->ToRosMessage());
+  }
+  return family_msg;
+}
+
+Histogram* HistogramFamily::Add(
+    const std::map<std::string, std::string>& labels) {
+  auto wrapper = absl::make_unique<Histogram>(labels, boundaries_);
+  auto* ptr = wrapper.get();
+  wrappers_.emplace_back(std::move(wrapper));
+  return ptr;
+}
+
+cartographer_ros_msgs::MetricFamily HistogramFamily::ToRosMessage() {
+  cartographer_ros_msgs::MetricFamily family_msg;
+  family_msg.name = name_;
+  family_msg.description = description_;
+  for (const auto& wrapper : wrappers_) {
+    family_msg.metrics.push_back(wrapper->ToRosMessage());
+  }
+  return family_msg;
+}
+
+}  // namespace metrics
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/family.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright 2018 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H
+#define CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H
+
+#include <memory>
+#include <string>
+
+#include "cartographer/metrics/family_factory.h"
+#include "cartographer_ros/metrics/internal/counter.h"
+#include "cartographer_ros/metrics/internal/gauge.h"
+#include "cartographer_ros/metrics/internal/histogram.h"
+#include "cartographer_ros_msgs/MetricFamily.h"
+
+namespace cartographer_ros {
+namespace metrics {
+class CounterFamily
+    : public ::cartographer::metrics::Family<::cartographer::metrics::Counter> {
+ public:
+  CounterFamily(const std::string& name, const std::string& description)
+      : name_(name), description_(description) {}
+  Counter* Add(const std::map<std::string, std::string>& labels) override;
+  cartographer_ros_msgs::MetricFamily ToRosMessage();
+
+ private:
+  std::string name_;
+  std::string description_;
+  std::vector<std::unique_ptr<Counter>> wrappers_;
+};
+
+class GaugeFamily
+    : public ::cartographer::metrics::Family<::cartographer::metrics::Gauge> {
+ public:
+  GaugeFamily(const std::string& name, const std::string& description)
+      : name_(name), description_(description) {}
+  Gauge* Add(const std::map<std::string, std::string>& labels) override;
+
+  cartographer_ros_msgs::MetricFamily ToRosMessage();
+
+ private:
+  std::string name_;
+  std::string description_;
+  std::vector<std::unique_ptr<Gauge>> wrappers_;
+};
+
+class HistogramFamily : public ::cartographer::metrics::Family<
+                            ::cartographer::metrics::Histogram> {
+ public:
+  HistogramFamily(const std::string& name, const std::string& description,
+                  const BucketBoundaries& boundaries)
+      : name_(name), description_(description), boundaries_(boundaries) {}
+
+  Histogram* Add(const std::map<std::string, std::string>& labels) override;
+
+  cartographer_ros_msgs::MetricFamily ToRosMessage();
+
+ private:
+  std::string name_;
+  std::string description_;
+  std::vector<std::unique_ptr<Histogram>> wrappers_;
+  const BucketBoundaries boundaries_;
+};
+
+}  // namespace metrics
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/gauge.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright 2018 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H
+#define CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H
+
+#include <map>
+#include <string>
+
+#include "absl/synchronization/mutex.h"
+#include "cartographer/metrics/gauge.h"
+#include "cartographer_ros_msgs/Metric.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+class Gauge : public ::cartographer::metrics::Gauge {
+ public:
+  explicit Gauge(const std::map<std::string, std::string>& labels)
+      : labels_(labels), value_(0.) {}
+
+  void Decrement(const double value) override { Add(-1. * value); }
+
+  void Decrement() override { Decrement(1.); }
+
+  void Increment(const double value) override { Add(value); }
+
+  void Increment() override { Increment(1.); }
+
+  void Set(double value) override {
+    absl::MutexLock lock(&mutex_);
+    value_ = value;
+  }
+
+  double Value() {
+    absl::MutexLock lock(&mutex_);
+    return value_;
+  }
+
+  cartographer_ros_msgs::Metric ToRosMessage() {
+    cartographer_ros_msgs::Metric msg;
+    msg.type = cartographer_ros_msgs::Metric::TYPE_GAUGE;
+    for (const auto& label : labels_) {
+      cartographer_ros_msgs::MetricLabel label_msg;
+      label_msg.key = label.first;
+      label_msg.value = label.second;
+      msg.labels.push_back(label_msg);
+    }
+    msg.value = Value();
+    return msg;
+  }
+
+ private:
+  void Add(const double value) {
+    absl::MutexLock lock(&mutex_);
+    value_ += value;
+  }
+
+  absl::Mutex mutex_;
+  const std::map<std::string, std::string> labels_;
+  double value_ GUARDED_BY(mutex_);
+};
+
+}  // namespace metrics
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc

@@ -0,0 +1,90 @@
+/*
+ * Copyright 2018 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 "cartographer_ros/metrics/internal/histogram.h"
+
+#include <algorithm>
+#include <numeric>
+
+#include "glog/logging.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
+
+Histogram::Histogram(const std::map<std::string, std::string>& labels,
+                     const BucketBoundaries& bucket_boundaries)
+    : labels_(labels),
+      bucket_boundaries_(bucket_boundaries),
+      bucket_counts_(bucket_boundaries.size() + 1) {
+  absl::MutexLock lock(&mutex_);
+  CHECK(std::is_sorted(std::begin(bucket_boundaries_),
+                       std::end(bucket_boundaries_)));
+}
+
+void Histogram::Observe(double value) {
+  auto bucket_index =
+      std::distance(bucket_boundaries_.begin(),
+                    std::upper_bound(bucket_boundaries_.begin(),
+                                     bucket_boundaries_.end(), value));
+  absl::MutexLock lock(&mutex_);
+  sum_ += value;
+  bucket_counts_[bucket_index] += 1;
+}
+
+std::map<double, double> Histogram::CountsByBucket() {
+  absl::MutexLock lock(&mutex_);
+  std::map<double, double> counts_by_bucket;
+  // Add the finite buckets.
+  for (size_t i = 0; i < bucket_boundaries_.size(); ++i) {
+    counts_by_bucket[bucket_boundaries_.at(i)] = bucket_counts_.at(i);
+  }
+  // Add the "infinite" bucket.
+  counts_by_bucket[kInfiniteBoundary] = bucket_counts_.back();
+  return counts_by_bucket;
+}
+
+double Histogram::Sum() {
+  absl::MutexLock lock(&mutex_);
+  return sum_;
+}
+
+double Histogram::CumulativeCount() {
+  absl::MutexLock lock(&mutex_);
+  return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.);
+}
+
+cartographer_ros_msgs::Metric Histogram::ToRosMessage() {
+  cartographer_ros_msgs::Metric msg;
+  msg.type = cartographer_ros_msgs::Metric::TYPE_HISTOGRAM;
+  for (const auto& label : labels_) {
+    cartographer_ros_msgs::MetricLabel label_msg;
+    label_msg.key = label.first;
+    label_msg.value = label.second;
+    msg.labels.push_back(label_msg);
+  }
+  for (const auto& bucket : CountsByBucket()) {
+    cartographer_ros_msgs::HistogramBucket bucket_msg;
+    bucket_msg.bucket_boundary = bucket.first;
+    bucket_msg.count = bucket.second;
+    msg.counts_by_bucket.push_back(bucket_msg);
+  }
+  return msg;
+}
+
+}  // namespace metrics
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/histogram.h

@@ -0,0 +1,60 @@
+/*
+ * Copyright 2018 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H
+#define CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H
+
+#include <map>
+#include <vector>
+
+#include "absl/synchronization/mutex.h"
+#include "cartographer/metrics/histogram.h"
+#include "cartographer_ros_msgs/Metric.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+constexpr double kInfiniteBoundary = std::numeric_limits<double>::infinity();
+
+using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
+
+class Histogram : public ::cartographer::metrics::Histogram {
+ public:
+  explicit Histogram(const std::map<std::string, std::string>& labels,
+                     const BucketBoundaries& bucket_boundaries);
+
+  void Observe(double value) override;
+
+  std::map<double, double> CountsByBucket();
+
+  double Sum();
+
+  double CumulativeCount();
+
+  cartographer_ros_msgs::Metric ToRosMessage();
+
+ private:
+  absl::Mutex mutex_;
+  const std::map<std::string, std::string> labels_;
+  const BucketBoundaries bucket_boundaries_;
+  std::vector<double> bucket_counts_ GUARDED_BY(mutex_);
+  double sum_ GUARDED_BY(mutex_);
+};
+
+}  // namespace metrics
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc

@@ -0,0 +1,104 @@
+/*
+ * Copyright 2018 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 <algorithm>
+#include <array>
+#include <numeric>
+
+#include "cartographer/metrics/histogram.h"
+#include "cartographer_ros/metrics/internal/counter.h"
+#include "cartographer_ros/metrics/internal/gauge.h"
+#include "cartographer_ros/metrics/internal/histogram.h"
+#include "gtest/gtest.h"
+
+namespace cartographer_ros {
+namespace metrics {
+
+TEST(Metrics, GaugeTest) {
+  Gauge gauge({});
+  EXPECT_EQ(gauge.Value(), 0.);
+  gauge.Increment(1.2);
+  EXPECT_EQ(gauge.Value(), 1.2);
+  gauge.Increment();
+  EXPECT_EQ(gauge.Value(), 2.2);
+  gauge.Decrement(2.2);
+  EXPECT_EQ(gauge.Value(), 0.);
+  gauge.Decrement();
+  EXPECT_EQ(gauge.Value(), -1.);
+}
+
+TEST(Metrics, CounterTest) {
+  Counter counter({});
+  EXPECT_EQ(counter.Value(), 0.);
+  counter.Increment(1.2);
+  EXPECT_EQ(counter.Value(), 1.2);
+  counter.Increment(0.8);
+  EXPECT_EQ(counter.Value(), 2.);
+  counter.Increment();
+  EXPECT_EQ(counter.Value(), 3.);
+}
+
+TEST(Metrics, HistogramFixedWidthTest) {
+  auto boundaries = ::cartographer::metrics::Histogram::FixedWidth(1, 3);
+  Histogram histogram({}, boundaries);
+
+  // Observe some values that fit in finite buckets.
+  std::array<double, 3> values = {{0., 2, 2.5}};
+  for (const auto& value : values) {
+    histogram.Observe(value);
+  }
+  //     1     2     3    inf
+  //  1  |  0  |  2  |  0  |
+  EXPECT_EQ(histogram.CountsByBucket()[1], 1);
+  EXPECT_EQ(histogram.CountsByBucket()[2], 0);
+  EXPECT_EQ(histogram.CountsByBucket()[3], 2);
+  EXPECT_EQ(histogram.CumulativeCount(), values.size());
+  EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.));
+
+  // Values above the last bucket boundary should go to the "infinite" bucket.
+  histogram.Observe(3.5);
+  //     1     2     3    inf
+  //  1  |  0  |  2  |  1  |
+  EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1);
+}
+
+TEST(Metrics, HistogramScaledPowersOfTest) {
+  auto boundaries =
+      ::cartographer::metrics::Histogram::ScaledPowersOf(2, 1, 2048);
+  Histogram histogram({}, boundaries);
+
+  // Observe some values that fit in finite buckets.
+  std::array<double, 3> values = {{256, 512, 666}};
+  for (const auto& value : values) {
+    histogram.Observe(value);
+  }
+  // ... 256   512   1024  inf
+  // ...  |  1  |  2  |  0  |
+  EXPECT_EQ(histogram.CountsByBucket()[256], 0);
+  EXPECT_EQ(histogram.CountsByBucket()[512], 1);
+  EXPECT_EQ(histogram.CountsByBucket()[1024], 2);
+  EXPECT_EQ(histogram.CumulativeCount(), values.size());
+  EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.));
+
+  // Values above the last bucket boundary should go to the "infinite" bucket.
+  histogram.Observe(2048);
+  // ... 256   512   1024  inf
+  // ...  |  1  |  2  |  1  |
+  EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1);
+}
+
+}  // namespace metrics
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -0,0 +1,420 @@
+/*
+ * 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 "cartographer_ros/msg_conversion.h"
+
+#include <cmath>
+
+#include "cartographer/common/math.h"
+#include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+#include "cartographer/io/submap_painter.h"
+#include "cartographer/transform/proto/transform.pb.h"
+#include "cartographer/transform/transform.h"
+#include "cartographer_ros/time_conversion.h"
+#include "geometry_msgs/Pose.h"
+#include "geometry_msgs/Quaternion.h"
+#include "geometry_msgs/Transform.h"
+#include "geometry_msgs/TransformStamped.h"
+#include "geometry_msgs/Vector3.h"
+#include "glog/logging.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+#include "pcl_conversions/pcl_conversions.h"
+#include "ros/ros.h"
+#include "ros/serialization.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/PointCloud2.h"
+
+namespace {
+
+// Sizes of PCL point types have to be 4n floats for alignment, as described in
+// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php
+struct PointXYZT {
+  float x;
+  float y;
+  float z;
+  float time;
+};
+
+struct PointXYZIT {
+  PCL_ADD_POINT4D;
+  float intensity;
+  float time;
+  float unused_padding[2];
+};
+
+}  // namespace
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time))
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    PointXYZIT,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity,
+                                            intensity)(float, time, time))
+
+namespace cartographer_ros {
+namespace {
+
+// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each
+// point. The last one must be this value or RViz is not showing the point cloud
+// properly.
+constexpr float kPointCloudComponentFourMagic = 1.;
+
+using ::cartographer::sensor::LandmarkData;
+using ::cartographer::sensor::LandmarkObservation;
+using ::cartographer::sensor::PointCloudWithIntensities;
+using ::cartographer::transform::Rigid3d;
+using ::cartographer_ros_msgs::LandmarkEntry;
+using ::cartographer_ros_msgs::LandmarkList;
+
+sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp,
+                                                   const std::string& frame_id,
+                                                   const int num_points) {
+  sensor_msgs::PointCloud2 msg;
+  msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
+  msg.header.frame_id = frame_id;
+  msg.height = 1;
+  msg.width = num_points;
+  msg.fields.resize(3);
+  msg.fields[0].name = "x";
+  msg.fields[0].offset = 0;
+  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
+  msg.fields[0].count = 1;
+  msg.fields[1].name = "y";
+  msg.fields[1].offset = 4;
+  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
+  msg.fields[1].count = 1;
+  msg.fields[2].name = "z";
+  msg.fields[2].offset = 8;
+  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
+  msg.fields[2].count = 1;
+  msg.is_bigendian = false;
+  msg.point_step = 16;
+  msg.row_step = 16 * msg.width;
+  msg.is_dense = true;
+  msg.data.resize(16 * num_points);
+  return msg;
+}
+
+// For sensor_msgs::LaserScan.
+bool HasEcho(float) { return true; }
+
+float GetFirstEcho(float range) { return range; }
+
+// For sensor_msgs::MultiEchoLaserScan.
+bool HasEcho(const sensor_msgs::LaserEcho& echo) {
+  return !echo.echoes.empty();
+}
+
+float GetFirstEcho(const sensor_msgs::LaserEcho& echo) {
+  return echo.echoes[0];
+}
+
+// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
+template <typename LaserMessageType>
+std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
+LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
+  CHECK_GE(msg.range_min, 0.f);
+  CHECK_GE(msg.range_max, msg.range_min);
+  if (msg.angle_increment > 0.f) {
+    CHECK_GT(msg.angle_max, msg.angle_min);
+  } else {
+    CHECK_GT(msg.angle_min, msg.angle_max);
+  }
+  PointCloudWithIntensities point_cloud;
+  float angle = msg.angle_min;
+  for (size_t i = 0; i < msg.ranges.size(); ++i) {
+    const auto& echoes = msg.ranges[i];
+    if (HasEcho(echoes)) {
+      const float first_echo = GetFirstEcho(echoes);
+      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
+        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
+        const cartographer::sensor::TimedRangefinderPoint point{
+            rotation * (first_echo * Eigen::Vector3f::UnitX()),
+            i * msg.time_increment};
+        point_cloud.points.push_back(point);
+        if (msg.intensities.size() > 0) {
+          CHECK_EQ(msg.intensities.size(), msg.ranges.size());
+          const auto& echo_intensities = msg.intensities[i];
+          CHECK(HasEcho(echo_intensities));
+          point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
+        } else {
+          point_cloud.intensities.push_back(0.f);
+        }
+      }
+    }
+    angle += msg.angle_increment;
+  }
+  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
+  if (!point_cloud.points.empty()) {
+    const double duration = point_cloud.points.back().time;
+    timestamp += cartographer::common::FromSeconds(duration);
+    for (auto& point : point_cloud.points) {
+      point.time -= duration;
+    }
+  }
+  return std::make_tuple(point_cloud, timestamp);
+}
+
+bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
+                         const std::string& field_name) {
+  for (const auto& field : pc2.fields) {
+    if (field.name == field_name) {
+      return true;
+    }
+  }
+  return false;
+}
+
+}  // namespace
+
+sensor_msgs::PointCloud2 ToPointCloud2Message(
+    const int64_t timestamp, const std::string& frame_id,
+    const ::cartographer::sensor::TimedPointCloud& point_cloud) {
+  auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
+  ::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
+  for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) {
+    stream.next(point.position.x());
+    stream.next(point.position.y());
+    stream.next(point.position.z());
+    stream.next(kPointCloudComponentFourMagic);
+  }
+  return msg;
+}
+
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
+  return LaserScanToPointCloudWithIntensities(msg);
+}
+
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
+  return LaserScanToPointCloudWithIntensities(msg);
+}
+
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
+  PointCloudWithIntensities point_cloud;
+  // We check for intensity field here to avoid run-time warnings if we pass in
+  // a PointCloud2 without intensity.
+  if (PointCloud2HasField(msg, "intensity")) {
+    if (PointCloud2HasField(msg, "time")) {
+      pcl::PointCloud<PointXYZIT> pcl_point_cloud;
+      pcl::fromROSMsg(msg, pcl_point_cloud);
+      point_cloud.points.reserve(pcl_point_cloud.size());
+      point_cloud.intensities.reserve(pcl_point_cloud.size());
+      for (const auto& point : pcl_point_cloud) {
+        point_cloud.points.push_back(
+            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
+        point_cloud.intensities.push_back(point.intensity);
+      }
+    } else {
+      pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
+      pcl::fromROSMsg(msg, pcl_point_cloud);
+      point_cloud.points.reserve(pcl_point_cloud.size());
+      point_cloud.intensities.reserve(pcl_point_cloud.size());
+      for (const auto& point : pcl_point_cloud) {
+        point_cloud.points.push_back(
+            {Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
+        point_cloud.intensities.push_back(point.intensity);
+      }
+    }
+  } else {
+    // If we don't have an intensity field, just copy XYZ and fill in 1.0f.
+    if (PointCloud2HasField(msg, "time")) {
+      pcl::PointCloud<PointXYZT> pcl_point_cloud;
+      pcl::fromROSMsg(msg, pcl_point_cloud);
+      point_cloud.points.reserve(pcl_point_cloud.size());
+      point_cloud.intensities.reserve(pcl_point_cloud.size());
+      for (const auto& point : pcl_point_cloud) {
+        point_cloud.points.push_back(
+            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
+        point_cloud.intensities.push_back(1.0f);
+      }
+    } else {
+      pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
+      pcl::fromROSMsg(msg, pcl_point_cloud);
+      point_cloud.points.reserve(pcl_point_cloud.size());
+      point_cloud.intensities.reserve(pcl_point_cloud.size());
+      for (const auto& point : pcl_point_cloud) {
+        point_cloud.points.push_back(
+            {Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
+        point_cloud.intensities.push_back(1.0f);
+      }
+    }
+  }
+  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
+  if (!point_cloud.points.empty()) {
+    const double duration = point_cloud.points.back().time;
+    timestamp += cartographer::common::FromSeconds(duration);
+    for (auto& point : point_cloud.points) {
+      point.time -= duration;
+      CHECK_LE(point.time, 0.f)
+          << "Encountered a point with a larger stamp than "
+             "the last point in the cloud.";
+    }
+  }
+  return std::make_tuple(point_cloud, timestamp);
+}
+
+LandmarkData ToLandmarkData(const LandmarkList& landmark_list) {
+  LandmarkData landmark_data;
+  landmark_data.time = FromRos(landmark_list.header.stamp);
+  for (const LandmarkEntry& entry : landmark_list.landmarks) {
+    landmark_data.landmark_observations.push_back(
+        {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
+         entry.translation_weight, entry.rotation_weight});
+  }
+  return landmark_data;
+}
+
+Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) {
+  return Rigid3d(ToEigen(transform.transform.translation),
+                 ToEigen(transform.transform.rotation));
+}
+
+Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) {
+  return Rigid3d({pose.position.x, pose.position.y, pose.position.z},
+                 ToEigen(pose.orientation));
+}
+
+Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3) {
+  return Eigen::Vector3d(vector3.x, vector3.y, vector3.z);
+}
+
+Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion) {
+  return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y,
+                            quaternion.z);
+}
+
+geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) {
+  geometry_msgs::Transform transform;
+  transform.translation.x = rigid3d.translation().x();
+  transform.translation.y = rigid3d.translation().y();
+  transform.translation.z = rigid3d.translation().z();
+  transform.rotation.w = rigid3d.rotation().w();
+  transform.rotation.x = rigid3d.rotation().x();
+  transform.rotation.y = rigid3d.rotation().y();
+  transform.rotation.z = rigid3d.rotation().z();
+  return transform;
+}
+
+geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) {
+  geometry_msgs::Pose pose;
+  pose.position = ToGeometryMsgPoint(rigid3d.translation());
+  pose.orientation.w = rigid3d.rotation().w();
+  pose.orientation.x = rigid3d.rotation().x();
+  pose.orientation.y = rigid3d.rotation().y();
+  pose.orientation.z = rigid3d.rotation().z();
+  return pose;
+}
+
+geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) {
+  geometry_msgs::Point point;
+  point.x = vector3d.x();
+  point.y = vector3d.y();
+  point.z = vector3d.z();
+  return point;
+}
+
+Eigen::Vector3d LatLongAltToEcef(const double latitude, const double longitude,
+                                 const double altitude) {
+  // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
+  constexpr double a = 6378137.;  // semi-major axis, equator to center.
+  constexpr double f = 1. / 298.257223563;
+  constexpr double b = a * (1. - f);  // semi-minor axis, pole to center.
+  constexpr double a_squared = a * a;
+  constexpr double b_squared = b * b;
+  constexpr double e_squared = (a_squared - b_squared) / a_squared;
+  const double sin_phi = std::sin(cartographer::common::DegToRad(latitude));
+  const double cos_phi = std::cos(cartographer::common::DegToRad(latitude));
+  const double sin_lambda = std::sin(cartographer::common::DegToRad(longitude));
+  const double cos_lambda = std::cos(cartographer::common::DegToRad(longitude));
+  const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi);
+  const double x = (N + altitude) * cos_phi * cos_lambda;
+  const double y = (N + altitude) * cos_phi * sin_lambda;
+  const double z = (b_squared / a_squared * N + altitude) * sin_phi;
+
+  return Eigen::Vector3d(x, y, z);
+}
+
+cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(
+    const double latitude, const double longitude) {
+  const Eigen::Vector3d translation = LatLongAltToEcef(latitude, longitude, 0.);
+  const Eigen::Quaterniond rotation =
+      Eigen::AngleAxisd(cartographer::common::DegToRad(latitude - 90.),
+                        Eigen::Vector3d::UnitY()) *
+      Eigen::AngleAxisd(cartographer::common::DegToRad(-longitude),
+                        Eigen::Vector3d::UnitZ());
+  return cartographer::transform::Rigid3d(rotation * -translation, rotation);
+}
+
+std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
+    const cartographer::io::PaintSubmapSlicesResult& painted_slices,
+    const double resolution, const std::string& frame_id,
+    const ros::Time& time) {
+  auto occupancy_grid = absl::make_unique<nav_msgs::OccupancyGrid>();
+
+  const int width = cairo_image_surface_get_width(painted_slices.surface.get());
+  const int height =
+      cairo_image_surface_get_height(painted_slices.surface.get());
+
+  occupancy_grid->header.stamp = time;
+  occupancy_grid->header.frame_id = frame_id;
+  occupancy_grid->info.map_load_time = time;
+  occupancy_grid->info.resolution = resolution;
+  occupancy_grid->info.width = width;
+  occupancy_grid->info.height = height;
+  occupancy_grid->info.origin.position.x =
+      -painted_slices.origin.x() * resolution;
+  occupancy_grid->info.origin.position.y =
+      (-height + painted_slices.origin.y()) * resolution;
+  occupancy_grid->info.origin.position.z = 0.;
+  occupancy_grid->info.origin.orientation.w = 1.;
+  occupancy_grid->info.origin.orientation.x = 0.;
+  occupancy_grid->info.origin.orientation.y = 0.;
+  occupancy_grid->info.origin.orientation.z = 0.;
+
+  const uint32_t* pixel_data = reinterpret_cast<uint32_t*>(
+      cairo_image_surface_get_data(painted_slices.surface.get()));
+  occupancy_grid->data.reserve(width * height);
+  for (int y = height - 1; y >= 0; --y) {
+    for (int x = 0; x < width; ++x) {
+      const uint32_t packed = pixel_data[y * width + x];
+      const unsigned char color = packed >> 16;
+      const unsigned char observed = packed >> 8;
+      const int value =
+          observed == 0
+              ? -1
+              : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
+      CHECK_LE(-1, value);
+      CHECK_GE(100, value);
+      occupancy_grid->data.push_back(value);
+    }
+  }
+
+  return occupancy_grid;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.h

@@ -0,0 +1,94 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
+
+#include "cartographer/common/time.h"
+#include "cartographer/io/submap_painter.h"
+#include "cartographer/sensor/landmark_data.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer_ros_msgs/LandmarkList.h"
+#include "geometry_msgs/Pose.h"
+#include "geometry_msgs/Transform.h"
+#include "geometry_msgs/TransformStamped.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/PointCloud2.h"
+
+namespace cartographer_ros {
+
+sensor_msgs::PointCloud2 ToPointCloud2Message(
+    int64_t timestamp, const std::string& frame_id,
+    const ::cartographer::sensor::TimedPointCloud& point_cloud);
+
+geometry_msgs::Transform ToGeometryMsgTransform(
+    const ::cartographer::transform::Rigid3d& rigid3d);
+
+geometry_msgs::Pose ToGeometryMsgPose(
+    const ::cartographer::transform::Rigid3d& rigid3d);
+
+geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d);
+
+// Converts ROS message to point cloud. Returns the time when the last point
+// was acquired (different from the ROS timestamp). Timing of points is given in
+// the fourth component of each point relative to `Time`.
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg);
+
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg);
+
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg);
+
+::cartographer::sensor::LandmarkData ToLandmarkData(
+    const cartographer_ros_msgs::LandmarkList& landmark_list);
+
+::cartographer::transform::Rigid3d ToRigid3d(
+    const geometry_msgs::TransformStamped& transform);
+
+::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose);
+
+Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3);
+
+Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion);
+
+// Converts from WGS84 (latitude, longitude, altitude) to ECEF.
+Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude,
+                                 double altitude);
+
+// Returns a transform that takes ECEF coordinates from nearby points to a local
+// frame that has z pointing upwards.
+cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude,
+                                                              double longitude);
+
+// Points to an occupancy grid message at a specific resolution from painted
+// submap slices obtained via ::cartographer::io::PaintSubmapSlices(...).
+std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
+    const cartographer::io::PaintSubmapSlicesResult& painted_slices,
+    const double resolution, const std::string& frame_id,
+    const ros::Time& time);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H

cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion_test.cc

@@ -0,0 +1,212 @@
+/*
+ * 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 <cmath>
+#include <random>
+
+#include "cartographer/transform/rigid_transform_test_helpers.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/time_conversion.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+#include "sensor_msgs/LaserScan.h"
+
+namespace cartographer_ros {
+namespace {
+
+using ::cartographer::sensor::LandmarkData;
+using ::cartographer::sensor::LandmarkObservation;
+using ::testing::AllOf;
+using ::testing::DoubleNear;
+using ::testing::ElementsAre;
+using ::testing::Field;
+
+constexpr double kEps = 1e-6;
+
+TEST(MsgConversion, LaserScanToPointCloud) {
+  sensor_msgs::LaserScan laser_scan;
+  for (int i = 0; i < 8; ++i) {
+    laser_scan.ranges.push_back(1.f);
+  }
+  laser_scan.angle_min = 0.f;
+  laser_scan.angle_max = 8.f * static_cast<float>(M_PI_4);
+  laser_scan.angle_increment = static_cast<float>(M_PI_4);
+  laser_scan.range_min = 0.f;
+  laser_scan.range_max = 10.f;
+
+  const auto point_cloud =
+      std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
+  EXPECT_TRUE(
+      point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps));
+  EXPECT_TRUE(point_cloud[1].position.isApprox(
+      Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
+  EXPECT_TRUE(
+      point_cloud[2].position.isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps));
+  EXPECT_TRUE(point_cloud[3].position.isApprox(
+      Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
+  EXPECT_TRUE(
+      point_cloud[4].position.isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), kEps));
+  EXPECT_TRUE(point_cloud[5].position.isApprox(
+      Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
+      kEps));
+  EXPECT_TRUE(
+      point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps));
+  EXPECT_TRUE(point_cloud[7].position.isApprox(
+      Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps));
+  for (int i = 0; i < 8; ++i) {
+    EXPECT_NEAR(point_cloud[i].time, 0.f, kEps);
+  }
+}
+
+TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
+  sensor_msgs::LaserScan laser_scan;
+  laser_scan.ranges.push_back(1.f);
+  laser_scan.ranges.push_back(std::numeric_limits<float>::infinity());
+  laser_scan.ranges.push_back(2.f);
+  laser_scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
+  laser_scan.ranges.push_back(3.f);
+  laser_scan.angle_min = 0.f;
+  laser_scan.angle_max = 3.f * static_cast<float>(M_PI_4);
+  laser_scan.angle_increment = static_cast<float>(M_PI_4);
+  laser_scan.range_min = 2.f;
+  laser_scan.range_max = 10.f;
+
+  const auto point_cloud =
+      std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
+  ASSERT_EQ(2, point_cloud.size());
+  EXPECT_TRUE(
+      point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps));
+  EXPECT_TRUE(
+      point_cloud[1].position.isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), kEps));
+  EXPECT_NEAR(point_cloud[0].time, 0.f, kEps);
+  EXPECT_NEAR(point_cloud[1].time, 0.f, kEps);
+}
+
+::testing::Matcher<const LandmarkObservation&> EqualsLandmark(
+    const LandmarkObservation& expected) {
+  return ::testing::AllOf(
+      Field(&LandmarkObservation::id, expected.id),
+      Field(&LandmarkObservation::landmark_to_tracking_transform,
+            ::cartographer::transform::IsNearly(
+                expected.landmark_to_tracking_transform, kEps)),
+      Field(&LandmarkObservation::translation_weight,
+            DoubleNear(expected.translation_weight, kEps)),
+      Field(&LandmarkObservation::rotation_weight,
+            DoubleNear(expected.rotation_weight, kEps)));
+}
+
+TEST(MsgConversion, LandmarkListToLandmarkData) {
+  cartographer_ros_msgs::LandmarkList message;
+  message.header.stamp.fromSec(10);
+
+  cartographer_ros_msgs::LandmarkEntry landmark_0;
+  landmark_0.id = "landmark_0";
+  landmark_0.tracking_from_landmark_transform.position.x = 1.0;
+  landmark_0.tracking_from_landmark_transform.position.y = 2.0;
+  landmark_0.tracking_from_landmark_transform.position.z = 3.0;
+  landmark_0.tracking_from_landmark_transform.orientation.w = 1.0;
+  landmark_0.tracking_from_landmark_transform.orientation.x = 0.0;
+  landmark_0.tracking_from_landmark_transform.orientation.y = 0.0;
+  landmark_0.tracking_from_landmark_transform.orientation.z = 0.0;
+  landmark_0.translation_weight = 1.0;
+  landmark_0.rotation_weight = 2.0;
+  message.landmarks.push_back(landmark_0);
+
+  LandmarkData actual_landmark_data = ToLandmarkData(message);
+  EXPECT_THAT(actual_landmark_data,
+              AllOf(Field(&LandmarkData::time, FromRos(message.header.stamp)),
+                    Field(&LandmarkData::landmark_observations,
+                          ElementsAre(EqualsLandmark(LandmarkObservation{
+                              "landmark_0",
+                              ::cartographer::transform::Rigid3d(
+                                  Eigen::Vector3d(1., 2., 3.),
+                                  Eigen::Quaterniond(1., 0., 0., 0.)),
+                              1.f,
+                              2.f,
+                          })))));
+}
+
+TEST(MsgConversion, LatLongAltToEcef) {
+  Eigen::Vector3d equator_prime_meridian = LatLongAltToEcef(0, 0, 0);
+  EXPECT_TRUE(equator_prime_meridian.isApprox(Eigen::Vector3d(6378137, 0, 0)))
+      << equator_prime_meridian;
+  Eigen::Vector3d plus_ten_meters = LatLongAltToEcef(0, 0, 10);
+  EXPECT_TRUE(plus_ten_meters.isApprox(Eigen::Vector3d(6378147, 0, 0)))
+      << plus_ten_meters;
+  Eigen::Vector3d north_pole = LatLongAltToEcef(90, 0, 0);
+  EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps))
+      << north_pole;
+  Eigen::Vector3d also_north_pole = LatLongAltToEcef(90, 90, 0);
+  EXPECT_TRUE(
+      also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps))
+      << also_north_pole;
+  Eigen::Vector3d south_pole = LatLongAltToEcef(-90, 0, 0);
+  EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), kEps))
+      << south_pole;
+  Eigen::Vector3d above_south_pole = LatLongAltToEcef(-90, 60, 20);
+  EXPECT_TRUE(
+      above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), kEps))
+      << above_south_pole;
+  Eigen::Vector3d somewhere_on_earth =
+      LatLongAltToEcef(48.1372149, 11.5748024, 517.1);
+  EXPECT_TRUE(somewhere_on_earth.isApprox(
+      Eigen::Vector3d(4177983, 855702, 4727457), kEps))
+      << somewhere_on_earth;
+}
+
+TEST(MsgConversion, ComputeLocalFrameFromLatLong) {
+  cartographer::transform::Rigid3d north_pole =
+      ComputeLocalFrameFromLatLong(90., 0.);
+  EXPECT_TRUE((north_pole * LatLongAltToEcef(90., 0., 1.))
+                  .isApprox(Eigen::Vector3d::UnitZ()));
+  cartographer::transform::Rigid3d south_pole =
+      ComputeLocalFrameFromLatLong(-90., 0.);
+  EXPECT_TRUE((south_pole * LatLongAltToEcef(-90., 0., 1.))
+                  .isApprox(Eigen::Vector3d::UnitZ()));
+  cartographer::transform::Rigid3d prime_meridian_equator =
+      ComputeLocalFrameFromLatLong(0., 0.);
+  EXPECT_TRUE((prime_meridian_equator * LatLongAltToEcef(0., 0., 1.))
+                  .isApprox(Eigen::Vector3d::UnitZ()))
+      << prime_meridian_equator * LatLongAltToEcef(0., 0., 1.);
+  cartographer::transform::Rigid3d meridian_90_equator =
+      ComputeLocalFrameFromLatLong(0., 90.);
+  EXPECT_TRUE((meridian_90_equator * LatLongAltToEcef(0., 90., 1.))
+                  .isApprox(Eigen::Vector3d::UnitZ()))
+      << meridian_90_equator * LatLongAltToEcef(0., 90., 1.);
+
+  std::mt19937 rng(42);
+  std::uniform_real_distribution<double> lat_distribution(-90., 90.);
+  std::uniform_real_distribution<double> long_distribution(-180., 180.);
+  std::uniform_real_distribution<double> alt_distribution(-519., 519.);
+
+  for (int i = 0; i < 1000; ++i) {
+    const double latitude = lat_distribution(rng);
+    const double longitude = long_distribution(rng);
+    const double altitude = alt_distribution(rng);
+    cartographer::transform::Rigid3d transform_to_local_frame =
+        ComputeLocalFrameFromLatLong(latitude, longitude);
+    EXPECT_TRUE((transform_to_local_frame *
+                 LatLongAltToEcef(latitude, longitude, altitude))
+                    .isApprox(altitude * Eigen::Vector3d::UnitZ(), kEps))
+        << transform_to_local_frame *
+               LatLongAltToEcef(latitude, longitude, altitude)
+        << "\n"
+        << altitude;
+  }
+}
+
+}  // namespace
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/node.cc

@@ -0,0 +1,841 @@
+/*
+ * 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 "cartographer_ros/node.h"
+
+#include <chrono>
+#include <string>
+#include <vector>
+
+#include "Eigen/Core"
+#include "absl/memory/memory.h"
+#include "cartographer/common/configuration_file_resolver.h"
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+#include "cartographer/mapping/pose_graph_interface.h"
+#include "cartographer/mapping/proto/submap_visualization.pb.h"
+#include "cartographer/metrics/register.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+#include "cartographer_ros/metrics/family_factory.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/sensor_bridge.h"
+#include "cartographer_ros/tf_bridge.h"
+#include "cartographer_ros/time_conversion.h"
+#include "cartographer_ros_msgs/StatusCode.h"
+#include "cartographer_ros_msgs/StatusResponse.h"
+#include "glog/logging.h"
+#include "nav_msgs/Odometry.h"
+#include "ros/serialization.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "tf2_eigen/tf2_eigen.h"
+#include "visualization_msgs/MarkerArray.h"
+
+namespace cartographer_ros {
+
+namespace {
+
+cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
+  cartographer_ros_msgs::SensorTopics topics;
+  topics.laser_scan_topic = kLaserScanTopic;
+  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
+  topics.point_cloud2_topic = kPointCloud2Topic;
+  topics.imu_topic = kImuTopic;
+  topics.odometry_topic = kOdometryTopic;
+  topics.nav_sat_fix_topic = kNavSatFixTopic;
+  topics.landmark_topic = kLandmarkTopic;
+  return topics;
+}
+
+// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and
+// calls 'handler' on the 'node' to handle messages. Returns the subscriber.
+template <typename MessageType>
+::ros::Subscriber SubscribeWithHandler(
+    void (Node::*handler)(int, const std::string&,
+                          const typename MessageType::ConstPtr&),
+    const int trajectory_id, const std::string& topic,
+    ::ros::NodeHandle* const node_handle, Node* const node) {
+  return node_handle->subscribe<MessageType>(
+      topic, kInfiniteSubscriberQueueSize,
+      boost::function<void(const typename MessageType::ConstPtr&)>(
+          [node, handler, trajectory_id,
+           topic](const typename MessageType::ConstPtr& msg) {
+            (node->*handler)(trajectory_id, topic, msg);
+          }));
+}
+
+}  // namespace
+
+namespace carto = ::cartographer;
+
+using carto::transform::Rigid3d;
+using TrajectoryState =
+    ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
+
+Node::Node(
+    const NodeOptions& node_options,
+    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
+    tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
+    : node_options_(node_options),
+      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
+  absl::MutexLock lock(&mutex_);
+  if (collect_metrics) {
+    metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
+    carto::metrics::RegisterAllMetrics(metrics_registry_.get());
+  }
+  //发布的 topics
+  submap_list_publisher_ =
+      node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
+          kSubmapListTopic, kLatestOnlyPublisherQueueSize);
+  trajectory_node_list_publisher_ =
+      node_handle_.advertise<::visualization_msgs::MarkerArray>(
+          kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
+  landmark_poses_list_publisher_ =
+      node_handle_.advertise<::visualization_msgs::MarkerArray>(
+          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
+  constraint_list_publisher_ =
+      node_handle_.advertise<::visualization_msgs::MarkerArray>(
+          kConstraintListTopic, kLatestOnlyPublisherQueueSize);
+      
+  //创建service节点
+  service_servers_.push_back(node_handle_.advertiseService(
+      kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kWriteStateServiceName, &Node::HandleWriteState, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kReadMetricsServiceName, &Node::HandleReadMetrics, this));
+
+  scan_matched_point_cloud_publisher_ =
+      node_handle_.advertise<sensor_msgs::PointCloud2>(
+          kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
+
+  wall_timers_.push_back(node_handle_.createWallTimer(
+      ::ros::WallDuration(node_options_.submap_publish_period_sec),
+      &Node::PublishSubmapList, this));
+  if (node_options_.pose_publish_period_sec > 0) {
+    publish_local_trajectory_data_timer_ = node_handle_.createTimer(
+        ::ros::Duration(node_options_.pose_publish_period_sec),
+        &Node::PublishLocalTrajectoryData, this);
+  }
+  wall_timers_.push_back(node_handle_.createWallTimer(
+      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
+      &Node::PublishTrajectoryNodeList, this));
+  wall_timers_.push_back(node_handle_.createWallTimer(
+      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
+      &Node::PublishLandmarkPosesList, this));
+  wall_timers_.push_back(node_handle_.createWallTimer(
+      ::ros::WallDuration(kConstraintPublishPeriodSec),
+      &Node::PublishConstraintList, this));
+}
+
+Node::~Node() { FinishAllTrajectories(); }
+
+::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
+
+bool Node::HandleSubmapQuery(
+    ::cartographer_ros_msgs::SubmapQuery::Request& request,
+    ::cartographer_ros_msgs::SubmapQuery::Response& response) {
+  absl::MutexLock lock(&mutex_);
+  map_builder_bridge_.HandleSubmapQuery(request, response);
+  return true;
+}
+
+void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
+  absl::MutexLock lock(&mutex_);
+  submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
+}
+
+void Node::AddExtrapolator(const int trajectory_id,
+                           const TrajectoryOptions& options) {
+  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
+  CHECK(extrapolators_.count(trajectory_id) == 0);
+  const double gravity_time_constant =
+      node_options_.map_builder_options.use_trajectory_builder_3d()
+          ? options.trajectory_builder_options.trajectory_builder_3d_options()
+                .imu_gravity_time_constant()
+          : options.trajectory_builder_options.trajectory_builder_2d_options()
+                .imu_gravity_time_constant();
+  extrapolators_.emplace(
+      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
+      std::forward_as_tuple(
+          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
+          gravity_time_constant));
+}
+
+void Node::AddSensorSamplers(const int trajectory_id,
+                             const TrajectoryOptions& options) {
+  CHECK(sensor_samplers_.count(trajectory_id) == 0);
+  sensor_samplers_.emplace(
+      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
+      std::forward_as_tuple(
+          options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
+          options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
+          options.landmarks_sampling_ratio));
+}
+
+void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
+  absl::MutexLock lock(&mutex_);
+  for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
+    const auto& trajectory_data = entry.second;
+
+    auto& extrapolator = extrapolators_.at(entry.first);
+    // We only publish a point cloud if it has changed. It is not needed at high
+    // frequency, and republishing it would be computationally wasteful.
+    if (trajectory_data.local_slam_data->time !=
+        extrapolator.GetLastPoseTime()) {
+      if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
+        // TODO(gaschler): Consider using other message without time
+        // information.
+        carto::sensor::TimedPointCloud point_cloud;
+        point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
+                                .returns.size());
+        for (const cartographer::sensor::RangefinderPoint point :
+             trajectory_data.local_slam_data->range_data_in_local.returns) {
+          point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
+              point, 0.f /* time */));
+        }
+        scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
+            carto::common::ToUniversal(trajectory_data.local_slam_data->time),
+            node_options_.map_frame,
+            carto::sensor::TransformTimedPointCloud(
+                point_cloud, trajectory_data.local_to_map.cast<float>())));
+      }
+      extrapolator.AddPose(trajectory_data.local_slam_data->time,
+                           trajectory_data.local_slam_data->local_pose);
+    }
+
+    geometry_msgs::TransformStamped stamped_transform;
+    // If we do not publish a new point cloud, we still allow time of the
+    // published poses to advance. If we already know a newer pose, we use its
+    // time instead. Since tf knows how to interpolate, providing newer
+    // information is better.
+    const ::cartographer::common::Time now = std::max(
+        FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime());
+    stamped_transform.header.stamp =
+        node_options_.use_pose_extrapolator
+            ? ToRos(now)
+            : ToRos(trajectory_data.local_slam_data->time);
+    const Rigid3d tracking_to_local_3d =
+        node_options_.use_pose_extrapolator
+            ? extrapolator.ExtrapolatePose(now)
+            : trajectory_data.local_slam_data->local_pose;
+    const Rigid3d tracking_to_local = [&] {
+      if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) {
+        return carto::transform::Embed3D(
+            carto::transform::Project2D(tracking_to_local_3d));
+      }
+      return tracking_to_local_3d;
+    }();
+
+    const Rigid3d tracking_to_map =
+        trajectory_data.local_to_map * tracking_to_local;
+
+    if (trajectory_data.published_to_tracking != nullptr) {
+      if (trajectory_data.trajectory_options.provide_odom_frame) {
+        std::vector<geometry_msgs::TransformStamped> stamped_transforms;
+
+        stamped_transform.header.frame_id = node_options_.map_frame;
+        stamped_transform.child_frame_id =
+            trajectory_data.trajectory_options.odom_frame;
+        stamped_transform.transform =
+            ToGeometryMsgTransform(trajectory_data.local_to_map);
+        stamped_transforms.push_back(stamped_transform);
+
+        stamped_transform.header.frame_id =
+            trajectory_data.trajectory_options.odom_frame;
+        stamped_transform.child_frame_id =
+            trajectory_data.trajectory_options.published_frame;
+        stamped_transform.transform = ToGeometryMsgTransform(
+            tracking_to_local * (*trajectory_data.published_to_tracking));
+        stamped_transforms.push_back(stamped_transform);
+
+        tf_broadcaster_.sendTransform(stamped_transforms);
+      } else {
+        stamped_transform.header.frame_id = node_options_.map_frame;
+        stamped_transform.child_frame_id =
+            trajectory_data.trajectory_options.published_frame;
+        stamped_transform.transform = ToGeometryMsgTransform(
+            tracking_to_map * (*trajectory_data.published_to_tracking));
+        tf_broadcaster_.sendTransform(stamped_transform);
+      }
+    }
+  }
+}
+
+void Node::PublishTrajectoryNodeList(
+    const ::ros::WallTimerEvent& unused_timer_event) {
+  if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
+    absl::MutexLock lock(&mutex_);
+    trajectory_node_list_publisher_.publish(
+        map_builder_bridge_.GetTrajectoryNodeList());
+  }
+}
+
+void Node::PublishLandmarkPosesList(
+    const ::ros::WallTimerEvent& unused_timer_event) {
+  if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
+    absl::MutexLock lock(&mutex_);
+    landmark_poses_list_publisher_.publish(
+        map_builder_bridge_.GetLandmarkPosesList());
+  }
+}
+
+void Node::PublishConstraintList(
+    const ::ros::WallTimerEvent& unused_timer_event) {
+  if (constraint_list_publisher_.getNumSubscribers() > 0) {
+    absl::MutexLock lock(&mutex_);
+    constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
+  }
+}
+
+std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
+Node::ComputeExpectedSensorIds(
+    const TrajectoryOptions& options,
+    const cartographer_ros_msgs::SensorTopics& topics) const {
+  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
+  using SensorType = SensorId::SensorType;
+  std::set<SensorId> expected_topics;
+  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
+  for (const std::string& topic : ComputeRepeatedTopicNames(
+           topics.laser_scan_topic, options.num_laser_scans)) {
+    expected_topics.insert(SensorId{SensorType::RANGE, topic});
+  }
+  for (const std::string& topic :
+       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
+                                 options.num_multi_echo_laser_scans)) {
+    expected_topics.insert(SensorId{SensorType::RANGE, topic});
+  }
+  for (const std::string& topic : ComputeRepeatedTopicNames(
+           topics.point_cloud2_topic, options.num_point_clouds)) {
+    expected_topics.insert(SensorId{SensorType::RANGE, topic});
+  }
+  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
+  // required.
+  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
+      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
+       options.trajectory_builder_options.trajectory_builder_2d_options()
+           .use_imu_data())) {
+    expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic});
+  }
+  // Odometry is optional.
+  if (options.use_odometry) {
+    expected_topics.insert(
+        SensorId{SensorType::ODOMETRY, topics.odometry_topic});
+  }
+  // NavSatFix is optional.
+  if (options.use_nav_sat) {
+    expected_topics.insert(
+        SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic});
+  }
+  // Landmark is optional.
+  if (options.use_landmarks) {
+    expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
+  }
+  return expected_topics;
+}
+
+int Node::AddTrajectory(const TrajectoryOptions& options,
+                        const cartographer_ros_msgs::SensorTopics& topics) 
+{
+  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
+      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
+  const int trajectory_id =
+      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
+  AddExtrapolator(trajectory_id, options);
+  AddSensorSamplers(trajectory_id, options);
+  LaunchSubscribers(options, topics, trajectory_id);
+  wall_timers_.push_back(node_handle_.createWallTimer(
+      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
+      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
+  for (const auto& sensor_id : expected_sensor_ids) {
+    subscribed_topics_.insert(sensor_id.id);
+  }
+  return trajectory_id;
+}
+
+void Node::LaunchSubscribers(const TrajectoryOptions& options,
+                             const cartographer_ros_msgs::SensorTopics& topics,
+                             const int trajectory_id)
+{
+  for (const std::string& topic : ComputeRepeatedTopicNames(topics.laser_scan_topic, options.num_laser_scans))
+  {
+    subscribers_[trajectory_id].push_back(
+        {
+	    SubscribeWithHandler<sensor_msgs::LaserScan>(
+             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
+             this),
+         topic}
+	);
+  }
+  for (const std::string& topic :
+       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
+                                 options.num_multi_echo_laser_scans)) {
+    subscribers_[trajectory_id].push_back(
+        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
+             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
+             &node_handle_, this),
+         topic});
+  }
+  for (const std::string& topic : ComputeRepeatedTopicNames(
+           topics.point_cloud2_topic, options.num_point_clouds)) {
+    subscribers_[trajectory_id].push_back(
+        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
+             &Node::HandlePointCloud2Message, trajectory_id, topic,
+             &node_handle_, this),
+         topic});
+  }
+
+  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
+  // required.
+  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
+      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
+       options.trajectory_builder_options.trajectory_builder_2d_options()
+           .use_imu_data())) {
+    std::string topic = topics.imu_topic;
+    subscribers_[trajectory_id].push_back(
+        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
+                                                trajectory_id, topic,
+                                                &node_handle_, this),
+         topic});
+  }
+
+  if (options.use_odometry) {
+    std::string topic = topics.odometry_topic;
+    subscribers_[trajectory_id].push_back(
+        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
+                                                  trajectory_id, topic,
+                                                  &node_handle_, this),
+         topic});
+  }
+  if (options.use_nav_sat) {
+    std::string topic = topics.nav_sat_fix_topic;
+    subscribers_[trajectory_id].push_back(
+        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
+             &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_,
+             this),
+         topic});
+  }
+  if (options.use_landmarks) {
+    std::string topic = topics.landmark_topic;
+    subscribers_[trajectory_id].push_back(
+        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
+             &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_,
+             this),
+         topic});
+  }
+}
+
+bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
+  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
+    return options.trajectory_builder_options
+        .has_trajectory_builder_2d_options();
+  }
+  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
+    return options.trajectory_builder_options
+        .has_trajectory_builder_3d_options();
+  }
+  return false;
+}
+
+bool Node::ValidateTopicNames(
+    const ::cartographer_ros_msgs::SensorTopics& topics,
+    const TrajectoryOptions& options) {
+  for (const auto& sensor_id : ComputeExpectedSensorIds(options, topics)) {
+    const std::string& topic = sensor_id.id;
+    if (subscribed_topics_.count(topic) > 0) {
+      LOG(ERROR) << "Topic name [" << topic << "] is already used.";
+      return false;
+    }
+  }
+  return true;
+}
+
+cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
+    const int trajectory_id) {
+  auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
+
+  cartographer_ros_msgs::StatusResponse status_response;
+  if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
+    const std::string message = "Trajectory " + std::to_string(trajectory_id) +
+                                " already pending to finish.";
+    status_response.code = cartographer_ros_msgs::StatusCode::OK;
+    status_response.message = message;
+    LOG(INFO) << message;
+    return status_response;
+  }
+
+  // First, check if we can actually finish the trajectory.
+  if (!(trajectory_states.count(trajectory_id))) {
+    const std::string error =
+        "Trajectory " + std::to_string(trajectory_id) + " doesn't exist.";
+    LOG(ERROR) << error;
+    status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
+    status_response.message = error;
+    return status_response;
+  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) {
+    const std::string error =
+        "Trajectory " + std::to_string(trajectory_id) + " is frozen.";
+    LOG(ERROR) << error;
+    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
+    status_response.message = error;
+    return status_response;
+  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) {
+    const std::string error = "Trajectory " + std::to_string(trajectory_id) +
+                              " has already been finished.";
+    LOG(ERROR) << error;
+    status_response.code =
+        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
+    status_response.message = error;
+    return status_response;
+  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) {
+    const std::string error =
+        "Trajectory " + std::to_string(trajectory_id) + " has been deleted.";
+    LOG(ERROR) << error;
+    status_response.code =
+        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
+    status_response.message = error;
+    return status_response;
+  }
+
+  // Shutdown the subscribers of this trajectory.
+  // A valid case with no subscribers is e.g. if we just visualize states.
+  if (subscribers_.count(trajectory_id)) {
+    for (auto& entry : subscribers_[trajectory_id]) {
+      entry.subscriber.shutdown();
+      subscribed_topics_.erase(entry.topic);
+      LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
+    }
+    CHECK_EQ(subscribers_.erase(trajectory_id), 1);
+  }
+  map_builder_bridge_.FinishTrajectory(trajectory_id);
+  trajectories_scheduled_for_finish_.emplace(trajectory_id);
+  const std::string message =
+      "Finished trajectory " + std::to_string(trajectory_id) + ".";
+  status_response.code = cartographer_ros_msgs::StatusCode::OK;
+  status_response.message = message;
+  return status_response;
+}
+
+bool Node::HandleStartTrajectory(
+    ::cartographer_ros_msgs::StartTrajectory::Request& request,
+    ::cartographer_ros_msgs::StartTrajectory::Response& response) {
+  absl::MutexLock lock(&mutex_);
+  TrajectoryOptions options;
+  if (!FromRosMessage(request.options, &options) ||
+      !ValidateTrajectoryOptions(options)) {
+    const std::string error = "Invalid trajectory options.";
+    LOG(ERROR) << error;
+    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
+    response.status.message = error;
+  } else if (!ValidateTopicNames(request.topics, options)) {
+    const std::string error = "Invalid topics.";
+    LOG(ERROR) << error;
+    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
+    response.status.message = error;
+  } else {
+    response.trajectory_id = AddTrajectory(options, request.topics);
+    response.status.code = cartographer_ros_msgs::StatusCode::OK;
+    response.status.message = "Success.";
+  }
+  return true;
+}
+
+void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
+  absl::MutexLock lock(&mutex_);
+  CHECK(ValidateTrajectoryOptions(options));
+  AddTrajectory(options, DefaultSensorTopics());
+}
+
+std::vector<
+    std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
+Node::ComputeDefaultSensorIdsForMultipleBags(
+    const std::vector<TrajectoryOptions>& bags_options) const {
+  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
+  std::vector<std::set<SensorId>> bags_sensor_ids;
+  for (size_t i = 0; i < bags_options.size(); ++i) {
+    std::string prefix;
+    if (bags_options.size() > 1) {
+      prefix = "bag_" + std::to_string(i + 1) + "_";
+    }
+    std::set<SensorId> unique_sensor_ids;
+    for (const auto& sensor_id :
+         ComputeExpectedSensorIds(bags_options.at(i), DefaultSensorTopics())) {
+      unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id});
+    }
+    bags_sensor_ids.push_back(unique_sensor_ids);
+  }
+  return bags_sensor_ids;
+}
+
+int Node::AddOfflineTrajectory(
+    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
+        expected_sensor_ids,
+    const TrajectoryOptions& options) {
+  absl::MutexLock lock(&mutex_);
+  const int trajectory_id =
+      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
+  AddExtrapolator(trajectory_id, options);
+  AddSensorSamplers(trajectory_id, options);
+  return trajectory_id;
+}
+
+bool Node::HandleGetTrajectoryStates(
+    ::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
+    ::cartographer_ros_msgs::GetTrajectoryStates::Response& response) {
+  using TrajectoryState =
+      ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
+  absl::MutexLock lock(&mutex_);
+  response.status.code = ::cartographer_ros_msgs::StatusCode::OK;
+  response.trajectory_states.header.stamp = ros::Time::now();
+  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
+    response.trajectory_states.trajectory_id.push_back(entry.first);
+    switch (entry.second) {
+      case TrajectoryState::ACTIVE:
+        response.trajectory_states.trajectory_state.push_back(
+            ::cartographer_ros_msgs::TrajectoryStates::ACTIVE);
+        break;
+      case TrajectoryState::FINISHED:
+        response.trajectory_states.trajectory_state.push_back(
+            ::cartographer_ros_msgs::TrajectoryStates::FINISHED);
+        break;
+      case TrajectoryState::FROZEN:
+        response.trajectory_states.trajectory_state.push_back(
+            ::cartographer_ros_msgs::TrajectoryStates::FROZEN);
+        break;
+      case TrajectoryState::DELETED:
+        response.trajectory_states.trajectory_state.push_back(
+            ::cartographer_ros_msgs::TrajectoryStates::DELETED);
+        break;
+    }
+  }
+  return true;
+}
+
+bool Node::HandleFinishTrajectory(
+    ::cartographer_ros_msgs::FinishTrajectory::Request& request,
+    ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
+  absl::MutexLock lock(&mutex_);
+  response.status = FinishTrajectoryUnderLock(request.trajectory_id);
+  return true;
+}
+
+bool Node::HandleWriteState(
+    ::cartographer_ros_msgs::WriteState::Request& request,
+    ::cartographer_ros_msgs::WriteState::Response& response) {
+  absl::MutexLock lock(&mutex_);
+  if (map_builder_bridge_.SerializeState(request.filename,
+                                         request.include_unfinished_submaps)) {
+    response.status.code = cartographer_ros_msgs::StatusCode::OK;
+    response.status.message = "State written to '" + request.filename + "'.";
+  } else {
+    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
+    response.status.message = "Failed to write '" + request.filename + "'.";
+  }
+  return true;
+}
+
+bool Node::HandleReadMetrics(
+    ::cartographer_ros_msgs::ReadMetrics::Request& request,
+    ::cartographer_ros_msgs::ReadMetrics::Response& response) {
+  absl::MutexLock lock(&mutex_);
+  response.timestamp = ros::Time::now();
+  if (!metrics_registry_) {
+    response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE;
+    response.status.message = "Collection of runtime metrics is not activated.";
+    return true;
+  }
+  metrics_registry_->ReadMetrics(&response);
+  response.status.code = cartographer_ros_msgs::StatusCode::OK;
+  response.status.message = "Successfully read metrics.";
+  return true;
+}
+
+void Node::FinishAllTrajectories() {
+  absl::MutexLock lock(&mutex_);
+  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
+    if (entry.second == TrajectoryState::ACTIVE) {
+      const int trajectory_id = entry.first;
+      CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
+               cartographer_ros_msgs::StatusCode::OK);
+    }
+  }
+}
+
+bool Node::FinishTrajectory(const int trajectory_id) {
+  absl::MutexLock lock(&mutex_);
+  return FinishTrajectoryUnderLock(trajectory_id).code ==
+         cartographer_ros_msgs::StatusCode::OK;
+}
+
+void Node::RunFinalOptimization() {
+  {
+    for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
+      const int trajectory_id = entry.first;
+      if (entry.second == TrajectoryState::ACTIVE) {
+        LOG(WARNING)
+            << "Can't run final optimization if there are one or more active "
+               "trajectories. Trying to finish trajectory with ID "
+            << std::to_string(trajectory_id) << " now.";
+        CHECK(FinishTrajectory(trajectory_id))
+            << "Failed to finish trajectory with ID "
+            << std::to_string(trajectory_id) << ".";
+      }
+    }
+  }
+  // Assuming we are not adding new data anymore, the final optimization
+  // can be performed without holding the mutex.
+  map_builder_bridge_.RunFinalOptimization();
+}
+
+void Node::HandleOdometryMessage(const int trajectory_id,
+                                 const std::string& sensor_id,
+                                 const nav_msgs::Odometry::ConstPtr& msg) {
+  absl::MutexLock lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
+    return;
+  }
+  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
+  auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
+  if (odometry_data_ptr != nullptr) {
+    extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
+  }
+  sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
+}
+
+void Node::HandleNavSatFixMessage(const int trajectory_id,
+                                  const std::string& sensor_id,
+                                  const sensor_msgs::NavSatFix::ConstPtr& msg) {
+  absl::MutexLock lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
+    return;
+  }
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleNavSatFixMessage(sensor_id, msg);
+}
+
+void Node::HandleLandmarkMessage(
+    const int trajectory_id, const std::string& sensor_id,
+    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
+  absl::MutexLock lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
+    return;
+  }
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleLandmarkMessage(sensor_id, msg);
+}
+
+void Node::HandleImuMessage(const int trajectory_id,
+                            const std::string& sensor_id,
+                            const sensor_msgs::Imu::ConstPtr& msg) {
+  absl::MutexLock lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
+    return;
+  }
+  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
+  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
+  if (imu_data_ptr != nullptr) {
+    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
+  }
+  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
+}
+
+void Node::HandleLaserScanMessage(const int trajectory_id,
+                                  const std::string& sensor_id,
+                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
+  absl::MutexLock lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
+    return;
+  }
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleLaserScanMessage(sensor_id, msg);
+}
+
+void Node::HandleMultiEchoLaserScanMessage(
+    const int trajectory_id, const std::string& sensor_id,
+    const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
+  absl::MutexLock lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
+    return;
+  }
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleMultiEchoLaserScanMessage(sensor_id, msg);
+}
+
+void Node::HandlePointCloud2Message(
+    const int trajectory_id, const std::string& sensor_id,
+    const sensor_msgs::PointCloud2::ConstPtr& msg) {
+  absl::MutexLock lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
+    return;
+  }
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandlePointCloud2Message(sensor_id, msg);
+}
+
+void Node::SerializeState(const std::string& filename,
+                          const bool include_unfinished_submaps) {
+  absl::MutexLock lock(&mutex_);
+  CHECK(
+      map_builder_bridge_.SerializeState(filename, include_unfinished_submaps))
+      << "Could not write state.";
+}
+
+void Node::LoadState(const std::string& state_filename,
+                     const bool load_frozen_state) {
+  absl::MutexLock lock(&mutex_);
+  map_builder_bridge_.LoadState(state_filename, load_frozen_state);
+}
+
+void Node::MaybeWarnAboutTopicMismatch(
+    const ::ros::WallTimerEvent& unused_timer_event) {
+  ::ros::master::V_TopicInfo ros_topics;
+  ::ros::master::getTopics(ros_topics);
+  std::set<std::string> published_topics;
+  std::stringstream published_topics_string;
+  for (const auto& it : ros_topics) {
+    std::string resolved_topic = node_handle_.resolveName(it.name, false);
+    published_topics.insert(resolved_topic);
+    published_topics_string << resolved_topic << ",";
+  }
+  bool print_topics = false;
+  for (const auto& entry : subscribers_) {
+    int trajectory_id = entry.first;
+    for (const auto& subscriber : entry.second) {
+      std::string resolved_topic = node_handle_.resolveName(subscriber.topic);
+      if (published_topics.count(resolved_topic) == 0) {
+        LOG(WARNING) << "Expected topic \"" << subscriber.topic
+                     << "\" (trajectory " << trajectory_id << ")"
+                     << " (resolved topic \"" << resolved_topic << "\")"
+                     << " but no publisher is currently active.";
+        print_topics = true;
+      }
+    }
+  }
+  if (print_topics) {
+    LOG(WARNING) << "Currently available topics are: "
+                 << published_topics_string.str();
+  }
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/node.h

@@ -0,0 +1,234 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H
+
+#include <map>
+#include <memory>
+#include <set>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#include "absl/synchronization/mutex.h"
+#include "cartographer/common/fixed_ratio_sampler.h"
+#include "cartographer/mapping/map_builder_interface.h"
+#include "cartographer/mapping/pose_extrapolator.h"
+#include "cartographer_ros/map_builder_bridge.h"
+#include "cartographer_ros/metrics/family_factory.h"
+#include "cartographer_ros/node_constants.h"
+#include "cartographer_ros/node_options.h"
+#include "cartographer_ros/trajectory_options.h"
+#include "cartographer_ros_msgs/FinishTrajectory.h"
+#include "cartographer_ros_msgs/GetTrajectoryStates.h"
+#include "cartographer_ros_msgs/ReadMetrics.h"
+#include "cartographer_ros_msgs/SensorTopics.h"
+#include "cartographer_ros_msgs/StartTrajectory.h"
+#include "cartographer_ros_msgs/StatusResponse.h"
+#include "cartographer_ros_msgs/SubmapEntry.h"
+#include "cartographer_ros_msgs/SubmapList.h"
+#include "cartographer_ros_msgs/SubmapQuery.h"
+#include "cartographer_ros_msgs/TrajectoryOptions.h"
+#include "cartographer_ros_msgs/WriteState.h"
+#include "nav_msgs/Odometry.h"
+#include "ros/ros.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/NavSatFix.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "tf2_ros/transform_broadcaster.h"
+
+namespace cartographer_ros {
+
+// Wires up ROS topics to SLAM.
+class Node {
+ public:
+  Node(const NodeOptions& node_options,
+       std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
+       tf2_ros::Buffer* tf_buffer, bool collect_metrics);
+  ~Node();
+
+  Node(const Node&) = delete;
+  Node& operator=(const Node&) = delete;
+
+  // Finishes all yet active trajectories.
+  void FinishAllTrajectories();
+  // Finishes a single given trajectory. Returns false if the trajectory did not
+  // exist or was already finished.
+  bool FinishTrajectory(int trajectory_id);
+
+  // Runs final optimization. All trajectories have to be finished when calling.
+  void RunFinalOptimization();
+
+  // Starts the first trajectory with the default topics.
+  void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
+
+  // Returns unique SensorIds for multiple input bag files based on
+  // their TrajectoryOptions.
+  // 'SensorId::id' is the expected ROS topic name.
+  std::vector<
+      std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
+  ComputeDefaultSensorIdsForMultipleBags(
+      const std::vector<TrajectoryOptions>& bags_options) const;
+
+  // Adds a trajectory for offline processing, i.e. not listening to topics.
+  int AddOfflineTrajectory(
+      const std::set<
+          cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
+          expected_sensor_ids,
+      const TrajectoryOptions& options);
+
+  // The following functions handle adding sensor data to a trajectory.
+  void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
+                             const nav_msgs::Odometry::ConstPtr& msg);
+  void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
+                              const sensor_msgs::NavSatFix::ConstPtr& msg);
+  void HandleLandmarkMessage(
+      int trajectory_id, const std::string& sensor_id,
+      const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
+  void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
+                        const sensor_msgs::Imu::ConstPtr& msg);
+  void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
+                              const sensor_msgs::LaserScan::ConstPtr& msg);
+  void HandleMultiEchoLaserScanMessage(
+      int trajectory_id, const std::string& sensor_id,
+      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
+  void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
+                                const sensor_msgs::PointCloud2::ConstPtr& msg);
+
+  // Serializes the complete Node state.
+  void SerializeState(const std::string& filename,
+                      const bool include_unfinished_submaps);
+
+  // Loads a serialized SLAM state from a .pbstream file.
+  void LoadState(const std::string& state_filename, bool load_frozen_state);
+
+  ::ros::NodeHandle* node_handle();
+
+ private:
+  struct Subscriber {
+    ::ros::Subscriber subscriber;
+
+    // ::ros::Subscriber::getTopic() does not necessarily return the same
+    // std::string
+    // it was given in its constructor. Since we rely on the topic name as the
+    // unique identifier of a subscriber, we remember it ourselves.
+    std::string topic;
+  };
+
+  bool HandleSubmapQuery(
+      cartographer_ros_msgs::SubmapQuery::Request& request,
+      cartographer_ros_msgs::SubmapQuery::Response& response);
+  bool HandleStartTrajectory(
+      cartographer_ros_msgs::StartTrajectory::Request& request,
+      cartographer_ros_msgs::StartTrajectory::Response& response);
+  bool HandleFinishTrajectory(
+      cartographer_ros_msgs::FinishTrajectory::Request& request,
+      cartographer_ros_msgs::FinishTrajectory::Response& response);
+  bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
+                        cartographer_ros_msgs::WriteState::Response& response);
+  bool HandleGetTrajectoryStates(
+      ::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
+      ::cartographer_ros_msgs::GetTrajectoryStates::Response& response);
+  bool HandleReadMetrics(
+      cartographer_ros_msgs::ReadMetrics::Request& request,
+      cartographer_ros_msgs::ReadMetrics::Response& response);
+
+  // Returns the set of SensorIds expected for a trajectory.
+  // 'SensorId::id' is the expected ROS topic name.
+  std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>
+  ComputeExpectedSensorIds(
+      const TrajectoryOptions& options,
+      const cartographer_ros_msgs::SensorTopics& topics) const;
+  int AddTrajectory(const TrajectoryOptions& options,
+                    const cartographer_ros_msgs::SensorTopics& topics);
+  void LaunchSubscribers(const TrajectoryOptions& options,
+                         const cartographer_ros_msgs::SensorTopics& topics,
+                         int trajectory_id);
+  void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
+  void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options);
+  void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
+  void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event);
+  void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
+  void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
+  void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
+  bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
+  bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
+                          const TrajectoryOptions& options);
+  cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(
+      int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);
+  void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&);
+
+  const NodeOptions node_options_;
+
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+
+  absl::Mutex mutex_;
+  MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
+
+  ::ros::NodeHandle node_handle_;
+  ::ros::Publisher submap_list_publisher_;
+  ::ros::Publisher trajectory_node_list_publisher_;
+  ::ros::Publisher landmark_poses_list_publisher_;
+  ::ros::Publisher constraint_list_publisher_;
+  // These ros::ServiceServers need to live for the lifetime of the node.
+  std::vector<::ros::ServiceServer> service_servers_;
+  ::ros::Publisher scan_matched_point_cloud_publisher_;
+
+  struct TrajectorySensorSamplers {
+    TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
+                             const double odometry_sampling_ratio,
+                             const double fixed_frame_pose_sampling_ratio,
+                             const double imu_sampling_ratio,
+                             const double landmark_sampling_ratio)
+        : rangefinder_sampler(rangefinder_sampling_ratio),
+          odometry_sampler(odometry_sampling_ratio),
+          fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
+          imu_sampler(imu_sampling_ratio),
+          landmark_sampler(landmark_sampling_ratio) {}
+
+    ::cartographer::common::FixedRatioSampler rangefinder_sampler;
+    ::cartographer::common::FixedRatioSampler odometry_sampler;
+    ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
+    ::cartographer::common::FixedRatioSampler imu_sampler;
+    ::cartographer::common::FixedRatioSampler landmark_sampler;
+  };
+
+  // These are keyed with 'trajectory_id'.
+  std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
+  std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
+  std::unordered_map<int, std::vector<Subscriber>> subscribers_;
+  std::unordered_set<std::string> subscribed_topics_;
+  std::unordered_set<int> trajectories_scheduled_for_finish_;
+
+  // We have to keep the timer handles of ::ros::WallTimers around, otherwise
+  // they do not fire.
+  std::vector<::ros::WallTimer> wall_timers_;
+
+  // The timer for publishing local trajectory data (i.e. pose transforms and
+  // range data point clouds) is a regular timer which is not triggered when
+  // simulation time is standing still. This prevents overflowing the transform
+  // listener buffer by publishing the same transforms over and over again.
+  ::ros::Timer publish_local_trajectory_data_timer_;
+
+  std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H

cartographer_ros/cartographer_ros/cartographer_ros/node_constants.cc

@@ -0,0 +1,37 @@
+/*
+ * 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 "cartographer_ros/node_constants.h"
+
+#include "glog/logging.h"
+
+namespace cartographer_ros {
+
+std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
+                                                   const int num_topics) {
+  CHECK_GE(num_topics, 0);
+  if (num_topics == 1) {
+    return {topic};
+  }
+  std::vector<std::string> topics;
+  topics.reserve(num_topics);
+  for (int i = 0; i < num_topics; ++i) {
+    topics.emplace_back(topic + "_" + std::to_string(i + 1));
+  }
+  return topics;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/node_constants.h

@@ -0,0 +1,57 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H
+
+#include <string>
+#include <vector>
+
+namespace cartographer_ros {
+
+// Default topic names; expected to be remapped as needed.
+constexpr char kLaserScanTopic[] = "scan";
+constexpr char kMultiEchoLaserScanTopic[] = "echoes";
+constexpr char kPointCloud2Topic[] = "points2";
+constexpr char kImuTopic[] = "imu";
+constexpr char kOdometryTopic[] = "odom";
+constexpr char kNavSatFixTopic[] = "fix";
+constexpr char kLandmarkTopic[] = "landmark";
+constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
+constexpr char kOccupancyGridTopic[] = "map";
+constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
+constexpr char kSubmapListTopic[] = "submap_list";
+constexpr char kSubmapQueryServiceName[] = "submap_query";
+constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
+constexpr char kWriteStateServiceName[] = "write_state";
+constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states";
+constexpr char kReadMetricsServiceName[] = "read_metrics";
+constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
+constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
+constexpr char kConstraintListTopic[] = "constraint_list";
+constexpr double kConstraintPublishPeriodSec = 0.5;
+constexpr double kTopicMismatchCheckDelaySec = 3.0;
+
+constexpr int kInfiniteSubscriberQueueSize = 0;
+constexpr int kLatestOnlyPublisherQueueSize = 1;
+
+// For multiple topics adds numbers to the topic name and returns the list.
+std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
+                                                   int num_topics);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H

cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc

@@ -0,0 +1,100 @@
+/*
+ * 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 "absl/memory/memory.h"
+#include "cartographer/mapping/map_builder.h"
+#include "cartographer_ros/node.h"
+#include "cartographer_ros/node_options.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "gflags/gflags.h"
+#include "tf2_ros/transform_listener.h"
+
+DEFINE_bool(collect_metrics, false,
+            "Activates the collection of runtime metrics. If activated, the "
+            "metrics can be accessed via a ROS service.");
+DEFINE_string(configuration_directory, "",
+              "First directory in which configuration files are searched, "
+              "second is always the Cartographer installation to allow "
+              "including files from there.");
+DEFINE_string(configuration_basename, "",
+              "Basename, i.e. not containing any directory prefix, of the "
+              "configuration file.");
+DEFINE_string(load_state_filename, "",
+              "If non-empty, filename of a .pbstream file to load, containing "
+              "a saved SLAM state.");
+DEFINE_bool(load_frozen_state, true,
+            "Load the saved state as frozen (non-optimized) trajectories.");
+DEFINE_bool(
+    start_trajectory_with_default_topics, true,
+    "Enable to immediately start the first trajectory with default topics.");
+DEFINE_string(
+    save_state_filename, "",
+    "If non-empty, serialize state and write it to disk before shutting down.");
+
+namespace cartographer_ros {
+namespace {
+
+void Run() {
+  constexpr double kTfBufferCacheTimeInSeconds = 10.;
+  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
+  tf2_ros::TransformListener tf(tf_buffer);
+  NodeOptions node_options;
+  TrajectoryOptions trajectory_options;
+  std::tie(node_options, trajectory_options) =
+      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
+
+  auto map_builder = absl::make_unique<cartographer::mapping::MapBuilder>(
+      node_options.map_builder_options);
+  Node node(node_options, std::move(map_builder), &tf_buffer,
+            FLAGS_collect_metrics);
+  if (!FLAGS_load_state_filename.empty()) {
+    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
+  }
+
+  if (FLAGS_start_trajectory_with_default_topics) {
+    node.StartTrajectoryWithDefaultTopics(trajectory_options);
+  }
+
+  ::ros::spin();
+
+  node.FinishAllTrajectories();
+  node.RunFinalOptimization();
+
+  if (!FLAGS_save_state_filename.empty()) {
+    node.SerializeState(FLAGS_save_state_filename,
+                        true /* include_unfinished_submaps */);
+  }
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_configuration_directory.empty())
+      << "-configuration_directory is missing.";
+  CHECK(!FLAGS_configuration_basename.empty())
+      << "-configuration_basename is missing.";
+
+  ::ros::init(argc, argv, "cartographer_node");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+  cartographer_ros::Run();
+  ::ros::shutdown();
+}

cartographer_ros/cartographer_ros/cartographer_ros/node_options.cc

@@ -0,0 +1,65 @@
+/*
+ * 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 "cartographer_ros/node_options.h"
+
+#include <vector>
+
+#include "cartographer/common/configuration_file_resolver.h"
+#include "cartographer/mapping/map_builder.h"
+#include "glog/logging.h"
+
+namespace cartographer_ros {
+
+NodeOptions CreateNodeOptions(
+    ::cartographer::common::LuaParameterDictionary* const
+        lua_parameter_dictionary) {
+  NodeOptions options;
+  options.map_builder_options =
+      ::cartographer::mapping::CreateMapBuilderOptions(
+          lua_parameter_dictionary->GetDictionary("map_builder").get());
+  options.map_frame = lua_parameter_dictionary->GetString("map_frame");
+  options.lookup_transform_timeout_sec =
+      lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec");
+  options.submap_publish_period_sec =
+      lua_parameter_dictionary->GetDouble("submap_publish_period_sec");
+  options.pose_publish_period_sec =
+      lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
+  options.trajectory_publish_period_sec =
+      lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec");
+  if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) {
+    options.use_pose_extrapolator =
+        lua_parameter_dictionary->GetBool("use_pose_extrapolator");
+  }
+  return options;
+}
+
+std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
+    const std::string& configuration_directory,
+    const std::string& configuration_basename) {
+  auto file_resolver =
+      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
+          std::vector<std::string>{configuration_directory});
+  const std::string code =
+      file_resolver->GetFileContentOrDie(configuration_basename);
+  cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
+      code, std::move(file_resolver));
+
+  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
+                         CreateTrajectoryOptions(&lua_parameter_dictionary));
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/node_options.h

@@ -0,0 +1,50 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H
+
+#include <string>
+#include <tuple>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
+#include "cartographer/mapping/proto/map_builder_options.pb.h"
+#include "cartographer_ros/trajectory_options.h"
+
+namespace cartographer_ros {
+
+// Top-level options of Cartographer's ROS integration.
+struct NodeOptions {
+  ::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
+  std::string map_frame;
+  double lookup_transform_timeout_sec;
+  double submap_publish_period_sec;
+  double pose_publish_period_sec;
+  double trajectory_publish_period_sec;
+  bool use_pose_extrapolator = true;
+};
+
+NodeOptions CreateNodeOptions(
+    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
+
+std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
+    const std::string& configuration_directory,
+    const std::string& configuration_basename);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H

cartographer_ros/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc

@@ -0,0 +1,192 @@
+/*
+ * 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 <cmath>
+#include <string>
+#include <vector>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "absl/synchronization/mutex.h"
+#include "cairo/cairo.h"
+#include "cartographer/common/port.h"
+#include "cartographer/io/image.h"
+#include "cartographer/io/submap_painter.h"
+#include "cartographer/mapping/id.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/node_constants.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "cartographer_ros/submap.h"
+#include "cartographer_ros_msgs/SubmapList.h"
+#include "cartographer_ros_msgs/SubmapQuery.h"
+#include "gflags/gflags.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "ros/ros.h"
+
+DEFINE_double(resolution, 0.05,
+              "Resolution of a grid cell in the published occupancy grid.");
+DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period.");
+DEFINE_bool(include_frozen_submaps, true,
+            "Include frozen submaps in the occupancy grid.");
+DEFINE_bool(include_unfrozen_submaps, true,
+            "Include unfrozen submaps in the occupancy grid.");
+DEFINE_string(occupancy_grid_topic, cartographer_ros::kOccupancyGridTopic,
+              "Name of the topic on which the occupancy grid is published.");
+
+namespace cartographer_ros {
+namespace {
+
+using ::cartographer::io::PaintSubmapSlicesResult;
+using ::cartographer::io::SubmapSlice;
+using ::cartographer::mapping::SubmapId;
+
+class Node {
+ public:
+  explicit Node(double resolution, double publish_period_sec);
+  ~Node() {}
+
+  Node(const Node&) = delete;
+  Node& operator=(const Node&) = delete;
+
+ private:
+  void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg);
+  void DrawAndPublish(const ::ros::WallTimerEvent& timer_event);
+
+  ::ros::NodeHandle node_handle_;
+  const double resolution_;
+
+  absl::Mutex mutex_;
+  ::ros::ServiceClient client_ GUARDED_BY(mutex_);
+  ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_);
+  ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_);
+  std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);
+  ::ros::WallTimer occupancy_grid_publisher_timer_;
+  std::string last_frame_id_;
+  ros::Time last_timestamp_;
+};
+
+Node::Node(const double resolution, const double publish_period_sec)
+    : resolution_(resolution),
+      client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
+          kSubmapQueryServiceName)),
+      submap_list_subscriber_(node_handle_.subscribe(
+          kSubmapListTopic, kLatestOnlyPublisherQueueSize,
+          boost::function<void(
+              const cartographer_ros_msgs::SubmapList::ConstPtr&)>(
+              [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
+                HandleSubmapList(msg);
+              }))),
+      occupancy_grid_publisher_(
+          node_handle_.advertise<::nav_msgs::OccupancyGrid>(
+              FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize,
+              true /* latched */)),
+      occupancy_grid_publisher_timer_(
+          node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
+                                       &Node::DrawAndPublish, this)) {}
+
+void Node::HandleSubmapList(
+    const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
+  absl::MutexLock locker(&mutex_);
+
+  // We do not do any work if nobody listens.
+  if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
+    return;
+  }
+
+  // Keep track of submap IDs that don't appear in the message anymore.
+  std::set<SubmapId> submap_ids_to_delete;
+  for (const auto& pair : submap_slices_) {
+    submap_ids_to_delete.insert(pair.first);
+  }
+
+  for (const auto& submap_msg : msg->submap) {
+    const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
+    submap_ids_to_delete.erase(id);
+    if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) ||
+        (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) {
+      continue;
+    }
+    SubmapSlice& submap_slice = submap_slices_[id];
+    submap_slice.pose = ToRigid3d(submap_msg.pose);
+    submap_slice.metadata_version = submap_msg.submap_version;
+    if (submap_slice.surface != nullptr &&
+        submap_slice.version == submap_msg.submap_version) {
+      continue;
+    }
+
+    auto fetched_textures =
+        ::cartographer_ros::FetchSubmapTextures(id, &client_);
+    if (fetched_textures == nullptr) {
+      continue;
+    }
+    CHECK(!fetched_textures->textures.empty());
+    submap_slice.version = fetched_textures->version;
+
+    // We use the first texture only. By convention this is the highest
+    // resolution texture and that is the one we want to use to construct the
+    // map for ROS.
+    const auto fetched_texture = fetched_textures->textures.begin();
+    submap_slice.width = fetched_texture->width;
+    submap_slice.height = fetched_texture->height;
+    submap_slice.slice_pose = fetched_texture->slice_pose;
+    submap_slice.resolution = fetched_texture->resolution;
+    submap_slice.cairo_data.clear();
+    submap_slice.surface = ::cartographer::io::DrawTexture(
+        fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,
+        fetched_texture->width, fetched_texture->height,
+        &submap_slice.cairo_data);
+  }
+
+  // Delete all submaps that didn't appear in the message.
+  for (const auto& id : submap_ids_to_delete) {
+    submap_slices_.erase(id);
+  }
+
+  last_timestamp_ = msg->header.stamp;
+  last_frame_id_ = msg->header.frame_id;
+}
+
+void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) {
+  absl::MutexLock locker(&mutex_);
+  if (submap_slices_.empty() || last_frame_id_.empty()) {
+    return;
+  }
+  auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_);
+  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
+      painted_slices, resolution_, last_frame_id_, last_timestamp_);
+  occupancy_grid_publisher_.publish(*msg_ptr);
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps)
+      << "Ignoring both frozen and unfrozen submaps makes no sense.";
+
+  ::ros::init(argc, argv, "cartographer_occupancy_grid_node");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+  ::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec);
+
+  ::ros::spin();
+  ::ros::shutdown();
+}

cartographer_ros/cartographer_ros/cartographer_ros/offline_node.cc

@@ -0,0 +1,386 @@
+/*
+ * Copyright 2018 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 "cartographer_ros/offline_node.h"
+
+#include <errno.h>
+#include <string.h>
+#ifndef WIN32
+#include <sys/resource.h>
+#endif
+#include <time.h>
+#include <chrono>
+
+#include "absl/strings/str_split.h"
+#include "cartographer_ros/node.h"
+#include "cartographer_ros/playable_bag.h"
+#include "cartographer_ros/urdf_reader.h"
+#include "gflags/gflags.h"
+#include "ros/callback_queue.h"
+#include "rosgraph_msgs/Clock.h"
+#include "tf2_ros/static_transform_broadcaster.h"
+#include "urdf/model.h"
+
+DEFINE_bool(collect_metrics, false,
+            "Activates the collection of runtime metrics. If activated, the "
+            "metrics can be accessed via a ROS service.");
+DEFINE_string(configuration_directory, "",
+              "First directory in which configuration files are searched, "
+              "second is always the Cartographer installation to allow "
+              "including files from there.");
+DEFINE_string(
+    configuration_basenames, "",
+    "Comma-separated list of basenames, i.e. not containing any "
+    "directory prefix, of the configuration files for each trajectory. "
+    "The first configuration file will be used for node options. "
+    "If less configuration files are specified than trajectories, the "
+    "first file will be used the remaining trajectories.");
+DEFINE_string(
+    bag_filenames, "",
+    "Comma-separated list of bags to process. One bag per trajectory. "
+    "Any combination of simultaneous and sequential bags is supported.");
+DEFINE_string(urdf_filenames, "",
+              "Comma-separated list of one or more URDF files that contain "
+              "static links for the sensor configuration(s).");
+DEFINE_bool(use_bag_transforms, true,
+            "Whether to read, use and republish transforms from bags.");
+DEFINE_string(load_state_filename, "",
+              "If non-empty, filename of a .pbstream file to load, containing "
+              "a saved SLAM state.");
+DEFINE_bool(load_frozen_state, true,
+            "Load the saved state as frozen (non-optimized) trajectories.");
+DEFINE_bool(keep_running, false,
+            "Keep running the offline node after all messages from the bag "
+            "have been processed.");
+DEFINE_double(skip_seconds, 0,
+              "Optional amount of seconds to skip from the beginning "
+              "(i.e. when the earliest bag starts.). ");
+
+namespace cartographer_ros {
+
+constexpr char kClockTopic[] = "clock";
+constexpr char kTfStaticTopic[] = "/tf_static";
+constexpr char kTfTopic[] = "tf";
+constexpr double kClockPublishFrequencySec = 1. / 30.;
+constexpr int kSingleThreaded = 1;
+// We publish tf messages one second earlier than other messages. Under
+// the assumption of higher frequency tf this should ensure that tf can
+// always interpolate.
+const ::ros::Duration kDelay = ::ros::Duration(1.0);
+
+void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
+  CHECK(!FLAGS_configuration_directory.empty())
+      << "-configuration_directory is missing.";
+  CHECK(!FLAGS_configuration_basenames.empty())
+      << "-configuration_basenames is missing.";
+  CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty()))
+      << "-bag_filenames and -load_state_filename cannot both be unspecified.";
+  const std::vector<std::string> bag_filenames =
+      absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty());
+  cartographer_ros::NodeOptions node_options;
+  const std::vector<std::string> configuration_basenames =
+      absl::StrSplit(FLAGS_configuration_basenames, ',', absl::SkipEmpty());
+  std::vector<TrajectoryOptions> bag_trajectory_options(1);
+  std::tie(node_options, bag_trajectory_options.at(0)) =
+      LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0));
+
+  for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) {
+    TrajectoryOptions current_trajectory_options;
+    if (bag_index < configuration_basenames.size()) {
+      std::tie(std::ignore, current_trajectory_options) = LoadOptions(
+          FLAGS_configuration_directory, configuration_basenames.at(bag_index));
+    } else {
+      current_trajectory_options = bag_trajectory_options.at(0);
+    }
+    bag_trajectory_options.push_back(current_trajectory_options);
+  }
+  if (bag_filenames.size() > 0) {
+    CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size());
+  }
+
+  // Since we preload the transform buffer, we should never have to wait for a
+  // transform. When we finish processing the bag, we will simply drop any
+  // remaining sensor data that cannot be transformed due to missing transforms.
+  node_options.lookup_transform_timeout_sec = 0.;
+
+  auto map_builder = map_builder_factory(node_options.map_builder_options);
+
+  const std::chrono::time_point<std::chrono::steady_clock> start_time =
+      std::chrono::steady_clock::now();
+
+  tf2_ros::Buffer tf_buffer;
+
+  std::vector<geometry_msgs::TransformStamped> urdf_transforms;
+  const std::vector<std::string> urdf_filenames =
+      absl::StrSplit(FLAGS_urdf_filenames, ',', absl::SkipEmpty());
+  for (const auto& urdf_filename : urdf_filenames) {
+    const auto current_urdf_transforms =
+        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
+    urdf_transforms.insert(urdf_transforms.end(),
+                           current_urdf_transforms.begin(),
+                           current_urdf_transforms.end());
+  }
+
+  tf_buffer.setUsingDedicatedThread(true);
+
+  Node node(node_options, std::move(map_builder), &tf_buffer,
+            FLAGS_collect_metrics);
+  if (!FLAGS_load_state_filename.empty()) {
+    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
+  }
+
+  ::ros::Publisher tf_publisher =
+      node.node_handle()->advertise<tf2_msgs::TFMessage>(
+          kTfTopic, kLatestOnlyPublisherQueueSize);
+
+  ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;
+
+  ::ros::Publisher clock_publisher =
+      node.node_handle()->advertise<rosgraph_msgs::Clock>(
+          kClockTopic, kLatestOnlyPublisherQueueSize);
+
+  if (urdf_transforms.size() > 0) {
+    static_tf_broadcaster.sendTransform(urdf_transforms);
+  }
+
+  ros::AsyncSpinner async_spinner(kSingleThreaded);
+  async_spinner.start();
+  rosgraph_msgs::Clock clock;
+  auto clock_republish_timer = node.node_handle()->createWallTimer(
+      ::ros::WallDuration(kClockPublishFrequencySec),
+      [&clock_publisher, &clock](const ::ros::WallTimerEvent&) {
+        clock_publisher.publish(clock);
+      },
+      false /* oneshot */, false /* autostart */);
+
+  std::vector<
+      std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
+      bag_expected_sensor_ids;
+  if (configuration_basenames.size() == 1) {
+    const auto current_bag_expected_sensor_ids =
+        node.ComputeDefaultSensorIdsForMultipleBags(
+            {bag_trajectory_options.front()});
+    bag_expected_sensor_ids = {bag_filenames.size(),
+                               current_bag_expected_sensor_ids.front()};
+  } else {
+    bag_expected_sensor_ids =
+        node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options);
+  }
+  CHECK_EQ(bag_expected_sensor_ids.size(), bag_filenames.size());
+
+  std::map<std::pair<int /* bag_index */, std::string>,
+           cartographer::mapping::TrajectoryBuilderInterface::SensorId>
+      bag_topic_to_sensor_id;
+  PlayableBagMultiplexer playable_bag_multiplexer;
+  for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size();
+       ++current_bag_index) {
+    const std::string& bag_filename = bag_filenames.at(current_bag_index);
+    if (!::ros::ok()) {
+      return;
+    }
+    for (const auto& expected_sensor_id :
+         bag_expected_sensor_ids.at(current_bag_index)) {
+      const auto bag_resolved_topic = std::make_pair(
+          static_cast<int>(current_bag_index),
+          node.node_handle()->resolveName(expected_sensor_id.id));
+      if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) {
+        LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag "
+                   << current_bag_index << " resolves to topic "
+                   << bag_resolved_topic.second << " which is already used by "
+                   << " sensor "
+                   << bag_topic_to_sensor_id.at(bag_resolved_topic).id;
+      }
+      bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id;
+    }
+
+    playable_bag_multiplexer.AddPlayableBag(PlayableBag(
+        bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay,
+        // PlayableBag::FilteringEarlyMessageHandler is used to get an early
+        // peek at the tf messages in the bag and insert them into 'tf_buffer'.
+        // When a message is retrieved by GetNextMessage() further below,
+        // we will have already inserted further 'kDelay' seconds worth of
+        // transforms into 'tf_buffer' via this lambda.
+        [&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) {
+          if (msg.isType<tf2_msgs::TFMessage>()) {
+            if (FLAGS_use_bag_transforms) {
+              const auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
+              tf_publisher.publish(tf_message);
+
+              for (const auto& transform : tf_message->transforms) {
+                try {
+                  // We need to keep 'tf_buffer' small because it becomes very
+                  // inefficient otherwise. We make sure that tf_messages are
+                  // published before any data messages, so that tf lookups
+                  // always work.
+                  tf_buffer.setTransform(transform, "unused_authority",
+                                         msg.getTopic() == kTfStaticTopic);
+                } catch (const tf2::TransformException& ex) {
+                  LOG(WARNING) << ex.what();
+                }
+              }
+            }
+            // Tell 'PlayableBag' to filter the tf message since there is no
+            // further use for it.
+            return false;
+          } else {
+            return true;
+          }
+        }));
+  }
+
+  std::set<std::string> bag_topics;
+  std::stringstream bag_topics_string;
+  for (const auto& topic : playable_bag_multiplexer.topics()) {
+    std::string resolved_topic = node.node_handle()->resolveName(topic, false);
+    bag_topics.insert(resolved_topic);
+    bag_topics_string << resolved_topic << ",";
+  }
+  bool print_topics = false;
+  for (const auto& entry : bag_topic_to_sensor_id) {
+    const std::string& resolved_topic = entry.first.second;
+    if (bag_topics.count(resolved_topic) == 0) {
+      LOG(WARNING) << "Expected resolved topic \"" << resolved_topic
+                   << "\" not found in bag file(s).";
+      print_topics = true;
+    }
+  }
+  if (print_topics) {
+    LOG(WARNING) << "Available topics in bag file(s) are "
+                 << bag_topics_string.str();
+  }
+
+  std::unordered_map<int, int> bag_index_to_trajectory_id;
+  const ros::Time begin_time =
+      // If no bags were loaded, we cannot peek the time of first message.
+      playable_bag_multiplexer.IsMessageAvailable()
+          ? playable_bag_multiplexer.PeekMessageTime()
+          : ros::Time();
+  while (playable_bag_multiplexer.IsMessageAvailable()) {
+    if (!::ros::ok()) {
+      return;
+    }
+
+    const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage();
+    const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple);
+    const int bag_index = std::get<1>(next_msg_tuple);
+    const bool is_last_message_in_bag = std::get<2>(next_msg_tuple);
+
+    if (msg.getTime() < (begin_time + ros::Duration(FLAGS_skip_seconds))) {
+      continue;
+    }
+
+    int trajectory_id;
+    // Lazily add trajectories only when the first message arrives in order
+    // to avoid blocking the sensor queue.
+    if (bag_index_to_trajectory_id.count(bag_index) == 0) {
+      trajectory_id =
+          node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index),
+                                    bag_trajectory_options.at(bag_index));
+      CHECK(bag_index_to_trajectory_id
+                .emplace(std::piecewise_construct,
+                         std::forward_as_tuple(bag_index),
+                         std::forward_as_tuple(trajectory_id))
+                .second);
+      LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag "
+                << bag_filenames.at(bag_index);
+    } else {
+      trajectory_id = bag_index_to_trajectory_id.at(bag_index);
+    }
+
+    const auto bag_topic = std::make_pair(
+        bag_index,
+        node.node_handle()->resolveName(msg.getTopic(), false /* resolve */));
+    auto it = bag_topic_to_sensor_id.find(bag_topic);
+    if (it != bag_topic_to_sensor_id.end()) {
+      const std::string& sensor_id = it->second.id;
+      if (msg.isType<sensor_msgs::LaserScan>()) {
+        node.HandleLaserScanMessage(trajectory_id, sensor_id,
+                                    msg.instantiate<sensor_msgs::LaserScan>());
+      }
+      if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
+        node.HandleMultiEchoLaserScanMessage(
+            trajectory_id, sensor_id,
+            msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
+      }
+      if (msg.isType<sensor_msgs::PointCloud2>()) {
+        node.HandlePointCloud2Message(
+            trajectory_id, sensor_id,
+            msg.instantiate<sensor_msgs::PointCloud2>());
+      }
+      if (msg.isType<sensor_msgs::Imu>()) {
+        node.HandleImuMessage(trajectory_id, sensor_id,
+                              msg.instantiate<sensor_msgs::Imu>());
+      }
+      if (msg.isType<nav_msgs::Odometry>()) {
+        node.HandleOdometryMessage(trajectory_id, sensor_id,
+                                   msg.instantiate<nav_msgs::Odometry>());
+      }
+      if (msg.isType<sensor_msgs::NavSatFix>()) {
+        node.HandleNavSatFixMessage(trajectory_id, sensor_id,
+                                    msg.instantiate<sensor_msgs::NavSatFix>());
+      }
+      if (msg.isType<cartographer_ros_msgs::LandmarkList>()) {
+        node.HandleLandmarkMessage(
+            trajectory_id, sensor_id,
+            msg.instantiate<cartographer_ros_msgs::LandmarkList>());
+      }
+    }
+    clock.clock = msg.getTime();
+    clock_publisher.publish(clock);
+
+    if (is_last_message_in_bag) {
+      node.FinishTrajectory(trajectory_id);
+    }
+  }
+
+  // Ensure the clock is republished after the bag has been finished, during the
+  // final optimization, serialization, and optional indefinite spinning at the
+  // end.
+  clock_republish_timer.start();
+  node.RunFinalOptimization();
+
+  const std::chrono::time_point<std::chrono::steady_clock> end_time =
+      std::chrono::steady_clock::now();
+  const double wall_clock_seconds =
+      std::chrono::duration_cast<std::chrono::duration<double>>(end_time -
+                                                                start_time)
+          .count();
+
+  LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s";
+#ifdef __linux__
+  timespec cpu_timespec = {};
+  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec);
+  LOG(INFO) << "Elapsed CPU time: "
+            << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
+  rusage usage;
+  CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno);
+  LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
+#endif
+
+  if (::ros::ok() && bag_filenames.size() > 0) {
+    const std::string output_filename = bag_filenames.front();
+    const std::string suffix = ".pbstream";
+    const std::string state_output_filename = output_filename + suffix;
+    LOG(INFO) << "Writing state to '" << state_output_filename << "'...";
+    node.SerializeState(state_output_filename,
+                        true /* include_unfinished_submaps */);
+  }
+  if (FLAGS_keep_running) {
+    ::ros::waitForShutdown();
+  }
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/offline_node.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2018 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "cartographer/mapping/map_builder_interface.h"
+#include "cartographer_ros/node_options.h"
+
+namespace cartographer_ros {
+
+using MapBuilderFactory =
+    std::function<std::unique_ptr<::cartographer::mapping::MapBuilderInterface>(
+        const ::cartographer::mapping::proto::MapBuilderOptions&)>;
+
+void RunOfflineNode(const MapBuilderFactory& map_builder_factory);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H

cartographer_ros/cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -0,0 +1,42 @@
+/*
+ * Copyright 2017 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 "cartographer/mapping/map_builder.h"
+#include "cartographer_ros/offline_node.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "gflags/gflags.h"
+#include "ros/ros.h"
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  ::ros::init(argc, argv, "cartographer_offline_node");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+
+  const cartographer_ros::MapBuilderFactory map_builder_factory =
+      [](const ::cartographer::mapping::proto::MapBuilderOptions&
+             map_builder_options) {
+        return absl::make_unique< ::cartographer::mapping::MapBuilder>(
+            map_builder_options);
+      };
+
+  cartographer_ros::RunOfflineNode(map_builder_factory);
+
+  ::ros::shutdown();
+}

cartographer_ros/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc

@@ -0,0 +1,99 @@
+/*
+ * Copyright 2018 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.
+ */
+
+// Publishes a frozen nav_msgs/OccupancyGrid map from serialized submaps.
+
+#include <map>
+#include <string>
+
+#include "cartographer/io/proto_stream.h"
+#include "cartographer/io/proto_stream_deserializer.h"
+#include "cartographer/io/submap_painter.h"
+#include "cartographer/mapping/2d/probability_grid.h"
+#include "cartographer/mapping/2d/submap_2d.h"
+#include "cartographer/mapping/3d/submap_3d.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/node_constants.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "cartographer_ros/ros_map.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "ros/ros.h"
+
+DEFINE_string(pbstream_filename, "",
+              "Filename of a pbstream to draw a map from.");
+DEFINE_string(map_topic, "map", "Name of the published map topic.");
+DEFINE_string(map_frame_id, "map", "Frame ID of the published map.");
+DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
+
+namespace cartographer_ros {
+namespace {
+
+std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
+    const std::string& pbstream_filename, const double resolution) {
+  ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
+  ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
+
+  LOG(INFO) << "Loading submap slices from serialized data.";
+  std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
+      submap_slices;
+  ::cartographer::mapping::ValueConversionTables conversion_tables;
+  ::cartographer::io::DeserializeAndFillSubmapSlices(
+      &deserializer, &submap_slices, &conversion_tables);
+  CHECK(reader.eof());
+
+  LOG(INFO) << "Generating combined map image from submap slices.";
+  const auto painted_slices =
+      ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
+  return CreateOccupancyGridMsg(painted_slices, resolution, FLAGS_map_frame_id,
+                                ros::Time::now());
+}
+
+void Run(const std::string& pbstream_filename, const std::string& map_topic,
+         const std::string& map_frame_id, const double resolution) {
+  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr =
+      LoadOccupancyGridMsg(pbstream_filename, resolution);
+
+  ::ros::NodeHandle node_handle("");
+  ::ros::Publisher pub = node_handle.advertise<nav_msgs::OccupancyGrid>(
+      map_topic, kLatestOnlyPublisherQueueSize, true /*latched */);
+
+  LOG(INFO) << "Publishing occupancy grid topic " << map_topic
+            << " (frame_id: " << map_frame_id
+            << ", resolution:" << std::to_string(resolution) << ").";
+  pub.publish(*msg_ptr);
+  ::ros::spin();
+  ::ros::shutdown();
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
+
+  ::ros::init(argc, argv, "cartographer_pbstream_map_publisher");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+
+  ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_topic,
+                          FLAGS_map_frame_id, FLAGS_resolution);
+}

cartographer_ros/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc

@@ -0,0 +1,81 @@
+/*
+ * Copyright 2017 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 <map>
+#include <string>
+
+#include "cartographer/io/proto_stream.h"
+#include "cartographer/io/proto_stream_deserializer.h"
+#include "cartographer/io/submap_painter.h"
+#include "cartographer/mapping/2d/probability_grid.h"
+#include "cartographer/mapping/2d/submap_2d.h"
+#include "cartographer/mapping/3d/submap_3d.h"
+#include "cartographer_ros/ros_map.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_string(pbstream_filename, "",
+              "Filename of a pbstream to draw a map from.");
+DEFINE_string(map_filestem, "map", "Stem of the output files.");
+DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
+
+namespace cartographer_ros {
+namespace {
+
+void Run(const std::string& pbstream_filename, const std::string& map_filestem,
+         const double resolution) {
+  ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
+  ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
+
+  LOG(INFO) << "Loading submap slices from serialized data.";
+  std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
+      submap_slices;
+  ::cartographer::mapping::ValueConversionTables conversion_tables;
+  ::cartographer::io::DeserializeAndFillSubmapSlices(
+      &deserializer, &submap_slices, &conversion_tables);
+  CHECK(reader.eof());
+
+  LOG(INFO) << "Generating combined map image from submap slices.";
+  auto result =
+      ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
+
+  ::cartographer::io::StreamFileWriter pgm_writer(map_filestem + ".pgm");
+
+  ::cartographer::io::Image image(std::move(result.surface));
+  WritePgm(image, resolution, &pgm_writer);
+
+  const Eigen::Vector2d origin(
+      -result.origin.x() * resolution,
+      (result.origin.y() - image.height()) * resolution);
+
+  ::cartographer::io::StreamFileWriter yaml_writer(map_filestem + ".yaml");
+  WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  FLAGS_alsologtostderr = true;
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
+  CHECK(!FLAGS_map_filestem.empty()) << "-map_filestem is missing.";
+
+  ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_filestem,
+                          FLAGS_resolution);
+}

cartographer_ros/cartographer_ros/cartographer_ros/playable_bag.cc

@@ -0,0 +1,170 @@
+/*
+ * Copyright 2018 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 "cartographer_ros/playable_bag.h"
+
+#include "absl/memory/memory.h"
+#include "cartographer_ros/node_constants.h"
+#include "glog/logging.h"
+#include "tf2_msgs/TFMessage.h"
+
+namespace cartographer_ros {
+
+PlayableBag::PlayableBag(
+    const std::string& bag_filename, const int bag_id,
+    const ros::Time start_time, const ros::Time end_time,
+    const ros::Duration buffer_delay,
+    FilteringEarlyMessageHandler filtering_early_message_handler)
+    : bag_(absl::make_unique<rosbag::Bag>(bag_filename, rosbag::bagmode::Read)),
+      view_(absl::make_unique<rosbag::View>(*bag_, start_time, end_time)),
+      view_iterator_(view_->begin()),
+      finished_(false),
+      bag_id_(bag_id),
+      bag_filename_(bag_filename),
+      duration_in_seconds_(
+          (view_->getEndTime() - view_->getBeginTime()).toSec()),
+      message_counter_(0),
+      buffer_delay_(buffer_delay),
+      filtering_early_message_handler_(
+          std::move(filtering_early_message_handler)) {
+  AdvanceUntilMessageAvailable();
+  for (const auto* connection_info : view_->getConnections()) {
+    topics_.insert(connection_info->topic);
+  }
+}
+
+ros::Time PlayableBag::PeekMessageTime() const {
+  CHECK(IsMessageAvailable());
+  return buffered_messages_.front().getTime();
+}
+
+std::tuple<ros::Time, ros::Time> PlayableBag::GetBeginEndTime() const {
+  return std::make_tuple(view_->getBeginTime(), view_->getEndTime());
+}
+
+rosbag::MessageInstance PlayableBag::GetNextMessage(
+    cartographer_ros_msgs::BagfileProgress* progress) {
+  CHECK(IsMessageAvailable());
+  const rosbag::MessageInstance msg = buffered_messages_.front();
+  buffered_messages_.pop_front();
+  AdvanceUntilMessageAvailable();
+  double processed_seconds = (msg.getTime() - view_->getBeginTime()).toSec();
+  if ((message_counter_ % 10000) == 0) {
+    LOG(INFO) << "Processed " << processed_seconds << " of "
+              << duration_in_seconds_ << " seconds of bag " << bag_filename_;
+  }
+
+  if (progress) {
+    progress->current_bagfile_name = bag_filename_;
+    progress->current_bagfile_id = bag_id_;
+    progress->total_messages = view_->size();
+    progress->processed_messages = message_counter_;
+    progress->total_seconds = duration_in_seconds_;
+    progress->processed_seconds = processed_seconds;
+  }
+
+  return msg;
+}
+
+bool PlayableBag::IsMessageAvailable() const {
+  return !buffered_messages_.empty() &&
+         (buffered_messages_.front().getTime() <
+          buffered_messages_.back().getTime() - buffer_delay_);
+}
+
+int PlayableBag::bag_id() const { return bag_id_; }
+
+void PlayableBag::AdvanceOneMessage() {
+  CHECK(!finished_);
+  if (view_iterator_ == view_->end()) {
+    finished_ = true;
+    return;
+  }
+  rosbag::MessageInstance& msg = *view_iterator_;
+  if (!filtering_early_message_handler_ ||
+      filtering_early_message_handler_(msg)) {
+    buffered_messages_.push_back(msg);
+  }
+  ++view_iterator_;
+  ++message_counter_;
+}
+
+void PlayableBag::AdvanceUntilMessageAvailable() {
+  if (finished_) {
+    return;
+  }
+  do {
+    AdvanceOneMessage();
+  } while (!finished_ && !IsMessageAvailable());
+}
+
+PlayableBagMultiplexer::PlayableBagMultiplexer() : pnh_("~") {
+  bag_progress_pub_ = pnh_.advertise<cartographer_ros_msgs::BagfileProgress>(
+      "bagfile_progress", 10);
+  progress_pub_interval_ = pnh_.param("bagfile_progress_pub_interval", 10.0);
+}
+
+void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) {
+  for (const auto& topic : playable_bag.topics()) {
+    topics_.insert(topic);
+  }
+  playable_bags_.push_back(std::move(playable_bag));
+  CHECK(playable_bags_.back().IsMessageAvailable());
+  next_message_queue_.emplace(
+      BagMessageItem{playable_bags_.back().PeekMessageTime(),
+                     static_cast<int>(playable_bags_.size() - 1)});
+  bag_progress_time_map_[playable_bag.bag_id()] = ros::Time::now();
+}
+
+bool PlayableBagMultiplexer::IsMessageAvailable() const {
+  return !next_message_queue_.empty();
+}
+
+std::tuple<rosbag::MessageInstance, int, bool>
+PlayableBagMultiplexer::GetNextMessage() {
+  CHECK(IsMessageAvailable());
+  const int current_bag_index = next_message_queue_.top().bag_index;
+  PlayableBag& current_bag = playable_bags_.at(current_bag_index);
+  cartographer_ros_msgs::BagfileProgress progress;
+  rosbag::MessageInstance msg = current_bag.GetNextMessage(&progress);
+  const bool publish_progress =
+      current_bag.finished() ||
+      ros::Time::now() - bag_progress_time_map_[current_bag.bag_id()] >=
+          ros::Duration(progress_pub_interval_);
+  if (bag_progress_pub_.getNumSubscribers() > 0 && publish_progress) {
+    progress.total_bagfiles = playable_bags_.size();
+    if (current_bag.finished()) {
+      progress.processed_seconds = current_bag.duration_in_seconds();
+    }
+    bag_progress_pub_.publish(progress);
+    bag_progress_time_map_[current_bag.bag_id()] = ros::Time::now();
+  }
+  CHECK_EQ(msg.getTime(), next_message_queue_.top().message_timestamp);
+  next_message_queue_.pop();
+  if (current_bag.IsMessageAvailable()) {
+    next_message_queue_.emplace(
+        BagMessageItem{current_bag.PeekMessageTime(), current_bag_index});
+  }
+  return std::make_tuple(std::move(msg), current_bag.bag_id(),
+                         !current_bag.IsMessageAvailable());
+}
+
+ros::Time PlayableBagMultiplexer::PeekMessageTime() const {
+  CHECK(IsMessageAvailable());
+  return next_message_queue_.top().message_timestamp;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/playable_bag.h

@@ -0,0 +1,116 @@
+/*
+ * Copyright 2018 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H
+#include <functional>
+#include <queue>
+
+#include <cartographer_ros_msgs/BagfileProgress.h>
+#include <ros/node_handle.h>
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+#include "tf2_ros/buffer.h"
+
+namespace cartographer_ros {
+
+class PlayableBag {
+ public:
+  // Handles messages early, i.e. when they are about to enter the buffer.
+  // Returns a boolean indicating whether the message should enter the buffer.
+  using FilteringEarlyMessageHandler =
+      std::function<bool /* forward_message_to_buffer */ (
+          const rosbag::MessageInstance&)>;
+
+  PlayableBag(const std::string& bag_filename, int bag_id, ros::Time start_time,
+              ros::Time end_time, ros::Duration buffer_delay,
+              FilteringEarlyMessageHandler filtering_early_message_handler);
+
+  ros::Time PeekMessageTime() const;
+  rosbag::MessageInstance GetNextMessage(
+      cartographer_ros_msgs::BagfileProgress* progress);
+  bool IsMessageAvailable() const;
+  std::tuple<ros::Time, ros::Time> GetBeginEndTime() const;
+
+  int bag_id() const;
+  std::set<std::string> topics() const { return topics_; }
+  double duration_in_seconds() const { return duration_in_seconds_; }
+  bool finished() const { return finished_; }
+
+ private:
+  void AdvanceOneMessage();
+  void AdvanceUntilMessageAvailable();
+
+  std::unique_ptr<rosbag::Bag> bag_;
+  std::unique_ptr<rosbag::View> view_;
+  rosbag::View::const_iterator view_iterator_;
+  bool finished_;
+  const int bag_id_;
+  const std::string bag_filename_;
+  const double duration_in_seconds_;
+  int message_counter_;
+  std::deque<rosbag::MessageInstance> buffered_messages_;
+  const ::ros::Duration buffer_delay_;
+  FilteringEarlyMessageHandler filtering_early_message_handler_;
+  std::set<std::string> topics_;
+};
+
+class PlayableBagMultiplexer {
+ public:
+  PlayableBagMultiplexer();
+  void AddPlayableBag(PlayableBag playable_bag);
+
+  // Returns the next message from the multiplexed (merge-sorted) message
+  // stream, along with the bag id corresponding to the message, and whether
+  // this was the last message in that bag.
+  std::tuple<rosbag::MessageInstance, int /* bag_id */,
+             bool /* is_last_message_in_bag */>
+  GetNextMessage();
+
+  bool IsMessageAvailable() const;
+  ros::Time PeekMessageTime() const;
+
+  std::set<std::string> topics() const { return topics_; }
+
+ private:
+  struct BagMessageItem {
+    ros::Time message_timestamp;
+    int bag_index;
+    struct TimestampIsGreater {
+      bool operator()(const BagMessageItem& l, const BagMessageItem& r) {
+        return l.message_timestamp > r.message_timestamp;
+      }
+    };
+  };
+
+  ros::NodeHandle pnh_;
+  // Publishes information about the bag-file(s) processing and its progress
+  ros::Publisher bag_progress_pub_;
+  // Map between bagfile id and the last time when its progress was published
+  std::map<int, ros::Time> bag_progress_time_map_;
+  // The time interval of publishing bag-file(s) processing in seconds
+  double progress_pub_interval_;
+
+  std::vector<PlayableBag> playable_bags_;
+  std::priority_queue<BagMessageItem, std::vector<BagMessageItem>,
+                      BagMessageItem::TimestampIsGreater>
+      next_message_queue_;
+  std::set<std::string> topics_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H

cartographer_ros/cartographer_ros/cartographer_ros/ros_log_sink.cc

@@ -0,0 +1,76 @@
+/*
+ * 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 "cartographer_ros/ros_log_sink.h"
+
+#include <chrono>
+#include <cstring>
+#include <string>
+#include <thread>
+
+#include "glog/log_severity.h"
+#include "ros/console.h"
+
+namespace cartographer_ros {
+
+namespace {
+
+const char* GetBasename(const char* filepath) {
+  const char* base = std::strrchr(filepath, '/');
+  return base ? (base + 1) : filepath;
+}
+
+}  // namespace
+
+ScopedRosLogSink::ScopedRosLogSink() : will_die_(false) { AddLogSink(this); }
+ScopedRosLogSink::~ScopedRosLogSink() { RemoveLogSink(this); }
+
+void ScopedRosLogSink::send(const ::google::LogSeverity severity,
+                            const char* const filename,
+                            const char* const base_filename, const int line,
+                            const struct std::tm* const tm_time,
+                            const char* const message,
+                            const size_t message_len) {
+  const std::string message_string = ::google::LogSink::ToString(
+      severity, GetBasename(filename), line, tm_time, message, message_len);
+  switch (severity) {
+    case ::google::GLOG_INFO:
+      ROS_INFO_STREAM(message_string);
+      break;
+
+    case ::google::GLOG_WARNING:
+      ROS_WARN_STREAM(message_string);
+      break;
+
+    case ::google::GLOG_ERROR:
+      ROS_ERROR_STREAM(message_string);
+      break;
+
+    case ::google::GLOG_FATAL:
+      ROS_FATAL_STREAM(message_string);
+      will_die_ = true;
+      break;
+  }
+}
+
+void ScopedRosLogSink::WaitTillSent() {
+  if (will_die_) {
+    // Give ROS some time to actually publish our message.
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  }
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/ros_log_sink.h

@@ -0,0 +1,45 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H
+
+#include <ctime>
+
+#include "glog/logging.h"
+
+namespace cartographer_ros {
+
+// Makes Google logging use ROS logging for output while an instance of this
+// class exists.
+class ScopedRosLogSink : public ::google::LogSink {
+ public:
+  ScopedRosLogSink();
+  ~ScopedRosLogSink() override;
+
+  void send(::google::LogSeverity severity, const char* filename,
+            const char* base_filename, int line, const struct std::tm* tm_time,
+            const char* message, size_t message_len) override;
+
+  void WaitTillSent() override;
+
+ private:
+  bool will_die_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H

cartographer_ros/cartographer_ros/cartographer_ros/ros_map.cc

@@ -0,0 +1,49 @@
+/*
+ * Copyright 2017 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 "cartographer_ros/ros_map.h"
+
+namespace cartographer_ros {
+
+void WritePgm(const ::cartographer::io::Image& image, const double resolution,
+              ::cartographer::io::FileWriter* file_writer) {
+  const std::string header = "P5\n# Cartographer map; " +
+                             std::to_string(resolution) + " m/pixel\n" +
+                             std::to_string(image.width()) + " " +
+                             std::to_string(image.height()) + "\n255\n";
+  file_writer->Write(header.data(), header.size());
+  for (int y = 0; y < image.height(); ++y) {
+    for (int x = 0; x < image.width(); ++x) {
+      const char color = image.GetPixel(x, y)[0];
+      file_writer->Write(&color, 1);
+    }
+  }
+}
+
+void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
+               const std::string& pgm_filename,
+               ::cartographer::io::FileWriter* file_writer) {
+  // Magic constants taken directly from ros map_saver code:
+  // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114
+  const std::string output =
+      "image: " + pgm_filename + "\n" +
+      "resolution: " + std::to_string(resolution) + "\n" + "origin: [" +
+      std::to_string(origin.x()) + ", " + std::to_string(origin.y()) +
+      ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n";
+  file_writer->Write(output.data(), output.size());
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/ros_map.h

@@ -0,0 +1,41 @@
+/*
+ * Copyright 2017 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H
+
+#include <string>
+
+#include "Eigen/Core"
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/image.h"
+#include "cartographer/mapping/2d/map_limits.h"
+
+namespace cartographer_ros {
+
+// Write 'image' as a pgm into 'file_writer'. The resolution is used in the
+// comment only'
+void WritePgm(const ::cartographer::io::Image& image, const double resolution,
+              ::cartographer::io::FileWriter* file_writer);
+
+// Write the corresponding yaml into 'file_writer'.
+void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
+               const std::string& pgm_filename,
+               ::cartographer::io::FileWriter* file_writer);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H

cartographer_ros/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc

@@ -0,0 +1,96 @@
+/*
+ * 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 "cartographer_ros/ros_map_writing_points_processor.h"
+
+#include "absl/memory/memory.h"
+#include "cartographer/io/image.h"
+#include "cartographer/io/probability_grid_points_processor.h"
+#include "cartographer_ros/ros_map.h"
+
+namespace cartographer_ros {
+
+RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
+    const double resolution,
+    const ::cartographer::mapping::proto::
+        ProbabilityGridRangeDataInserterOptions2D& range_data_inserter_options,
+    ::cartographer::io::FileWriterFactory file_writer_factory,
+    const std::string& filestem,
+    ::cartographer::io::PointsProcessor* const next)
+    : filestem_(filestem),
+      next_(next),
+      file_writer_factory_(file_writer_factory),
+      range_data_inserter_(range_data_inserter_options),
+      probability_grid_(::cartographer::io::CreateProbabilityGrid(
+          resolution, &conversion_tables_)) {}
+
+std::unique_ptr<RosMapWritingPointsProcessor>
+RosMapWritingPointsProcessor::FromDictionary(
+    ::cartographer::io::FileWriterFactory file_writer_factory,
+    ::cartographer::common::LuaParameterDictionary* const dictionary,
+    ::cartographer::io::PointsProcessor* const next) {
+  return absl::make_unique<RosMapWritingPointsProcessor>(
+      dictionary->GetDouble("resolution"),
+      ::cartographer::mapping::CreateProbabilityGridRangeDataInserterOptions2D(
+          dictionary->GetDictionary("range_data_inserter").get()),
+      file_writer_factory, dictionary->GetString("filestem"), next);
+}
+
+void RosMapWritingPointsProcessor::Process(
+    std::unique_ptr<::cartographer::io::PointsBatch> batch) {
+  range_data_inserter_.Insert({batch->origin, batch->points, {}},
+                              &probability_grid_);
+  next_->Process(std::move(batch));
+}
+
+::cartographer::io::PointsProcessor::FlushResult
+RosMapWritingPointsProcessor::Flush() {
+  Eigen::Array2i offset;
+  std::unique_ptr<::cartographer::io::Image> image =
+      ::cartographer::io::DrawProbabilityGrid(probability_grid_, &offset);
+  if (image != nullptr) {
+    auto pgm_writer = file_writer_factory_(filestem_ + ".pgm");
+    const std::string pgm_filename = pgm_writer->GetFilename();
+    const auto& limits = probability_grid_.limits();
+    image->Rotate90DegreesClockwise();
+
+    WritePgm(*image, limits.resolution(), pgm_writer.get());
+    CHECK(pgm_writer->Close());
+
+    const Eigen::Vector2d origin(
+        limits.max().x() - (offset.y() + image->width()) * limits.resolution(),
+        limits.max().y() -
+            (offset.x() + image->height()) * limits.resolution());
+    auto yaml_writer = file_writer_factory_(filestem_ + ".yaml");
+    WriteYaml(limits.resolution(), origin, pgm_filename, yaml_writer.get());
+    CHECK(yaml_writer->Close());
+  }
+
+  switch (next_->Flush()) {
+    case FlushResult::kRestartStream:
+      LOG(FATAL) << "ROS map writing must be configured to occur after any "
+                    "stages that require multiple passes.";
+
+    case FlushResult::kFinished:
+      return FlushResult::kFinished;
+  }
+  LOG(FATAL);
+  // The following unreachable return statement is needed to avoid a GCC bug
+  // described at https://gcc.gnu.org/bugzilla/show_bug.cgi?id=81508
+  return FlushResult::kFinished;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h

@@ -0,0 +1,69 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/points_processor.h"
+#include "cartographer/mapping/2d/probability_grid.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
+#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h"
+#include "cartographer/mapping/value_conversion_tables.h"
+
+namespace cartographer_ros {
+
+// Very similar to Cartographer's ProbabilityGridPointsProcessor, but writes
+// out a PGM and YAML suitable for ROS map server to consume.
+class RosMapWritingPointsProcessor
+    : public ::cartographer::io::PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName = "write_ros_map";
+  RosMapWritingPointsProcessor(
+      double resolution,
+      const ::cartographer::mapping::proto::
+          ProbabilityGridRangeDataInserterOptions2D&
+              range_data_inserter_options,
+      ::cartographer::io::FileWriterFactory file_writer_factory,
+      const std::string& filestem, PointsProcessor* next);
+  RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete;
+  RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) =
+      delete;
+
+  static std::unique_ptr<RosMapWritingPointsProcessor> FromDictionary(
+      ::cartographer::io::FileWriterFactory file_writer_factory,
+      ::cartographer::common::LuaParameterDictionary* dictionary,
+      PointsProcessor* next);
+
+  ~RosMapWritingPointsProcessor() override {}
+
+  void Process(std::unique_ptr<::cartographer::io::PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  const std::string filestem_;
+  PointsProcessor* const next_;
+  ::cartographer::io::FileWriterFactory file_writer_factory_;
+  ::cartographer::mapping::ProbabilityGridRangeDataInserter2D
+      range_data_inserter_;
+  ::cartographer::mapping::ValueConversionTables conversion_tables_;
+  ::cartographer::mapping::ProbabilityGrid probability_grid_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H

cartographer_ros/cartographer_ros/cartographer_ros/rosbag_validate_main.cc

@@ -0,0 +1,425 @@
+/*
+ * Copyright 2017 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 <fstream>
+#include <iostream>
+#include <map>
+#include <set>
+#include <string>
+
+#include "absl/memory/memory.h"
+#include "cartographer/common/histogram.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "nav_msgs/Odometry.h"
+#include "ros/ros.h"
+#include "ros/time.h"
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "tf2_eigen/tf2_eigen.h"
+#include "tf2_msgs/TFMessage.h"
+#include "tf2_ros/buffer.h"
+#include "urdf/model.h"
+
+DEFINE_string(bag_filename, "", "Bag to process.");
+DEFINE_bool(dump_timing, false,
+            "Dump per-sensor timing information in files called "
+            "timing_<frame_id>.csv in the current directory.");
+
+namespace cartographer_ros {
+namespace {
+
+struct FrameProperties {
+  ros::Time last_timestamp;
+  std::string topic;
+  std::vector<float> time_deltas;
+  std::unique_ptr<std::ofstream> timing_file;
+  std::string data_type;
+};
+
+const double kMinLinearAcceleration = 3.;
+const double kMaxLinearAcceleration = 30.;
+const double kTimeDeltaSerializationSensorWarning = 0.1;
+const double kTimeDeltaSerializationSensorError = 0.5;
+const double kMinAverageAcceleration = 9.5;
+const double kMaxAverageAcceleration = 10.5;
+const double kMaxGapPointsData = 0.1;
+const double kMaxGapImuData = 0.05;
+const std::set<std::string> kPointDataTypes = {
+    std::string(
+        ros::message_traits::DataType<sensor_msgs::PointCloud2>::value()),
+    std::string(ros::message_traits::DataType<
+                sensor_msgs::MultiEchoLaserScan>::value()),
+    std::string(
+        ros::message_traits::DataType<sensor_msgs::LaserScan>::value())};
+
+std::unique_ptr<std::ofstream> CreateTimingFile(const std::string& frame_id) {
+  auto timing_file = absl::make_unique<std::ofstream>(
+      std::string("timing_") + frame_id + ".csv", std::ios_base::out);
+
+  (*timing_file)
+      << "# Timing information for sensor with frame id: " << frame_id
+      << std::endl
+      << "# Columns are in order" << std::endl
+      << "# - packet index of the packet in the bag, first packet is 1"
+      << std::endl
+      << "# - timestamp when rosbag wrote the packet, i.e. "
+         "rosbag::MessageInstance::getTime().toNSec()"
+      << std::endl
+      << "# - timestamp when data was acquired, i.e. "
+         "message.header.stamp.toNSec()"
+      << std::endl
+      << "#" << std::endl
+      << "# The data can be read in python using" << std::endl
+      << "# import numpy" << std::endl
+      << "# np.loadtxt(<filename>, dtype='uint64')" << std::endl;
+
+  return timing_file;
+}
+
+void CheckImuMessage(const sensor_msgs::Imu& imu_message) {
+  auto linear_acceleration = ToEigen(imu_message.linear_acceleration);
+  if (std::isnan(linear_acceleration.norm()) ||
+      linear_acceleration.norm() < kMinLinearAcceleration ||
+      linear_acceleration.norm() > kMaxLinearAcceleration) {
+    LOG_FIRST_N(WARNING, 3)
+        << "frame_id " << imu_message.header.frame_id << " time "
+        << imu_message.header.stamp.toNSec() << ": IMU linear acceleration is "
+        << linear_acceleration.norm() << " m/s^2,"
+        << " expected is [" << kMinLinearAcceleration << ", "
+        << kMaxLinearAcceleration << "] m/s^2."
+        << " (It should include gravity and be given in m/s^2.)"
+        << " linear_acceleration " << linear_acceleration.transpose();
+  }
+}
+
+bool IsValidPose(const geometry_msgs::Pose& pose) {
+  return ToRigid3d(pose).IsValid();
+}
+
+void CheckOdometryMessage(const nav_msgs::Odometry& message) {
+  const auto& pose = message.pose.pose;
+  if (!IsValidPose(pose)) {
+    LOG_FIRST_N(ERROR, 3) << "frame_id " << message.header.frame_id << " time "
+                          << message.header.stamp.toNSec()
+                          << ": Odometry pose is invalid."
+                          << " pose " << pose;
+  }
+}
+
+void CheckTfMessage(const tf2_msgs::TFMessage& message) {
+  for (const auto& transform : message.transforms) {
+    if (transform.header.frame_id == "map") {
+      LOG_FIRST_N(ERROR, 1)
+          << "Input contains transform message from frame_id \""
+          << transform.header.frame_id << "\" to child_frame_id \""
+          << transform.child_frame_id
+          << "\". This is almost always output published by cartographer and "
+             "should not appear as input. (Unless you have some complex "
+             "remove_frames configuration, this is will not work. Simplest "
+             "solution is to record input without cartographer running.)";
+    }
+  }
+}
+
+bool IsPointDataType(const std::string& data_type) {
+  return (kPointDataTypes.count(data_type) != 0);
+}
+
+class RangeDataChecker {
+ public:
+  template <typename MessageType>
+  void CheckMessage(const MessageType& message) {
+    const std::string& frame_id = message.header.frame_id;
+    ros::Time current_time_stamp = message.header.stamp;
+    RangeChecksum current_checksum;
+    cartographer::common::Time time_from, time_to;
+    ReadRangeMessage(message, &current_checksum, &time_from, &time_to);
+    auto previous_time_to_it = frame_id_to_previous_time_to_.find(frame_id);
+    if (previous_time_to_it != frame_id_to_previous_time_to_.end() &&
+        previous_time_to_it->second >= time_from) {
+      if (previous_time_to_it->second >= time_to) {
+        LOG_FIRST_N(ERROR, 3) << "Sensor with frame_id \"" << frame_id
+                              << "\" is not sequential in time."
+                              << "Previous range message ends at time "
+                              << previous_time_to_it->second
+                              << ", current one at time " << time_to;
+      } else {
+        LOG_FIRST_N(WARNING, 3)
+            << "Sensor with frame_id \"" << frame_id
+            << "\" measurements overlap in time. "
+            << "Previous range message, ending at time stamp "
+            << previous_time_to_it->second
+            << ", must finish before current range message, "
+            << "which ranges from " << time_from << " to " << time_to;
+      }
+      double overlap = cartographer::common::ToSeconds(
+          previous_time_to_it->second - time_from);
+      auto it = frame_id_to_max_overlap_duration_.find(frame_id);
+      if (it == frame_id_to_max_overlap_duration_.end() ||
+          overlap > frame_id_to_max_overlap_duration_.at(frame_id)) {
+        frame_id_to_max_overlap_duration_[frame_id] = overlap;
+      }
+    }
+    frame_id_to_previous_time_to_[frame_id] = time_to;
+    if (current_checksum.first == 0) {
+      return;
+    }
+    auto it = frame_id_to_range_checksum_.find(frame_id);
+    if (it != frame_id_to_range_checksum_.end()) {
+      RangeChecksum previous_checksum = it->second;
+      if (previous_checksum == current_checksum) {
+        LOG_FIRST_N(ERROR, 3)
+            << "Sensor with frame_id \"" << frame_id
+            << "\" sends exactly the same range measurements multiple times. "
+            << "Range data at time " << current_time_stamp
+            << " equals preceding data with " << current_checksum.first
+            << " points.";
+      }
+    }
+    frame_id_to_range_checksum_[frame_id] = current_checksum;
+  }
+
+  void PrintReport() {
+    for (auto& it : frame_id_to_max_overlap_duration_) {
+      LOG(WARNING) << "Sensor with frame_id \"" << it.first
+                   << "\" range measurements have longest overlap of "
+                   << it.second << " s";
+    }
+  }
+
+ private:
+  typedef std::pair<size_t /* num_points */, Eigen::Vector4f /* points_sum */>
+      RangeChecksum;
+
+  template <typename MessageType>
+  static void ReadRangeMessage(const MessageType& message,
+                               RangeChecksum* range_checksum,
+                               cartographer::common::Time* from,
+                               cartographer::common::Time* to) {
+    auto point_cloud_time = ToPointCloudWithIntensities(message);
+    const cartographer::sensor::TimedPointCloud& point_cloud =
+        std::get<0>(point_cloud_time).points;
+    *to = std::get<1>(point_cloud_time);
+    *from = *to + cartographer::common::FromSeconds(point_cloud[0].time);
+    Eigen::Vector4f points_sum = Eigen::Vector4f::Zero();
+    for (const auto& point : point_cloud) {
+      points_sum.head<3>() += point.position;
+      points_sum[3] += point.time;
+    }
+    if (point_cloud.size() > 0) {
+      double first_point_relative_time = point_cloud[0].time;
+      double last_point_relative_time = (*point_cloud.rbegin()).time;
+      if (first_point_relative_time > 0) {
+        LOG_FIRST_N(ERROR, 1)
+            << "Sensor with frame_id \"" << message.header.frame_id
+            << "\" has range data that has positive relative time "
+            << first_point_relative_time << " s, must negative or zero.";
+      }
+      if (last_point_relative_time != 0) {
+        LOG_FIRST_N(INFO, 1)
+            << "Sensor with frame_id \"" << message.header.frame_id
+            << "\" has range data whose last point has relative time "
+            << last_point_relative_time << " s, should be zero.";
+      }
+    }
+    *range_checksum = {point_cloud.size(), points_sum};
+  }
+
+  std::map<std::string, RangeChecksum> frame_id_to_range_checksum_;
+  std::map<std::string, cartographer::common::Time>
+      frame_id_to_previous_time_to_;
+  std::map<std::string, double> frame_id_to_max_overlap_duration_;
+};
+
+void Run(const std::string& bag_filename, const bool dump_timing) {
+  rosbag::Bag bag;
+  bag.open(bag_filename, rosbag::bagmode::Read);
+  rosbag::View view(bag);
+
+  std::map<std::string, FrameProperties> frame_id_to_properties;
+  size_t message_index = 0;
+  int num_imu_messages = 0;
+  double sum_imu_acceleration = 0.;
+  RangeDataChecker range_data_checker;
+  for (const rosbag::MessageInstance& message : view) {
+    ++message_index;
+    std::string frame_id;
+    ros::Time time;
+    if (message.isType<sensor_msgs::PointCloud2>()) {
+      auto msg = message.instantiate<sensor_msgs::PointCloud2>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+      range_data_checker.CheckMessage(*msg);
+    } else if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
+      auto msg = message.instantiate<sensor_msgs::MultiEchoLaserScan>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+      range_data_checker.CheckMessage(*msg);
+    } else if (message.isType<sensor_msgs::LaserScan>()) {
+      auto msg = message.instantiate<sensor_msgs::LaserScan>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+      range_data_checker.CheckMessage(*msg);
+    } else if (message.isType<sensor_msgs::Imu>()) {
+      auto msg = message.instantiate<sensor_msgs::Imu>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+      CheckImuMessage(*msg);
+      num_imu_messages++;
+      sum_imu_acceleration += ToEigen(msg->linear_acceleration).norm();
+    } else if (message.isType<nav_msgs::Odometry>()) {
+      auto msg = message.instantiate<nav_msgs::Odometry>();
+      time = msg->header.stamp;
+      frame_id = msg->header.frame_id;
+      CheckOdometryMessage(*msg);
+    } else if (message.isType<tf2_msgs::TFMessage>()) {
+      auto msg = message.instantiate<tf2_msgs::TFMessage>();
+      CheckTfMessage(*msg);
+      continue;
+    } else {
+      continue;
+    }
+
+    bool first_packet = false;
+    if (!frame_id_to_properties.count(frame_id)) {
+      frame_id_to_properties.emplace(
+          frame_id,
+          FrameProperties{time, message.getTopic(), std::vector<float>(),
+                          dump_timing ? CreateTimingFile(frame_id) : nullptr,
+                          message.getDataType()});
+      first_packet = true;
+    }
+
+    auto& entry = frame_id_to_properties.at(frame_id);
+    if (!first_packet) {
+      const double delta_t_sec = (time - entry.last_timestamp).toSec();
+      if (delta_t_sec <= 0) {
+        LOG_FIRST_N(ERROR, 3)
+            << "Sensor with frame_id \"" << frame_id
+            << "\" jumps backwards in time, i.e. timestamps are not strictly "
+               "increasing. Make sure that the bag contains the data for each "
+               "frame_id sorted by header.stamp, i.e. the order in which they "
+               "were acquired from the sensor.";
+      }
+      entry.time_deltas.push_back(delta_t_sec);
+    }
+
+    if (entry.topic != message.getTopic()) {
+      LOG_FIRST_N(ERROR, 3)
+          << "frame_id \"" << frame_id
+          << "\" is send on multiple topics. It was seen at least on "
+          << entry.topic << " and " << message.getTopic();
+    }
+    entry.last_timestamp = time;
+
+    if (dump_timing) {
+      CHECK(entry.timing_file != nullptr);
+      (*entry.timing_file) << message_index << "\t"
+                           << message.getTime().toNSec() << "\t"
+                           << time.toNSec() << std::endl;
+    }
+
+    double duration_serialization_sensor = (time - message.getTime()).toSec();
+    if (std::abs(duration_serialization_sensor) >
+        kTimeDeltaSerializationSensorWarning) {
+      std::stringstream stream;
+      stream << "frame_id \"" << frame_id << "\" on topic "
+             << message.getTopic() << " has serialization time "
+             << message.getTime() << " but sensor time " << time
+             << " differing by " << duration_serialization_sensor << " s.";
+      if (std::abs(duration_serialization_sensor) >
+          kTimeDeltaSerializationSensorError) {
+        LOG_FIRST_N(ERROR, 3) << stream.str();
+      } else {
+        LOG_FIRST_N(WARNING, 1) << stream.str();
+      }
+    }
+  }
+  bag.close();
+
+  range_data_checker.PrintReport();
+
+  if (num_imu_messages > 0) {
+    double average_imu_acceleration = sum_imu_acceleration / num_imu_messages;
+    if (std::isnan(average_imu_acceleration) ||
+        average_imu_acceleration < kMinAverageAcceleration ||
+        average_imu_acceleration > kMaxAverageAcceleration) {
+      LOG(ERROR) << "Average IMU linear acceleration is "
+                 << average_imu_acceleration << " m/s^2,"
+                 << " expected is [" << kMinAverageAcceleration << ", "
+                 << kMaxAverageAcceleration
+                 << "] m/s^2. Linear acceleration data "
+                    "should include gravity and be given in m/s^2.";
+    }
+  }
+
+  constexpr int kNumBucketsForHistogram = 10;
+  for (const auto& entry_pair : frame_id_to_properties) {
+    const FrameProperties& frame_properties = entry_pair.second;
+    float max_time_delta =
+        *std::max_element(frame_properties.time_deltas.begin(),
+                          frame_properties.time_deltas.end());
+    if (IsPointDataType(frame_properties.data_type) &&
+        max_time_delta > kMaxGapPointsData) {
+      LOG(ERROR) << "Point data (frame_id: \"" << entry_pair.first
+                 << "\") has a large gap, largest is " << max_time_delta
+                 << " s, recommended is [0.0005, 0.05] s with no jitter.";
+    }
+    if (frame_properties.data_type ==
+            ros::message_traits::DataType<sensor_msgs::Imu>::value() &&
+        max_time_delta > kMaxGapImuData) {
+      LOG(ERROR) << "IMU data (frame_id: \"" << entry_pair.first
+                 << "\") has a large gap, largest is " << max_time_delta
+                 << " s, recommended is [0.0005, 0.005] s with no jitter.";
+    }
+
+    cartographer::common::Histogram histogram;
+    for (float time_delta : frame_properties.time_deltas) {
+      histogram.Add(time_delta);
+    }
+    LOG(INFO) << "Time delta histogram for consecutive messages on topic \""
+              << frame_properties.topic << "\" (frame_id: \""
+              << entry_pair.first << "\"):\n"
+              << histogram.ToString(kNumBucketsForHistogram);
+  }
+
+  if (dump_timing) {
+    for (const auto& entry_pair : frame_id_to_properties) {
+      entry_pair.second.timing_file->close();
+      CHECK(*entry_pair.second.timing_file)
+          << "Could not write timing information for \"" << entry_pair.first
+          << "\"";
+    }
+  }
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  FLAGS_alsologtostderr = true;
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
+  ::cartographer_ros::Run(FLAGS_bag_filename, FLAGS_dump_timing);
+}

cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -0,0 +1,243 @@
+/*
+ * 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 "cartographer_ros/sensor_bridge.h"
+
+#include "absl/memory/memory.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/time_conversion.h"
+
+namespace cartographer_ros {
+
+namespace carto = ::cartographer;
+
+using carto::transform::Rigid3d;
+
+namespace {
+
+const std::string& CheckNoLeadingSlash(const std::string& frame_id) {
+  if (frame_id.size() > 0) {
+    CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id
+                               << " should not start with a /. See 1.7 in "
+                                  "http://wiki.ros.org/tf2/Migration.";
+  }
+  return frame_id;
+}
+
+}  // namespace
+
+SensorBridge::SensorBridge(
+    const int num_subdivisions_per_laser_scan,
+    const std::string& tracking_frame,
+    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
+    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
+    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
+      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
+      trajectory_builder_(trajectory_builder) {}
+
+std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
+    const nav_msgs::Odometry::ConstPtr& msg) {
+  const carto::common::Time time = FromRos(msg->header.stamp);
+  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
+      time, CheckNoLeadingSlash(msg->child_frame_id));
+  if (sensor_to_tracking == nullptr) {
+    return nullptr;
+  }
+  return absl::make_unique<carto::sensor::OdometryData>(
+      carto::sensor::OdometryData{
+          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
+}
+
+void SensorBridge::HandleOdometryMessage(
+    const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
+  std::unique_ptr<carto::sensor::OdometryData> odometry_data =
+      ToOdometryData(msg);
+  if (odometry_data != nullptr) {
+    trajectory_builder_->AddSensorData(
+        sensor_id,
+        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
+  }
+}
+
+void SensorBridge::HandleNavSatFixMessage(
+    const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) {
+  const carto::common::Time time = FromRos(msg->header.stamp);
+  if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
+    trajectory_builder_->AddSensorData(
+        sensor_id,
+        carto::sensor::FixedFramePoseData{time, absl::optional<Rigid3d>()});
+    return;
+  }
+
+  if (!ecef_to_local_frame_.has_value()) {
+    ecef_to_local_frame_ =
+        ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude);
+    LOG(INFO) << "Using NavSatFix. Setting ecef_to_local_frame with lat = "
+              << msg->latitude << ", long = " << msg->longitude << ".";
+  }
+
+  trajectory_builder_->AddSensorData(
+      sensor_id, carto::sensor::FixedFramePoseData{
+                     time, absl::optional<Rigid3d>(Rigid3d::Translation(
+                               ecef_to_local_frame_.value() *
+                               LatLongAltToEcef(msg->latitude, msg->longitude,
+                                                msg->altitude)))});
+}
+
+void SensorBridge::HandleLandmarkMessage(
+    const std::string& sensor_id,
+    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
+  auto landmark_data = ToLandmarkData(*msg);
+
+  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
+      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));
+  if (tracking_from_landmark_sensor != nullptr) {
+    for (auto& observation : landmark_data.landmark_observations) {
+      observation.landmark_to_tracking_transform =
+          *tracking_from_landmark_sensor *
+          observation.landmark_to_tracking_transform;
+    }
+  }
+  trajectory_builder_->AddSensorData(sensor_id, landmark_data);
+}
+
+std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(
+    const sensor_msgs::Imu::ConstPtr& msg) {
+  CHECK_NE(msg->linear_acceleration_covariance[0], -1)
+      << "Your IMU data claims to not contain linear acceleration measurements "
+         "by setting linear_acceleration_covariance[0] to -1. Cartographer "
+         "requires this data to work. See "
+         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
+  CHECK_NE(msg->angular_velocity_covariance[0], -1)
+      << "Your IMU data claims to not contain angular velocity measurements "
+         "by setting angular_velocity_covariance[0] to -1. Cartographer "
+         "requires this data to work. See "
+         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
+
+  const carto::common::Time time = FromRos(msg->header.stamp);
+  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
+      time, CheckNoLeadingSlash(msg->header.frame_id));
+  if (sensor_to_tracking == nullptr) {
+    return nullptr;
+  }
+  CHECK(sensor_to_tracking->translation().norm() < 1e-5)
+      << "The IMU frame must be colocated with the tracking frame. "
+         "Transforming linear acceleration into the tracking frame will "
+         "otherwise be imprecise.";
+  return absl::make_unique<carto::sensor::ImuData>(carto::sensor::ImuData{
+      time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
+      sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
+}
+
+void SensorBridge::HandleImuMessage(const std::string& sensor_id,
+                                    const sensor_msgs::Imu::ConstPtr& msg) {
+  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
+  if (imu_data != nullptr) {
+    trajectory_builder_->AddSensorData(
+        sensor_id,
+        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
+                               imu_data->angular_velocity});
+  }
+}
+
+void SensorBridge::HandleLaserScanMessage(
+    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
+  carto::sensor::PointCloudWithIntensities point_cloud;
+  carto::common::Time time;
+  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
+  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
+}
+
+void SensorBridge::HandleMultiEchoLaserScanMessage(
+    const std::string& sensor_id,
+    const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
+  carto::sensor::PointCloudWithIntensities point_cloud;
+  carto::common::Time time;
+  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
+  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
+}
+
+void SensorBridge::HandlePointCloud2Message(
+    const std::string& sensor_id,
+    const sensor_msgs::PointCloud2::ConstPtr& msg) {
+  carto::sensor::PointCloudWithIntensities point_cloud;
+  carto::common::Time time;
+  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
+  HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
+}
+
+const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
+
+void SensorBridge::HandleLaserScan(
+    const std::string& sensor_id, const carto::common::Time time,
+    const std::string& frame_id,
+    const carto::sensor::PointCloudWithIntensities& points) {
+  if (points.points.empty()) {
+    return;
+  }
+  CHECK_LE(points.points.back().time, 0.f);
+  // TODO(gaschler): Use per-point time instead of subdivisions.
+  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
+    const size_t start_index =
+        points.points.size() * i / num_subdivisions_per_laser_scan_;
+    const size_t end_index =
+        points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
+    carto::sensor::TimedPointCloud subdivision(
+        points.points.begin() + start_index, points.points.begin() + end_index);
+    if (start_index == end_index) {
+      continue;
+    }
+    const double time_to_subdivision_end = subdivision.back().time;
+    // `subdivision_time` is the end of the measurement so sensor::Collator will
+    // send all other sensor data first.
+    const carto::common::Time subdivision_time =
+        time + carto::common::FromSeconds(time_to_subdivision_end);
+    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
+    if (it != sensor_to_previous_subdivision_time_.end() &&
+        it->second >= subdivision_time) {
+      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
+                   << sensor_id << " because previous subdivision time "
+                   << it->second << " is not before current subdivision time "
+                   << subdivision_time;
+      continue;
+    }
+    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
+    for (auto& point : subdivision) {
+      point.time -= time_to_subdivision_end;
+    }
+    CHECK_EQ(subdivision.back().time, 0.f);
+    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
+  }
+}
+
+void SensorBridge::HandleRangefinder(
+    const std::string& sensor_id, const carto::common::Time time,
+    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
+  if (!ranges.empty()) {
+    CHECK_LE(ranges.back().time, 0.f);
+  }
+  const auto sensor_to_tracking =
+      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
+  if (sensor_to_tracking != nullptr) {
+    trajectory_builder_->AddSensorData(
+        sensor_id, carto::sensor::TimedPointCloudData{
+                       time, sensor_to_tracking->translation().cast<float>(),
+                       carto::sensor::TransformTimedPointCloud(
+                           ranges, sensor_to_tracking->cast<float>())});
+  }
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -0,0 +1,99 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H
+
+#include <memory>
+
+#include "absl/types/optional.h"
+#include "cartographer/mapping/trajectory_builder_interface.h"
+#include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+#include "cartographer_ros/tf_bridge.h"
+#include "cartographer_ros_msgs/LandmarkList.h"
+#include "geometry_msgs/Transform.h"
+#include "geometry_msgs/TransformStamped.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "nav_msgs/Odometry.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/NavSatFix.h"
+#include "sensor_msgs/PointCloud2.h"
+
+namespace cartographer_ros {
+
+// Converts ROS messages into SensorData in tracking frame for the MapBuilder.
+class SensorBridge {
+ public:
+  explicit SensorBridge(
+      int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
+      double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
+      ::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);
+
+  SensorBridge(const SensorBridge&) = delete;
+  SensorBridge& operator=(const SensorBridge&) = delete;
+
+  std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
+      const nav_msgs::Odometry::ConstPtr& msg);
+  void HandleOdometryMessage(const std::string& sensor_id,
+                             const nav_msgs::Odometry::ConstPtr& msg);
+  void HandleNavSatFixMessage(const std::string& sensor_id,
+                              const sensor_msgs::NavSatFix::ConstPtr& msg);
+  void HandleLandmarkMessage(
+      const std::string& sensor_id,
+      const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
+
+  std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
+      const sensor_msgs::Imu::ConstPtr& msg);
+  void HandleImuMessage(const std::string& sensor_id,
+                        const sensor_msgs::Imu::ConstPtr& msg);
+  void HandleLaserScanMessage(const std::string& sensor_id,
+                              const sensor_msgs::LaserScan::ConstPtr& msg);
+  void HandleMultiEchoLaserScanMessage(
+      const std::string& sensor_id,
+      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
+  void HandlePointCloud2Message(const std::string& sensor_id,
+                                const sensor_msgs::PointCloud2::ConstPtr& msg);
+
+  const TfBridge& tf_bridge() const;
+
+ private:
+  void HandleLaserScan(
+      const std::string& sensor_id, ::cartographer::common::Time start_time,
+      const std::string& frame_id,
+      const ::cartographer::sensor::PointCloudWithIntensities& points);
+  void HandleRangefinder(const std::string& sensor_id,
+                         ::cartographer::common::Time time,
+                         const std::string& frame_id,
+                         const ::cartographer::sensor::TimedPointCloud& ranges);
+
+  const int num_subdivisions_per_laser_scan_;
+  std::map<std::string, cartographer::common::Time>
+      sensor_to_previous_subdivision_time_;
+  const TfBridge tf_bridge_;
+  ::cartographer::mapping::TrajectoryBuilderInterface* const
+      trajectory_builder_;
+
+  absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H

cartographer_ros/cartographer_ros/cartographer_ros/start_trajectory_main.cc

@@ -0,0 +1,126 @@
+/*
+ * 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 <string>
+#include <vector>
+
+#include "absl/memory/memory.h"
+#include "cartographer/common/configuration_file_resolver.h"
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
+#include "cartographer_ros/node_constants.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "cartographer_ros/trajectory_options.h"
+#include "cartographer_ros_msgs/StartTrajectory.h"
+#include "cartographer_ros_msgs/StatusCode.h"
+#include "cartographer_ros_msgs/TrajectoryOptions.h"
+#include "gflags/gflags.h"
+#include "ros/ros.h"
+
+DEFINE_string(configuration_directory, "",
+              "First directory in which configuration files are searched, "
+              "second is always the Cartographer installation to allow "
+              "including files from there.");
+DEFINE_string(configuration_basename, "",
+              "Basename, i.e. not containing any directory prefix, of the "
+              "configuration file.");
+
+DEFINE_string(initial_pose, "", "Starting pose of a new trajectory");
+
+namespace cartographer_ros {
+namespace {
+
+TrajectoryOptions LoadOptions() {
+  auto file_resolver =
+      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
+          std::vector<std::string>{FLAGS_configuration_directory});
+  const std::string code =
+      file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
+  auto lua_parameter_dictionary =
+      cartographer::common::LuaParameterDictionary::NonReferenceCounted(
+          code, std::move(file_resolver));
+  if (!FLAGS_initial_pose.empty()) {
+    auto initial_trajectory_pose_file_resolver =
+        absl::make_unique<cartographer::common::ConfigurationFileResolver>(
+            std::vector<std::string>{FLAGS_configuration_directory});
+    auto initial_trajectory_pose =
+        cartographer::common::LuaParameterDictionary::NonReferenceCounted(
+            "return " + FLAGS_initial_pose,
+            std::move(initial_trajectory_pose_file_resolver));
+    return CreateTrajectoryOptions(lua_parameter_dictionary.get(),
+                                   initial_trajectory_pose.get());
+  } else {
+    return CreateTrajectoryOptions(lua_parameter_dictionary.get());
+  }
+}
+
+bool Run() {
+  ros::NodeHandle node_handle;
+  ros::ServiceClient client =
+      node_handle.serviceClient<cartographer_ros_msgs::StartTrajectory>(
+          kStartTrajectoryServiceName);
+  cartographer_ros_msgs::StartTrajectory srv;
+  srv.request.options = ToRosMessage(LoadOptions());
+  srv.request.topics.laser_scan_topic = node_handle.resolveName(
+      kLaserScanTopic, true /* apply topic remapping */);
+  srv.request.topics.multi_echo_laser_scan_topic =
+      node_handle.resolveName(kMultiEchoLaserScanTopic, true);
+  srv.request.topics.point_cloud2_topic =
+      node_handle.resolveName(kPointCloud2Topic, true);
+  srv.request.topics.imu_topic = node_handle.resolveName(kImuTopic, true);
+  srv.request.topics.odometry_topic =
+      node_handle.resolveName(kOdometryTopic, true);
+
+  if (!client.call(srv)) {
+    LOG(ERROR) << "Failed to call " << kStartTrajectoryServiceName << ".";
+    return false;
+  }
+  if (srv.response.status.code != cartographer_ros_msgs::StatusCode::OK) {
+    LOG(ERROR) << "Error starting trajectory - message: '"
+               << srv.response.status.message
+               << "' (status code: " << std::to_string(srv.response.status.code)
+               << ").";
+    return false;
+  }
+  LOG(INFO) << "Started trajectory " << srv.response.trajectory_id;
+  return true;
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::SetUsageMessage(
+      "\n\n"
+      "Convenience tool around the start_trajectory service. This takes a Lua "
+      "file that is accepted by the node as well and starts a new trajectory "
+      "using its settings.\n");
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_configuration_directory.empty())
+      << "-configuration_directory is missing.";
+  CHECK(!FLAGS_configuration_basename.empty())
+      << "-configuration_basename is missing.";
+
+  ::ros::init(argc, argv, "cartographer_start_trajectory");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+  int exit_code = cartographer_ros::Run() ? 0 : 1;
+  ::ros::shutdown();
+  return exit_code;
+}

cartographer_ros/cartographer_ros/cartographer_ros/submap.cc

@@ -0,0 +1,55 @@
+/*
+ * 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 "cartographer_ros/submap.h"
+
+#include "absl/memory/memory.h"
+#include "cartographer/common/port.h"
+#include "cartographer/transform/transform.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros_msgs/StatusCode.h"
+#include "cartographer_ros_msgs/SubmapQuery.h"
+
+namespace cartographer_ros {
+
+std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
+    const ::cartographer::mapping::SubmapId& submap_id,
+    ros::ServiceClient* client) {
+  ::cartographer_ros_msgs::SubmapQuery srv;
+  srv.request.trajectory_id = submap_id.trajectory_id;
+  srv.request.submap_index = submap_id.submap_index;
+  if (!client->call(srv) ||
+      srv.response.status.code != ::cartographer_ros_msgs::StatusCode::OK) {
+    return nullptr;
+  }
+  if (srv.response.textures.empty()) {
+    return nullptr;
+  }
+  auto response = absl::make_unique<::cartographer::io::SubmapTextures>();
+  response->version = srv.response.submap_version;
+  for (const auto& texture : srv.response.textures) {
+    const std::string compressed_cells(texture.cells.begin(),
+                                       texture.cells.end());
+    response->textures.emplace_back(::cartographer::io::SubmapTexture{
+        ::cartographer::io::UnpackTextureData(compressed_cells, texture.width,
+                                              texture.height),
+        texture.width, texture.height, texture.resolution,
+        ToRigid3d(texture.slice_pose)});
+  }
+  return response;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/submap.h

@@ -0,0 +1,40 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "cartographer/io/image.h"
+#include "cartographer/io/submap_painter.h"
+#include "cartographer/mapping/id.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "ros/ros.h"
+
+namespace cartographer_ros {
+
+// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr'
+// on error.
+std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
+    const ::cartographer::mapping::SubmapId& submap_id,
+    ros::ServiceClient* client);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H

cartographer_ros/cartographer_ros/cartographer_ros/tf_bridge.cc

@@ -0,0 +1,57 @@
+/*
+ * 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 "absl/memory/memory.h"
+
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/tf_bridge.h"
+
+namespace cartographer_ros {
+
+TfBridge::TfBridge(const std::string& tracking_frame,
+                   const double lookup_transform_timeout_sec,
+                   const tf2_ros::Buffer* buffer)
+    : tracking_frame_(tracking_frame),
+      lookup_transform_timeout_sec_(lookup_transform_timeout_sec),
+      buffer_(buffer) {}
+
+std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking(
+    const ::cartographer::common::Time time,
+    const std::string& frame_id) const {
+  ::ros::Duration timeout(lookup_transform_timeout_sec_);
+  std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking;
+  try {
+    const ::ros::Time latest_tf_time =
+        buffer_
+            ->lookupTransform(tracking_frame_, frame_id, ::ros::Time(0.),
+                              timeout)
+            .header.stamp;
+    const ::ros::Time requested_time = ToRos(time);
+    if (latest_tf_time >= requested_time) {
+      // We already have newer data, so we do not wait. Otherwise, we would wait
+      // for the full 'timeout' even if we ask for data that is too old.
+      timeout = ::ros::Duration(0.);
+    }
+    return absl::make_unique<::cartographer::transform::Rigid3d>(
+        ToRigid3d(buffer_->lookupTransform(tracking_frame_, frame_id,
+                                           requested_time, timeout)));
+  } catch (const tf2::TransformException& ex) {
+    LOG(WARNING) << ex.what();
+  }
+  return nullptr;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/tf_bridge.h

@@ -0,0 +1,51 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H
+
+#include <memory>
+
+#include "cartographer/transform/rigid_transform.h"
+#include "tf2_ros/buffer.h"
+
+#include "cartographer_ros/time_conversion.h"
+
+namespace cartographer_ros {
+
+class TfBridge {
+ public:
+  TfBridge(const std::string& tracking_frame,
+           double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer);
+  ~TfBridge() {}
+
+  TfBridge(const TfBridge&) = delete;
+  TfBridge& operator=(const TfBridge&) = delete;
+
+  // Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at
+  // 'time'.
+  std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking(
+      ::cartographer::common::Time time, const std::string& frame_id) const;
+
+ private:
+  const std::string tracking_frame_;
+  const double lookup_transform_timeout_sec_;
+  const tf2_ros::Buffer* const buffer_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H

cartographer_ros/cartographer_ros/cartographer_ros/time_conversion.cc

@@ -0,0 +1,47 @@
+/*
+ * 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 "cartographer_ros/time_conversion.h"
+
+#include "cartographer/common/time.h"
+#include "ros/ros.h"
+
+namespace cartographer_ros {
+
+::ros::Time ToRos(::cartographer::common::Time time) {
+  int64_t uts_timestamp = ::cartographer::common::ToUniversal(time);
+  int64_t ns_since_unix_epoch =
+      (uts_timestamp -
+       ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds *
+           10000000ll) *
+      100ll;
+  ::ros::Time ros_time;
+  ros_time.fromNSec(ns_since_unix_epoch);
+  return ros_time;
+}
+
+// TODO(pedrofernandez): Write test.
+::cartographer::common::Time FromRos(const ::ros::Time& time) {
+  // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000",
+  // exactly 719162 days before the Unix epoch.
+  return ::cartographer::common::FromUniversal(
+      (time.sec +
+       ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds) *
+          10000000ll +
+      (time.nsec + 50) / 100);  // + 50 to get the rounding correct.
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/time_conversion.h

@@ -0,0 +1,31 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H
+
+#include "cartographer/common/time.h"
+#include "ros/ros.h"
+
+namespace cartographer_ros {
+
+::ros::Time ToRos(::cartographer::common::Time time);
+
+::cartographer::common::Time FromRos(const ::ros::Time& time);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H

cartographer_ros/cartographer_ros/cartographer_ros/time_conversion_test.cc

@@ -0,0 +1,44 @@
+/*
+ * 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 "cartographer_ros/time_conversion.h"
+
+#include <chrono>
+
+#include "cartographer/common/time.h"
+#include "gtest/gtest.h"
+#include "ros/ros.h"
+
+namespace cartographer_ros {
+namespace {
+
+TEST(TimeConversion, testToRos) {
+  std::vector<int64_t> values = {0, 1469091375, 1466481821, 1462101382,
+                                 1468238899};
+  for (int64_t seconds_since_epoch : values) {
+    ::ros::Time ros_now;
+    ros_now.fromSec(seconds_since_epoch);
+    ::cartographer::common::Time cartographer_now(
+        ::cartographer::common::FromSeconds(
+            seconds_since_epoch +
+            ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds));
+    EXPECT_EQ(cartographer_now, ::cartographer_ros::FromRos(ros_now));
+    EXPECT_EQ(ros_now, ::cartographer_ros::ToRos(cartographer_now));
+  }
+}
+
+}  // namespace
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -0,0 +1,163 @@
+/*
+ * 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 "cartographer_ros/trajectory_options.h"
+
+#include "cartographer/mapping/trajectory_builder_interface.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+#include "cartographer_ros/time_conversion.h"
+#include "glog/logging.h"
+
+namespace cartographer_ros {
+
+namespace {
+
+void CheckTrajectoryOptions(const TrajectoryOptions& options) {
+  CHECK_GE(options.num_subdivisions_per_laser_scan, 1);
+  CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans +
+               options.num_point_clouds,
+           1)
+      << "Configuration error: 'num_laser_scans', "
+         "'num_multi_echo_laser_scans' and 'num_point_clouds' are "
+         "all zero, but at least one is required.";
+}
+
+}  // namespace
+
+TrajectoryOptions CreateTrajectoryOptions(
+    ::cartographer::common::LuaParameterDictionary* const
+        lua_parameter_dictionary) {
+  TrajectoryOptions options;
+  options.trajectory_builder_options =
+      ::cartographer::mapping::CreateTrajectoryBuilderOptions(
+          lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
+  options.tracking_frame =
+      lua_parameter_dictionary->GetString("tracking_frame");
+  options.published_frame =
+      lua_parameter_dictionary->GetString("published_frame");
+  options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
+  options.provide_odom_frame =
+      lua_parameter_dictionary->GetBool("provide_odom_frame");
+  options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
+  options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat");
+  options.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks");
+  options.publish_frame_projected_to_2d =
+      lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d");
+  options.num_laser_scans =
+      lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans");
+  options.num_multi_echo_laser_scans =
+      lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans");
+  options.num_subdivisions_per_laser_scan =
+      lua_parameter_dictionary->GetNonNegativeInt(
+          "num_subdivisions_per_laser_scan");
+  options.num_point_clouds =
+      lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
+  options.rangefinder_sampling_ratio =
+      lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio");
+  options.odometry_sampling_ratio =
+      lua_parameter_dictionary->GetDouble("odometry_sampling_ratio");
+  options.fixed_frame_pose_sampling_ratio =
+      lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio");
+  options.imu_sampling_ratio =
+      lua_parameter_dictionary->GetDouble("imu_sampling_ratio");
+  options.landmarks_sampling_ratio =
+      lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio");
+  CheckTrajectoryOptions(options);
+  return options;
+}
+
+TrajectoryOptions CreateTrajectoryOptions(
+    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary,
+    ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose) {
+  TrajectoryOptions options = CreateTrajectoryOptions(lua_parameter_dictionary);
+  *options.trajectory_builder_options.mutable_initial_trajectory_pose() =
+      CreateInitialTrajectoryPose(initial_trajectory_pose);
+  return options;
+}
+
+::cartographer::mapping::proto::InitialTrajectoryPose
+CreateInitialTrajectoryPose(
+    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary) {
+  ::cartographer::mapping::proto::InitialTrajectoryPose pose;
+  pose.set_to_trajectory_id(
+      lua_parameter_dictionary->GetNonNegativeInt("to_trajectory_id"));
+  *pose.mutable_relative_pose() =
+      cartographer::transform::ToProto(cartographer::transform::FromDictionary(
+          lua_parameter_dictionary->GetDictionary("relative_pose").get()));
+  pose.set_timestamp(
+      lua_parameter_dictionary->HasKey("timestamp")
+          ? lua_parameter_dictionary->GetNonNegativeInt("timestamp")
+          : cartographer::common::ToUniversal(FromRos(ros::Time::now())));
+  return pose;
+}
+
+bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
+                    TrajectoryOptions* options) {
+  options->tracking_frame = msg.tracking_frame;
+  options->published_frame = msg.published_frame;
+  options->odom_frame = msg.odom_frame;
+  options->provide_odom_frame = msg.provide_odom_frame;
+  options->use_odometry = msg.use_odometry;
+  options->use_nav_sat = msg.use_nav_sat;
+  options->use_landmarks = msg.use_landmarks;
+  options->publish_frame_projected_to_2d = msg.publish_frame_projected_to_2d;
+  options->num_laser_scans = msg.num_laser_scans;
+  options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans;
+  options->num_subdivisions_per_laser_scan =
+      msg.num_subdivisions_per_laser_scan;
+  options->num_point_clouds = msg.num_point_clouds;
+  options->rangefinder_sampling_ratio = msg.rangefinder_sampling_ratio;
+  options->odometry_sampling_ratio = msg.odometry_sampling_ratio;
+  options->fixed_frame_pose_sampling_ratio =
+      msg.fixed_frame_pose_sampling_ratio;
+  options->imu_sampling_ratio = msg.imu_sampling_ratio;
+  options->landmarks_sampling_ratio = msg.landmarks_sampling_ratio;
+  if (!options->trajectory_builder_options.ParseFromString(
+          msg.trajectory_builder_options_proto)) {
+    LOG(ERROR) << "Failed to parse protobuf";
+    return false;
+  }
+  CheckTrajectoryOptions(*options);
+  return true;
+}
+
+cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
+    const TrajectoryOptions& options) {
+  cartographer_ros_msgs::TrajectoryOptions msg;
+  msg.tracking_frame = options.tracking_frame;
+  msg.published_frame = options.published_frame;
+  msg.odom_frame = options.odom_frame;
+  msg.provide_odom_frame = options.provide_odom_frame;
+  msg.use_odometry = options.use_odometry;
+  msg.use_nav_sat = options.use_nav_sat;
+  msg.use_landmarks = options.use_landmarks;
+  msg.publish_frame_projected_to_2d = options.publish_frame_projected_to_2d;
+  msg.num_laser_scans = options.num_laser_scans;
+  msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans;
+  msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan;
+  msg.num_point_clouds = options.num_point_clouds;
+  msg.rangefinder_sampling_ratio = options.rangefinder_sampling_ratio;
+  msg.odometry_sampling_ratio = options.odometry_sampling_ratio;
+  msg.fixed_frame_pose_sampling_ratio = options.fixed_frame_pose_sampling_ratio;
+  msg.imu_sampling_ratio = options.imu_sampling_ratio;
+  msg.landmarks_sampling_ratio = options.landmarks_sampling_ratio;
+  options.trajectory_builder_options.SerializeToString(
+      &msg.trajectory_builder_options_proto);
+  return msg;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/trajectory_options.h

@@ -0,0 +1,72 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H
+
+#include <string>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
+#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+#include "cartographer_ros_msgs/TrajectoryOptions.h"
+
+namespace cartographer_ros {
+
+struct TrajectoryOptions {
+  ::cartographer::mapping::proto::TrajectoryBuilderOptions
+      trajectory_builder_options;
+  std::string tracking_frame;
+  std::string published_frame;
+  std::string odom_frame;
+  bool provide_odom_frame;
+  bool use_odometry;
+  bool use_nav_sat;
+  bool use_landmarks;
+  bool publish_frame_projected_to_2d;
+  int num_laser_scans;
+  int num_multi_echo_laser_scans;
+  int num_subdivisions_per_laser_scan;
+  int num_point_clouds;
+  double rangefinder_sampling_ratio;
+  double odometry_sampling_ratio;
+  double fixed_frame_pose_sampling_ratio;
+  double imu_sampling_ratio;
+  double landmarks_sampling_ratio;
+};
+
+::cartographer::mapping::proto::InitialTrajectoryPose
+CreateInitialTrajectoryPose(
+    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
+
+TrajectoryOptions CreateTrajectoryOptions(
+    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
+
+TrajectoryOptions CreateTrajectoryOptions(
+    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary,
+    ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose);
+
+// Try to convert 'msg' into 'options'. Returns false on failure.
+bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
+                    TrajectoryOptions* options);
+
+// Converts 'trajectory_options' into a ROS message.
+cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
+    const TrajectoryOptions& trajectory_options);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H

cartographer_ros/cartographer_ros/cartographer_ros/urdf_reader.cc

@@ -0,0 +1,59 @@
+/*
+ * 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 "cartographer_ros/urdf_reader.h"
+
+#include <string>
+#include <vector>
+
+#include "cartographer_ros/msg_conversion.h"
+#include "urdf/model.h"
+
+namespace cartographer_ros {
+
+std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
+    const std::string& urdf_filename, tf2_ros::Buffer* const tf_buffer) {
+  urdf::Model model;
+  CHECK(model.initFile(urdf_filename));
+#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
+  std::vector<urdf::LinkSharedPtr> links;
+#else
+  std::vector<boost::shared_ptr<urdf::Link> > links;
+#endif
+  model.getLinks(links);
+  std::vector<geometry_msgs::TransformStamped> transforms;
+  for (const auto& link : links) {
+    if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) {
+      continue;
+    }
+
+    const urdf::Pose& pose =
+        link->parent_joint->parent_to_joint_origin_transform;
+    geometry_msgs::TransformStamped transform;
+    transform.transform =
+        ToGeometryMsgTransform(cartographer::transform::Rigid3d(
+            Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z),
+            Eigen::Quaterniond(pose.rotation.w, pose.rotation.x,
+                               pose.rotation.y, pose.rotation.z)));
+    transform.child_frame_id = link->name;
+    transform.header.frame_id = link->getParent()->name;
+    tf_buffer->setTransform(transform, "urdf", true /* is_static */);
+    transforms.push_back(transform);
+  }
+  return transforms;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/urdf_reader.h

@@ -0,0 +1,32 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H
+#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H
+
+#include <vector>
+
+#include "cartographer/common/port.h"
+#include "tf2_ros/buffer.h"
+
+namespace cartographer_ros {
+
+std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
+    const std::string& urdf_filename, tf2_ros::Buffer* tf_buffer);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H

cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua

@@ -0,0 +1,131 @@
+-- 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.
+
+-- WARNING: we create a lot of X-Rays of a potentially large space in this
+-- pipeline. For example, running over the
+-- cartographer_paper_deutsches_museum.bag requires ~25GiB of memory. You can
+-- reduce this by writing fewer X-Rays or upping VOXEL_SIZE - which is the size
+-- of a pixel in a X-Ray.
+VOXEL_SIZE = 5e-2
+
+include "transform.lua"
+
+options = {
+  tracking_frame = "base_link",
+  pipeline = {
+    {
+      action = "min_max_range_filter",
+      min_range = 1.,
+      max_range = 60.,
+    },
+    {
+      action = "dump_num_points",
+    },
+
+    -- Gray X-Rays. These only use geometry to color pixels.
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all",
+      transform = XZ_TRANSFORM,
+    },
+
+    -- We now use the intensities to color our points. We apply a linear
+    -- transform to clamp our intensity values into [0, 255] and then use this
+    -- value for RGB of our points. Every stage in the pipeline after this now
+    -- receives colored points.
+    --
+    -- We write xrays again. These now use geometry and the intensities to
+    -- color pixels - they look quite similar, just a little lighter.
+    {
+      action = "intensity_to_color",
+      min_intensity = 0.,
+      max_intensity = 4095.,
+    },
+
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all_intensity",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all_intensity",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all_intensity",
+      transform = XZ_TRANSFORM,
+    },
+
+    -- We also write a PLY file at this stage, because gray points look good.
+    -- The points in the PLY can be visualized using
+    -- https://github.com/googlecartographer/point_cloud_viewer.
+    {
+      action = "write_ply",
+      filename = "points.ply",
+    },
+
+    -- Now we recolor our points by frame and write another batch of X-Rays. It
+    -- is visible in them what was seen by the horizontal and the vertical
+    -- laser.
+    {
+      action = "color_points",
+      frame_id = "horizontal_laser_link",
+      color = { 255., 0., 0. },
+    },
+    {
+      action = "color_points",
+      frame_id = "vertical_laser_link",
+      color = { 0., 255., 0. },
+    },
+
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all_color",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all_color",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all_color",
+      transform = XZ_TRANSFORM,
+    },
+  }
+}
+
+return options

cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_2d_ci.lua

@@ -0,0 +1,56 @@
+-- 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.
+
+-- WARNING: we create a lot of X-Rays of a potentially large space in this
+-- pipeline. For example, running over the
+-- cartographer_paper_deutsches_museum.bag requires ~25GiB of memory. You can
+-- reduce this by writing fewer X-Rays or upping VOXEL_SIZE - which is the size
+-- of a pixel in a X-Ray.
+VOXEL_SIZE = 5e-2
+
+include "transform.lua"
+
+options = {
+  tracking_frame = "base_link",
+  pipeline = {
+    {
+      action = "min_max_range_filter",
+      min_range = 1.,
+      max_range = 60.,
+    },
+    {
+      action = "dump_num_points",
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all",
+      transform = XZ_TRANSFORM,
+    },
+  }
+}
+
+return options

cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua

@@ -0,0 +1,86 @@
+-- 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.
+
+VOXEL_SIZE = 5e-2
+
+include "transform.lua"
+
+options = {
+  tracking_frame = "base_link",
+  pipeline = {
+    {
+      action = "min_max_range_filter",
+      min_range = 1.,
+      max_range = 60.,
+    },
+    {
+      action = "dump_num_points",
+    },
+
+    -- Gray X-Rays. These only use geometry to color pixels.
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all",
+      transform = XZ_TRANSFORM,
+    },
+
+    -- Now we recolor our points by frame and write another batch of X-Rays. It
+    -- is visible in them what was seen by the horizontal and the vertical
+    -- laser.
+    {
+      action = "color_points",
+      frame_id = "horizontal_vlp16_link",
+      color = { 255., 0., 0. },
+    },
+    {
+      action = "color_points",
+      frame_id = "vertical_vlp16_link",
+      color = { 0., 255., 0. },
+    },
+
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all_color",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all_color",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all_color",
+      transform = XZ_TRANSFORM,
+    },
+  }
+}
+
+return options

cartographer_ros/cartographer_ros/configuration_files/assets_writer_ros_map.lua

@@ -0,0 +1,36 @@
+-- Copyright 2018 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.
+
+options = {
+  tracking_frame = "base_link",
+  pipeline = {
+    {
+      action = "min_max_range_filter",
+      min_range = 1.,
+      max_range = 60.,
+    },
+    {
+      action = "write_ros_map",
+      range_data_inserter = {
+        insert_free_space = true,
+        hit_probability = 0.55,
+        miss_probability = 0.49,
+      },
+      filestem = "map",
+      resolution = 0.05,
+    }
+  }
+}
+
+return options

cartographer_ros/cartographer_ros/configuration_files/backpack_2d.lua

@@ -0,0 +1,49 @@
+-- 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 "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "base_link",
+  published_frame = "base_link",
+  odom_frame = "odom",
+  provide_odom_frame = true,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = false,
+  use_nav_sat = false,
+  use_landmarks = false,
+  num_laser_scans = 0,
+  num_multi_echo_laser_scans = 1,
+  num_subdivisions_per_laser_scan = 10,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
+
+return options

cartographer_ros/cartographer_ros/configuration_files/backpack_2d_localization.lua

@@ -0,0 +1,25 @@
+-- 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_trimmer = {
+  max_submaps_to_keep = 3,
+}
+POSE_GRAPH.optimize_every_n_nodes = 10
+POSE_GRAPH.constraint_builder.sampling_ratio=0.2
+POSE_GRAPH.constraint_builder.min_score=0.75
+POSE_GRAPH.global_sampling_ratio=0.002
+
+return options

cartographer_ros/cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua

@@ -0,0 +1,27 @@
+-- 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_localization.lua"
+
+-- output map to base_link for evaluation
+options.provide_odom_frame = false
+POSE_GRAPH.optimization_problem.log_solver_summary = true
+
+-- fast localization
+MAP_BUILDER.num_background_threads = 12
+POSE_GRAPH.constraint_builder.sampling_ratio = 0.5 * POSE_GRAPH.constraint_builder.sampling_ratio
+POSE_GRAPH.global_sampling_ratio = 0.1 * POSE_GRAPH.global_sampling_ratio
+POSE_GRAPH.max_num_final_iterations = 1
+
+return options

cartographer_ros/cartographer_ros/configuration_files/backpack_2d_server.lua

@@ -0,0 +1,19 @@
+-- Copyright 2018 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 "map_builder_server.lua"
+
+MAP_BUILDER_SERVER.map_builder.use_trajectory_builder_2d = true
+
+return MAP_BUILDER_SERVER

cartographer_ros/cartographer_ros/configuration_files/backpack_3d.lua

@@ -0,0 +1,57 @@
+-- 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 "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "base_link",
+  published_frame = "base_link",
+  odom_frame = "odom",
+  provide_odom_frame = true,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = false,
+  use_nav_sat = false,
+  use_landmarks = false,
+  num_laser_scans = 0,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 2,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160
+
+MAP_BUILDER.use_trajectory_builder_3d = true
+MAP_BUILDER.num_background_threads = 7
+POSE_GRAPH.optimization_problem.huber_scale = 5e2
+POSE_GRAPH.optimize_every_n_nodes = 320
+POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
+POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
+POSE_GRAPH.constraint_builder.min_score = 0.62
+POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
+
+return options

cartographer_ros/cartographer_ros/configuration_files/backpack_3d_localization.lua

@@ -0,0 +1,22 @@
+-- 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_3d.lua"
+
+TRAJECTORY_BUILDER.pure_localization_trimmer = {
+  max_submaps_to_keep = 3,
+}
+POSE_GRAPH.optimize_every_n_nodes = 100
+
+return options

cartographer_ros/cartographer_ros/configuration_files/demo_sick.lua

@@ -0,0 +1,56 @@
+-- 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 "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "laser",
+  published_frame = "laser",
+  odom_frame = "odom",
+  provide_odom_frame = false,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = false,
+  use_nav_sat = false,
+  use_landmarks = false,
+  num_laser_scans = 1,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 3e-2,
+  trajectory_publish_period_sec = 3e-2,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+MAP_BUILDER.num_background_threads=6
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+TRAJECTORY_BUILDER_2D.use_imu_data = false
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
+
+POSE_GRAPH.optimization_problem.huber_scale = 1e2
+
+return options

cartographer_ros/cartographer_ros/configuration_files/demo_sick_location.lua

@@ -0,0 +1,68 @@
+-- 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 "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "laser",
+  published_frame = "laser",
+  odom_frame = "odom",
+  provide_odom_frame = false,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = false,
+  use_nav_sat = false,
+  use_landmarks = false,
+  num_laser_scans = 1,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 3e-2,
+  trajectory_publish_period_sec = 3e-2,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+MAP_BUILDER.num_background_threads=6
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+TRAJECTORY_BUILDER_2D.use_imu_data = false
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
+
+POSE_GRAPH.optimization_problem.huber_scale = 1e2
+
+
+TRAJECTORY_BUILDER.pure_localization_trimmer = {
+  max_submaps_to_keep = 3,
+}
+
+POSE_GRAPH.constraint_builder.sampling_ratio=0.2
+POSE_GRAPH.constraint_builder.min_score=0.75
+POSE_GRAPH.global_sampling_ratio=0.002
+
+
+POSE_GRAPH.optimize_every_n_nodes = 1
+
+return options

cartographer_ros/cartographer_ros/configuration_files/pr2.lua

@@ -0,0 +1,55 @@
+-- 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 "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "base_footprint",
+  published_frame = "base_footprint",
+  odom_frame = "odom",
+  provide_odom_frame = true,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = false,
+  use_nav_sat = false,
+  use_landmarks = false,
+  num_laser_scans = 1,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+TRAJECTORY_BUILDER_2D.use_imu_data = false
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
+
+POSE_GRAPH.optimization_problem.huber_scale = 1e2
+
+return options

cartographer_ros/cartographer_ros/configuration_files/revo_lds.lua

@@ -0,0 +1,52 @@
+-- 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 "trajectory_builder.lua"
+include "map_builder.lua"
+options = {
+  map_builder = MAP_BUILDER,
+  sensor_bridge={
+  horizontal_laser_min_range = 0.3,
+  horizontal_laser_max_range = 8.,
+  horizontal_laser_missing_echo_ray_length = 1.2,
+  constant_odometry_translational_variance = 0.,
+  constant_odometry_rotational_variance = 0.,
+},
+  map_frame = "map",
+  tracking_frame = "laser_frame",
+  published_frame = "laser_frame",
+  odom_frame = "odom",
+  provide_odom_frame = true,
+  use_odometry_data = false,
+  use_laser_scan = true,
+  use_constant_odometry_variance = false,
+  constant_odometry_translational_variance = 0.,
+  constant_odometry_rotational_variance = 0.,
+  use_horizontal_laser = true,
+  use_horizontal_multi_echo_laser = false,
+  horizontal_laser_min_range = 0.3,
+  horizontal_laser_max_range = 8.,
+  horizontal_laser_missing_echo_ray_length = 1.,
+  num_lasers_3d = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+}
+ 
+MAP_BUILDER.use_trajectory_builder_2d = true
+TRAJECTORY_BUILDER_2D.use_imu_data = false
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2
+ 
+return options
+

cartographer_ros/cartographer_ros/configuration_files/taurob_tracker.lua

@@ -0,0 +1,60 @@
+-- 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 "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "base_link",
+  published_frame = "odom",
+  odom_frame = "odom",
+  provide_odom_frame = false,
+  publish_frame_projected_to_2d = false,
+  use_pose_extrapolator = true,
+  use_odometry = true,
+  use_nav_sat = false,
+  use_landmarks = false,
+  num_laser_scans = 1,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+  rangefinder_sampling_ratio = 1.,
+  odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
+  imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
+}
+
+TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 180
+TRAJECTORY_BUILDER_3D.min_range = 0.5
+TRAJECTORY_BUILDER_3D.max_range = 20.
+TRAJECTORY_BUILDER_3D.submaps.num_range_data = 40.
+
+MAP_BUILDER.use_trajectory_builder_3d = true
+MAP_BUILDER.num_background_threads = 7
+POSE_GRAPH.optimization_problem.huber_scale = 5e2
+POSE_GRAPH.optimize_every_n_nodes = 40
+POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
+POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
+POSE_GRAPH.constraint_builder.min_score = 0.62
+POSE_GRAPH.constraint_builder.log_matches = true
+
+return options

cartographer_ros/cartographer_ros/configuration_files/transform.lua

@@ -0,0 +1,28 @@
+-- Copyright 2017 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.
+
+XY_TRANSFORM =  {
+  translation = { 0., 0., 0. },
+  rotation = { 0., -math.pi / 2., 0., },
+}
+
+XZ_TRANSFORM =  {
+  translation = { 0., 0., 0. },
+  rotation = { 0. , 0., -math.pi / 2, },
+}
+
+YZ_TRANSFORM =  {
+  translation = { 0., 0., 0. },
+  rotation = { 0. , 0., math.pi, },
+}

cartographer_ros/cartographer_ros/configuration_files/visualize_pbstream.lua

@@ -0,0 +1,21 @@
+-- Copyright 2018 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"
+
+POSE_GRAPH.constraint_builder.sampling_ratio = 0
+POSE_GRAPH.global_sampling_ratio = 0
+POSE_GRAPH.optimize_every_n_nodes = 0
+
+return options