Implement gRPC data handlers and SLAM thread. (#749)

cartographer_grpc/framework/execution_context.h

@@ -30,9 +30,9 @@ namespace framework {
 // 'RpcHandler::GetContext<MyContext>()'.
 class ExecutionContext {
  public:
+  // Automatically locks an ExecutionContext for shared use by RPC handlers.
   // This non-movable, non-copyable class is used to broker access from various
-  // RPC handlers to the shared 'ExecutionContext'. Handles automatically lock
-  // the context they point to.
+  // RPC handlers to the shared 'ExecutionContext'.
   template <typename ContextType>
   class Synchronized {
    public:
@@ -49,6 +49,7 @@ class ExecutionContext {
     cartographer::common::MutexLocker locker_;
     ExecutionContext* execution_context_;
   };
+  virtual ~ExecutionContext() = default;
   cartographer::common::Mutex* lock() { return &lock_; }
 
  private:

cartographer_grpc/framework/rpc_handler.h

@@ -53,6 +53,10 @@ class RpcHandler : public RpcHandlerInterface {
   ExecutionContext::Synchronized<T> GetContext() {
     return {execution_context_->lock(), execution_context_};
   }
+  template <typename T>
+  T* GetUnsynchronizedContext() {
+    return dynamic_cast<T*>(execution_context_);
+  }
 
  private:
   Rpc* rpc_;

cartographer_grpc/framework/server.h

@@ -96,6 +96,11 @@ class Server {
   // Sets the server-wide context object shared between RPC handlers.
   void SetExecutionContext(std::unique_ptr<ExecutionContext> execution_context);
 
+  template <typename T>
+  ExecutionContext::Synchronized<T> GetContext() {
+    return {execution_context_->lock(), execution_context_.get()};
+  }
+
  private:
   Server(const Options& options);
   Server(const Server&) = delete;

cartographer_grpc/handlers/add_imu_data_handler.h

@@ -0,0 +1,52 @@
+/*
+ * 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_IMU_DATA_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_ADD_IMU_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 AddImuDataHandler
+    : public framework::RpcHandler<framework::Stream<proto::AddImuDataRequest>,
+                                   google::protobuf::Empty> {
+ public:
+  void OnRequest(const proto::AddImuDataRequest &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.imu_data()));
+  }
+
+  void OnReadsDone() {
+    Send(cartographer::common::make_unique<google::protobuf::Empty>());
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_ADD_IMU_DATA_HANDLER_H

cartographer_grpc/handlers/add_odometry_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_ODOMETRY_DATA_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_ADD_ODOMETRY_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 AddOdometryDataHandler
+    : public framework::RpcHandler<
+          framework::Stream<proto::AddOdometryDataRequest>,
+          google::protobuf::Empty> {
+ public:
+  void OnRequest(const proto::AddOdometryDataRequest &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.odometry_data()));
+  }
+
+  void OnReadsDone() {
+    Send(cartographer::common::make_unique<google::protobuf::Empty>());
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_ADD_ODOMETRY_DATA_HANDLER_H

cartographer_grpc/handlers/add_rangefinder_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_RANGEFINDER_DATA_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_ADD_RANGEFINDER_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 AddRangefinderDataHandler
+    : public framework::RpcHandler<
+          framework::Stream<proto::AddRangefinderDataRequest>,
+          google::protobuf::Empty> {
+ public:
+  void OnRequest(const proto::AddRangefinderDataRequest &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.timed_point_cloud_data()));
+  }
+
+  void OnReadsDone() {
+    Send(cartographer::common::make_unique<google::protobuf::Empty>());
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_ADD_RANGEFINDER_DATA_HANDLER_H

cartographer_grpc/map_builder_server.cc

@@ -13,8 +13,12 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+
 #include "cartographer_grpc/map_builder_server.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"
 #include "cartographer_grpc/handlers/add_trajectory_handler.h"
 #include "cartographer_grpc/handlers/finish_trajectory_handler.h"
 #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
@@ -22,6 +26,27 @@
 
 namespace cartographer_grpc {
 
+MapBuilderServer::MapBuilderContext::MapBuilderContext(
+    cartographer::mapping::MapBuilder* map_builder,
+    cartographer::common::BlockingQueue<SensorData>* sensor_data_queue)
+    : map_builder_(map_builder), sensor_data_queue_(sensor_data_queue) {}
+
+cartographer::mapping::MapBuilder&
+MapBuilderServer::MapBuilderContext::map_builder() {
+  return *map_builder_;
+}
+
+cartographer::common::BlockingQueue<MapBuilderServer::SensorData>&
+MapBuilderServer::MapBuilderContext::sensor_data_queue() {
+  return *sensor_data_queue_;
+}
+
+void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory(
+    const SensorData& sensor_data) {
+  sensor_data.sensor_data->AddToTrajectoryBuilder(
+      map_builder_->GetTrajectoryBuilder(sensor_data.trajectory_id));
+}
+
 MapBuilderServer::MapBuilderServer(
     const proto::MapBuilderServerOptions& map_builder_server_options)
     : map_builder_(map_builder_server_options.map_builder_options()) {
@@ -31,11 +56,20 @@ MapBuilderServer::MapBuilderServer(
       map_builder_server_options.num_grpc_threads());
   server_builder.RegisterHandler<handlers::AddTrajectoryHandler,
                                  proto::MapBuilderService>("AddTrajectory");
+  server_builder.RegisterHandler<handlers::AddOdometryDataHandler,
+                                 proto::MapBuilderService>("AddOdometryData");
+  server_builder
+      .RegisterHandler<handlers::AddImuDataHandler, proto::MapBuilderService>(
+          "AddImuData");
+  server_builder.RegisterHandler<handlers::AddRangefinderDataHandler,
+                                 proto::MapBuilderService>(
+      "AddRangefinderData");
   server_builder.RegisterHandler<handlers::FinishTrajectoryHandler,
                                  proto::MapBuilderService>("FinishTrajectory");
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
-      cartographer::common::make_unique<MapBuilderContext>(&map_builder_));
+      cartographer::common::make_unique<MapBuilderContext>(
+          &map_builder_, &sensor_data_queue_));
 }
 
 void MapBuilderServer::Start() {
@@ -60,8 +94,11 @@ void MapBuilderServer::Shutdown() {
 }
 
 void MapBuilderServer::ProcessSensorDataQueue() {
+  LOG(INFO) << "Starting SLAM thread.";
   while (!shutting_down_) {
-    // TODO(cschuet): Implement this.
+    SensorData sensor_data = sensor_data_queue_.Pop();
+    grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
+        sensor_data);
   }
 }
 

cartographer_grpc/map_builder_server.h

@@ -28,14 +28,31 @@ namespace cartographer_grpc {
 
 class MapBuilderServer {
  public:
+  struct SensorData {
+    int trajectory_id;
+    std::unique_ptr<cartographer::sensor::Data> sensor_data;
+  };
+
   class MapBuilderContext : public framework::ExecutionContext {
    public:
-    MapBuilderContext(cartographer::mapping::MapBuilder* map_builder)
-        : map_builder_(map_builder) {}
-    cartographer::mapping::MapBuilder& map_builder() { return *map_builder_; }
+    MapBuilderContext(
+        cartographer::mapping::MapBuilder *map_builder,
+        cartographer::common::BlockingQueue<SensorData> *sensor_data_queue);
+    cartographer::mapping::MapBuilder &map_builder();
+    cartographer::common::BlockingQueue<SensorData> &sensor_data_queue();
+    void AddSensorDataToTrajectory(const SensorData &sensor_data);
+
+    template <typename SensorDataType>
+    void EnqueueSensorData(int trajectory_id, const std::string &sensor_id,
+                           const SensorDataType &sensor_data) {
+      sensor_data_queue_->Push(MapBuilderServer::SensorData{
+          trajectory_id,
+          cartographer::sensor::MakeDispatchable(sensor_id, sensor_data)});
+    }
 
    private:
     cartographer::mapping::MapBuilder *map_builder_;
+    cartographer::common::BlockingQueue<SensorData> *sensor_data_queue_;
   };
 
   MapBuilderServer(
@@ -60,6 +77,7 @@ class MapBuilderServer {
   std::unique_ptr<std::thread> slam_thread_;
   std::unique_ptr<framework::Server> grpc_server_;
   cartographer::mapping::MapBuilder map_builder_;
+  cartographer::common::BlockingQueue<SensorData> sensor_data_queue_;
 };
 
 }  // namespace cartographer_grpc