FixedFramePoseData in gRPC server (#761)

Defines the FixedFramePoseData RPC and implements
the handler for the gRPC server.

cartographer/mapping/map_builder_test.cc

@@ -33,8 +33,8 @@ namespace {
 
 constexpr char kRangeSensorId[] = "range";
 constexpr char kIMUSensorId[] = "imu";
-constexpr double kDuration = 4.;  // Seconds.
+constexpr double kDuration = 4.;         // Seconds.
-constexpr double kTimeStep = 0.1;  // Seconds.
+constexpr double kTimeStep = 0.1;        // Seconds.
 constexpr double kTravelDistance = 1.2;  // Meters.
 
 std::vector<sensor::TimedPointCloudData> GenerateFakeRangeMeasurements() {

cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h

@@ -0,0 +1,53 @@
+/*
+ * 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_GRPC_HANDLERS_ADD_FIXED_FRAME_POSE_DATA_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_ADD_FIXED_FRAME_POSE_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 AddFixedFramePoseDataHandler
+    : public framework::RpcHandler<
+          framework::Stream<proto::AddFixedFramePoseDataRequest>,
+          google::protobuf::Empty> {
+ public:
+  void OnRequest(const proto::AddFixedFramePoseDataRequest &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.fixed_frame_pose_data()));
+  }
+
+  void OnReadsDone() {
+    Send(cartographer::common::make_unique<google::protobuf::Empty>());
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_ADD_FIXED_FRAME_POSE_DATA_HANDLER_H

cartographer_grpc/map_builder_server.cc

@@ -16,6 +16,7 @@
 
 #include "cartographer_grpc/map_builder_server.h"
 
+#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_odometry_data_handler.h"
 #include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
@@ -66,6 +67,9 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::AddRangefinderDataHandler,
                                  proto::MapBuilderService>(
       "AddRangefinderData");
+  server_builder.RegisterHandler<handlers::AddFixedFramePoseDataHandler,
+                                 proto::MapBuilderService>(
+      "AddFixedFramePoseData");
   server_builder.RegisterHandler<handlers::FinishTrajectoryHandler,
                                  proto::MapBuilderService>("FinishTrajectory");
   grpc_server_ = server_builder.Build();

cartographer_grpc/proto/map_builder_service.proto

@@ -50,6 +50,11 @@ message AddRangefinderDataRequest {
   cartographer.sensor.proto.TimedPointCloudData timed_point_cloud_data = 2;
 }
 
+message AddFixedFramePoseDataRequest {
+  SensorMetadata sensor_metadata = 1;
+  cartographer.sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
+}
+
 message FinishTrajectoryRequest {
   int32 trajectory_id = 1;
 }
@@ -63,15 +68,17 @@ service MapBuilderService {
   rpc AddOdometryData(stream AddOdometryDataRequest)
       returns (google.protobuf.Empty);
 
-  // Adds IMU data from the sensor with id 'sensor_metadata.sensor_id' to
+  // Same for IMU data.
-  // the trajectory corresponding to 'sensor_metadata.trajectory_id'.
   rpc AddImuData(stream AddImuDataRequest) returns (google.protobuf.Empty);
 
-  // Adds range-finder data from the sensor with id 'sensor_metadata.sensor_id'
+  // Same for range-finder data.
-  // to the trajectory corresponding to 'sensor_metadata.trajectory_id'.
   rpc AddRangefinderData(stream AddRangefinderDataRequest)
       returns (google.protobuf.Empty);
 
+  // Same for fixed-frame pose data.
+  rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
+      returns (google.protobuf.Empty);
+
   // Marks a trajectory corresponding to 'trajectory_id' as finished,
   // i.e. no further sensor data is expected.
   rpc FinishTrajectory(FinishTrajectoryRequest) returns (google.protobuf.Empty);