Stub receives LocalSlamResults. (#778)

[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)
PAIR=cschuet

cartographer_grpc/mapping/map_builder_stub.cc

@@ -49,7 +49,8 @@ int MapBuilderStub::AddTrajectoryBuilder(
       std::piecewise_construct, std::forward_as_tuple(result.trajectory_id()),
       std::forward_as_tuple(cartographer::common::make_unique<
                             cartographer_grpc::mapping::TrajectoryBuilderStub>(
-          client_channel_, result.trajectory_id())));
+          client_channel_, result.trajectory_id(),
+          local_slam_result_callback)));
   return result.trajectory_id();
 }
 

cartographer_grpc/mapping/trajectory_builder_stub.cc

@@ -23,13 +23,30 @@ namespace cartographer_grpc {
 namespace mapping {
 
 TrajectoryBuilderStub::TrajectoryBuilderStub(
-    std::shared_ptr<grpc::Channel> client_channel, const int trajectory_id)
+    std::shared_ptr<grpc::Channel> client_channel, const int trajectory_id,
+    LocalSlamResultCallback local_slam_result_callback)
     : client_channel_(client_channel), trajectory_id_(trajectory_id) {
   stub_ = proto::MapBuilderService::NewStub(client_channel_);
   CHECK(stub_) << "Failed to create stub.";
+  if (local_slam_result_callback) {
+    proto::ReceiveLocalSlamResultsRequest request;
+    request.set_trajectory_id(trajectory_id);
+    local_slam_result_reader_.client_reader = stub_->ReceiveLocalSlamResults(
+        &local_slam_result_reader_.client_context, request);
+    auto* client_reader_ptr = local_slam_result_reader_.client_reader.get();
+    local_slam_result_reader_.thread =
+        cartographer::common::make_unique<std::thread>(
+            [client_reader_ptr, local_slam_result_callback]() {
+              RunLocalSlamResultReader(client_reader_ptr,
+                                       local_slam_result_callback);
+            });
+  }
 }
 
 TrajectoryBuilderStub::~TrajectoryBuilderStub() {
+  if (local_slam_result_reader_.thread) {
+    local_slam_result_reader_.thread->join();
+  }
   if (rangefinder_writer_.client_writer) {
     CHECK(rangefinder_writer_.client_writer->WritesDone());
     CHECK(rangefinder_writer_.client_writer->Finish().ok());
@@ -115,5 +132,30 @@ proto::SensorMetadata TrajectoryBuilderStub::CreateSensorMetadata(
   return sensor_metadata;
 }
 
+void TrajectoryBuilderStub::RunLocalSlamResultReader(
+    grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader,
+    LocalSlamResultCallback local_slam_result_callback) {
+  proto::ReceiveLocalSlamResultsResponse response;
+  while (client_reader->Read(&response)) {
+    int trajectory_id = response.trajectory_id();
+    cartographer::common::Time time =
+        cartographer::common::FromUniversal(response.timestamp());
+    cartographer::transform::Rigid3d local_pose =
+        cartographer::transform::ToRigid3(response.local_pose());
+    cartographer::sensor::RangeData range_data =
+        cartographer::sensor::FromProto(response.range_data());
+    auto node_id =
+        response.has_node_id()
+            ? cartographer::common::make_unique<cartographer::mapping::NodeId>(
+                  cartographer::mapping::NodeId{
+                      response.node_id().trajectory_id(),
+                      response.node_id().node_index()})
+            : nullptr;
+    local_slam_result_callback(trajectory_id, time, local_pose, range_data,
+                               std::move(node_id));
+  }
+  client_reader->Finish();
+}
+
 }  // namespace mapping
 }  // namespace cartographer_grpc

cartographer_grpc/mapping/trajectory_builder_stub.h

@@ -17,6 +17,8 @@
 #ifndef CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_
 #define CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_
 
+#include <thread>
+
 #include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
 #include "grpc++/grpc++.h"
@@ -28,7 +30,8 @@ class TrajectoryBuilderStub
     : public cartographer::mapping::TrajectoryBuilderInterface {
  public:
   TrajectoryBuilderStub(std::shared_ptr<grpc::Channel> client_channel,
-                        const int trajectory_id);
+                        const int trajectory_id,
+                        LocalSlamResultCallback local_slam_result_callback);
   ~TrajectoryBuilderStub() override;
   TrajectoryBuilderStub(const TrajectoryBuilderStub&) = delete;
   TrajectoryBuilderStub& operator=(const TrajectoryBuilderStub&) = delete;
@@ -52,8 +55,17 @@ class TrajectoryBuilderStub
     std::unique_ptr<grpc::ClientWriter<RequestType>> client_writer;
     google::protobuf::Empty response;
   };
+  struct LocalSlamResultReader {
+    grpc::ClientContext client_context;
+    std::unique_ptr<grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>>
+        client_reader;
+    std::unique_ptr<std::thread> thread;
+  };
 
   proto::SensorMetadata CreateSensorMetadata(const std::string& sensor_id);
+  static void RunLocalSlamResultReader(
+      grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader,
+      LocalSlamResultCallback local_slam_result_callback);
 
   std::shared_ptr<grpc::Channel> client_channel_;
   const int trajectory_id_;
@@ -62,6 +74,7 @@ class TrajectoryBuilderStub
   SensorClientWriter<proto::AddImuDataRequest> imu_writer_;
   SensorClientWriter<proto::AddOdometryDataRequest> odometry_writer_;
   SensorClientWriter<proto::AddFixedFramePoseDataRequest> fixed_frame_writer_;
+  LocalSlamResultReader local_slam_result_reader_;
 };
 
 }  // namespace mapping