From acc2143ff7975b48ce91de701d16fe7c894e7870 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Fri, 5 Jan 2018 11:35:42 +0100
Subject: [PATCH] Implement offline gRPC bridge. (#645)

---
 .../cartographer_ros/CMakeLists.txt           | 11 +++
 .../offline_node_grpc_main.cc                 | 68 +++++++++++++++++++
 .../cartographer_ros/offline_node_main.cc     |  2 +-
 3 files changed, 80 insertions(+), 1 deletion(-)
 create mode 100644 cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc

diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt
index 0755e2d..5b9bc91 100644
--- a/cartographer_ros/cartographer_ros/CMakeLists.txt
+++ b/cartographer_ros/cartographer_ros/CMakeLists.txt
@@ -104,6 +104,17 @@ if (${BUILD_GRPC})
     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}
diff --git a/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc b/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc
new file mode 100644
index 0000000..a103bae
--- /dev/null
+++ b/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc
@@ -0,0 +1,68 @@
+/*
+ * 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_grpc/mapping/map_builder_stub.h"
+#include "cartographer_ros/node.h"
+#include "cartographer_ros/offline_node.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "cartographer_ros/split_string.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(bag_filenames, "", "Comma-separated list of bags to process.");
+DEFINE_string(server_address, "localhost:50051",
+              "gRPC server address to "
+              "stream the sensor data to.");
+
+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_bag_filenames.empty()) << "-bag_filenames is missing.";
+
+  ::ros::init(argc, argv, "cartographer_grpc_offline_node");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+
+  cartographer_ros::NodeOptions node_options;
+  cartographer_ros::TrajectoryOptions trajectory_options;
+  std::tie(node_options, trajectory_options) = cartographer_ros::LoadOptions(
+      FLAGS_configuration_directory, FLAGS_configuration_basename);
+
+  // 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 = cartographer::common::make_unique<
+      ::cartographer_grpc::mapping::MapBuilderStub>(FLAGS_server_address);
+
+  cartographer_ros::RunOfflineNode(
+      std::move(map_builder), node_options, trajectory_options,
+      cartographer_ros::SplitString(FLAGS_bag_filenames, ','));
+
+  ::ros::shutdown();
+}
diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc
index 4156667..985d37b 100644
--- a/cartographer_ros/cartographer_ros/offline_node_main.cc
+++ b/cartographer_ros/cartographer_ros/offline_node_main.cc
@@ -1,5 +1,5 @@
 /*
- * Copyright 2016 The Cartographer Authors
+ * 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.