From e0faf7094eb937bf236637104db62f97fb3f2217 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Wed, 20 Dec 2017 09:46:54 +0100
Subject: [PATCH] Change MapBuilderServer::LocalSlamSubscriptionCallback (#776)

PAIR=gaschler
---
 .../receive_local_slam_results_handler.h      | 35 ++++++++-----------
 cartographer_grpc/map_builder_server.cc       |  5 +--
 cartographer_grpc/map_builder_server.h        | 15 ++++----
 3 files changed, 27 insertions(+), 28 deletions(-)

diff --git a/cartographer_grpc/handlers/receive_local_slam_results_handler.h b/cartographer_grpc/handlers/receive_local_slam_results_handler.h
index 6137527..03a431e 100644
--- a/cartographer_grpc/handlers/receive_local_slam_results_handler.h
+++ b/cartographer_grpc/handlers/receive_local_slam_results_handler.h
@@ -37,14 +37,9 @@ class ReceiveLocalSlamResultsHandler
         GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
             ->SubscribeLocalSlamResults(
                 request.trajectory_id(),
-                [writer](int trajectory_id, cartographer::common::Time time,
-                         cartographer::transform::Rigid3d local_pose,
-                         std::shared_ptr<const cartographer::sensor::RangeData>
-                             range_data,
-                         std::unique_ptr<const cartographer::mapping::NodeId>
-                             node_id) {
-                  writer(GenerateResponse(trajectory_id, time, local_pose,
-                                          range_data, std::move(node_id)));
+                [writer](std::unique_ptr<MapBuilderServer::LocalSlamResult>
+                             local_slam_result) {
+                  writer(GenerateResponse(std::move(local_slam_result)));
                 });
 
     subscription_id_ =
@@ -54,23 +49,23 @@ class ReceiveLocalSlamResultsHandler
 
   static std::unique_ptr<proto::ReceiveLocalSlamResultsResponse>
   GenerateResponse(
-      int trajectory_id, cartographer::common::Time time,
-      const cartographer::transform::Rigid3d& local_pose,
-      std::shared_ptr<const cartographer::sensor::RangeData> range_data,
-      std::unique_ptr<const cartographer::mapping::NodeId> node_id) {
+      std::unique_ptr<MapBuilderServer::LocalSlamResult> local_slam_result) {
     auto response = cartographer::common::make_unique<
         proto::ReceiveLocalSlamResultsResponse>();
-    response->set_trajectory_id(trajectory_id);
-    response->set_timestamp(cartographer::common::ToUniversal(time));
+    response->set_trajectory_id(local_slam_result->trajectory_id);
+    response->set_timestamp(
+        cartographer::common::ToUniversal(local_slam_result->time));
     *response->mutable_local_pose() =
-        cartographer::transform::ToProto(local_pose);
-    if (range_data) {
+        cartographer::transform::ToProto(local_slam_result->local_pose);
+    if (local_slam_result->range_data) {
       *response->mutable_range_data() =
-          cartographer::sensor::ToProto(*range_data);
+          cartographer::sensor::ToProto(*local_slam_result->range_data);
     }
-    if (node_id) {
-      response->mutable_node_id()->set_trajectory_id(node_id->trajectory_id);
-      response->mutable_node_id()->set_node_index(node_id->node_index);
+    if (local_slam_result->node_id) {
+      response->mutable_node_id()->set_trajectory_id(
+          local_slam_result->node_id->trajectory_id);
+      response->mutable_node_id()->set_node_index(
+          local_slam_result->node_id->node_index);
     }
     return response;
   }
diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc
index 1702d0c..65499b0 100644
--- a/cartographer_grpc/map_builder_server.cc
+++ b/cartographer_grpc/map_builder_server.cc
@@ -171,8 +171,9 @@ void MapBuilderServer::OnLocalSlamResult(
                       const cartographer::mapping::NodeId>(*node_id)
                 : nullptr;
     LocalSlamSubscriptionCallback callback = entry.second;
-    callback(trajectory_id, time, local_pose, shared_range_data,
-             std::move(copy_of_node_id));
+    callback(cartographer::common::make_unique<LocalSlamResult>(
+        LocalSlamResult{trajectory_id, time, local_pose, shared_range_data,
+                        std::move(copy_of_node_id)}));
   }
 }
 
diff --git a/cartographer_grpc/map_builder_server.h b/cartographer_grpc/map_builder_server.h
index 91cd0c2..08e17ba 100644
--- a/cartographer_grpc/map_builder_server.h
+++ b/cartographer_grpc/map_builder_server.h
@@ -30,12 +30,15 @@ namespace cartographer_grpc {
 
 class MapBuilderServer {
  public:
-  using LocalSlamSubscriptionCallback = std::function<void(
-      int /* trajectory ID */, cartographer::common::Time,
-      cartographer::transform::Rigid3d /* local pose estimate */,
-      std::shared_ptr<
-          const cartographer::sensor::RangeData> /* in local frame */,
-      std::unique_ptr<const cartographer::mapping::NodeId>)>;
+  struct LocalSlamResult {
+    int trajectory_id;
+    cartographer::common::Time time;
+    cartographer::transform::Rigid3d local_pose;
+    std::shared_ptr<const cartographer::sensor::RangeData> range_data;
+    std::unique_ptr<const cartographer::mapping::NodeId> node_id;
+  };
+  using LocalSlamSubscriptionCallback =
+      std::function<void(std::unique_ptr<LocalSlamResult>)>;
   struct SensorData {
     int trajectory_id;
     std::unique_ptr<cartographer::sensor::Data> sensor_data;