Send LandmarkData via gRPC. (#831)

* Send LandmarkData via gRPC.

[RFC PR](https://github.com/googlecartographer/rfcs/pull/18)

* Fix the nits.

cartographer_grpc/handlers/add_landmark_data_handler.h

@@ -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.
+ */
+
+#ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer_grpc/framework/rpc_handler.h"
+#include "cartographer_grpc/map_builder_server.h"
+#include "cartographer_grpc/proto/map_builder_service.pb.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer_grpc {
+namespace handlers {
+
+class AddLandmarkDataHandler
+    : public framework::RpcHandler<
+          framework::Stream<proto::AddLandmarkDataRequest>,
+          google::protobuf::Empty> {
+ public:
+  void OnRequest(const proto::AddLandmarkDataRequest &request) override {
+    // The 'BlockingQueue' returned by 'sensor_data_queue()' is already
+    // thread-safe. Therefore it suffices to get an unsynchronized reference to
+    // the 'MapBuilderContext'.
+    GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+        ->EnqueueSensorData(
+            request.sensor_metadata().trajectory_id(),
+            request.sensor_metadata().sensor_id(),
+            cartographer::sensor::FromProto(request.landmark_data()));
+
+    // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
+    // Therefore it suffices to get an unsynchronized reference to the
+    // 'MapBuilderContext'.
+    if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+            ->local_trajectory_uploader()) {
+      auto data_request =
+          cartographer::common::make_unique<proto::AddLandmarkDataRequest>();
+      sensor::CreateAddLandmarkDataRequest(
+          request.sensor_metadata().sensor_id(),
+          request.sensor_metadata().trajectory_id(), request.landmark_data(),
+          data_request.get());
+      GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+          ->local_trajectory_uploader()
+          ->EnqueueDataRequest(std::move(data_request));
+    }
+  }
+
+  void OnReadsDone() override {
+    Send(cartographer::common::make_unique<google::protobuf::Empty>());
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H

cartographer_grpc/map_builder_server.cc

@@ -18,6 +18,7 @@
 
 #include "cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h"
 #include "cartographer_grpc/handlers/add_imu_data_handler.h"
+#include "cartographer_grpc/handlers/add_landmark_data_handler.h"
 #include "cartographer_grpc/handlers/add_local_slam_result_data_handler.h"
 #include "cartographer_grpc/handlers/add_odometry_data_handler.h"
 #include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
@@ -225,6 +226,8 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::AddFixedFramePoseDataHandler,
                                  proto::MapBuilderService>(
       "AddFixedFramePoseData");
+  server_builder.RegisterHandler<handlers::AddLandmarkDataHandler,
+                                 proto::MapBuilderService>("AddLandmarkData");
   server_builder.RegisterHandler<handlers::AddLocalSlamResultDataHandler,
                                  proto::MapBuilderService>(
       "AddLocalSlamResultData");

cartographer_grpc/mapping/trajectory_builder_stub.cc

@@ -130,7 +130,16 @@ void TrajectoryBuilderStub::AddSensorData(
 void TrajectoryBuilderStub::AddSensorData(
     const std::string& sensor_id,
     const cartographer::sensor::LandmarkData& landmark_data) {
-  LOG(FATAL) << "Not implemented";
+  if (!landmark_writer_.client_writer) {
+    landmark_writer_.client_writer = stub_->AddLandmarkData(
+        &landmark_writer_.client_context, &landmark_writer_.response);
+    CHECK(landmark_writer_.client_writer);
+  }
+  proto::AddLandmarkDataRequest request;
+  sensor::CreateAddLandmarkDataRequest(
+      sensor_id, trajectory_id_, cartographer::sensor::ToProto(landmark_data),
+      &request);
+  landmark_writer_.client_writer->Write(request);
 }
 
 void TrajectoryBuilderStub::AddLocalSlamResultData(

cartographer_grpc/mapping/trajectory_builder_stub.h

@@ -76,6 +76,7 @@ class TrajectoryBuilderStub
   framework::ClientWriter<proto::AddOdometryDataRequest> odometry_writer_;
   framework::ClientWriter<proto::AddFixedFramePoseDataRequest>
       fixed_frame_writer_;
+  framework::ClientWriter<proto::AddLandmarkDataRequest> landmark_writer_;
   LocalSlamResultReader local_slam_result_reader_;
 };
 

cartographer_grpc/proto/map_builder_service.proto

@@ -59,6 +59,11 @@ message AddFixedFramePoseDataRequest {
   cartographer.sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
 }
 
+message AddLandmarkDataRequest {
+  SensorMetadata sensor_metadata = 1;
+  cartographer.sensor.proto.LandmarkData landmark_data = 2;
+}
+
 message FinishTrajectoryRequest {
   int32 trajectory_id = 1;
 }
@@ -145,6 +150,10 @@ service MapBuilderService {
   rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
       returns (google.protobuf.Empty);
 
+  // Same for landmark data.
+  rpc AddLandmarkData(stream AddLandmarkDataRequest)
+      returns (google.protobuf.Empty);
+
   // Adds a local SLAM result.
   rpc AddLocalSlamResultData(stream AddLocalSlamResultDataRequest)
       returns (google.protobuf.Empty);

cartographer_grpc/sensor/serialization.cc

@@ -63,5 +63,14 @@ void CreateAddRangeFinderDataRequest(
   *proto->mutable_timed_point_cloud_data() = timed_point_cloud_data;
 }
 
+void CreateAddLandmarkDataRequest(
+    const std::string& sensor_id, int trajectory_id,
+    const cartographer::sensor::proto::LandmarkData& landmark_data,
+    proto::AddLandmarkDataRequest* proto) {
+  CreateSensorMetadata(sensor_id, trajectory_id,
+                       proto->mutable_sensor_metadata());
+  *proto->mutable_landmark_data() = landmark_data;
+}
+
 }  // namespace sensor
 }  // namespace cartographer_grpc

cartographer_grpc/sensor/serialization.h

@@ -19,6 +19,7 @@
 
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/landmark_data.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/timed_point_cloud_data.h"
 #include "cartographer_grpc/proto/map_builder_service.pb.h"
@@ -28,6 +29,7 @@ namespace sensor {
 
 void CreateSensorMetadata(const std::string& sensor_id, int trajectory_id,
                           proto::SensorMetadata* proto);
+
 void CreateAddFixedFramePoseDataRequest(
     const std::string& sensor_id, int trajectory_id,
     const cartographer::sensor::proto::FixedFramePoseData&
@@ -46,6 +48,10 @@ void CreateAddRangeFinderDataRequest(
     const cartographer::sensor::proto::TimedPointCloudData&
         timed_point_cloud_data,
     proto::AddRangefinderDataRequest* proto);
+void CreateAddLandmarkDataRequest(
+    const std::string& sensor_id, int trajectory_id,
+    const cartographer::sensor::proto::LandmarkData& landmark_data,
+    proto::AddLandmarkDataRequest* proto);
 
 }  // namespace sensor
 }  // namespace cartographer_grpc