Implement ReceiveLocalSlamResultsHandler. (#772)

cartographer/sensor/proto/sensor.proto

@@ -33,6 +33,13 @@ message TimedPointCloudData {
   repeated transform.proto.Vector4f point_data = 3;
 }
 
+// Proto representation of ::cartographer::sensor::RangeData.
+message RangeData {
+  transform.proto.Vector3f origin = 1;
+  repeated transform.proto.Vector3f returns = 2;
+  repeated transform.proto.Vector3f misses = 3;
+}
+
 // Proto representation of ::cartographer::sensor::ImuData.
 message ImuData {
   int64 timestamp = 1;

cartographer/sensor/range_data.cc

@@ -54,5 +54,36 @@ TimedRangeData CropTimedRangeData(const TimedRangeData& range_data,
                         CropTimedPointCloud(range_data.misses, min_z, max_z)};
 }
 
+proto::RangeData ToProto(const RangeData& range_data) {
+  proto::RangeData proto;
+  *proto.mutable_origin() = transform::ToProto(range_data.origin);
+  proto.mutable_returns()->Reserve(range_data.returns.size());
+  for (const auto& point : range_data.returns) {
+    *proto.add_returns() = transform::ToProto(point);
+  }
+  proto.mutable_misses()->Reserve(range_data.misses.size());
+  for (const auto& point : range_data.misses) {
+    *proto.add_misses() = transform::ToProto(point);
+  }
+  return proto;
+}
+
+RangeData FromProto(const proto::RangeData& proto) {
+  PointCloud returns;
+  returns.reserve(proto.returns().size());
+  std::transform(
+      proto.returns().begin(), proto.returns().end(),
+      std::back_inserter(returns),
+      static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
+          transform::ToEigen));
+  PointCloud misses;
+  misses.reserve(proto.misses().size());
+  std::transform(
+      proto.misses().begin(), proto.misses().end(), std::back_inserter(misses),
+      static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
+          transform::ToEigen));
+  return RangeData{transform::ToEigen(proto.origin()), returns, misses};
+}
+
 }  // namespace sensor
 }  // namespace cartographer

cartographer/sensor/range_data.h

@@ -55,6 +55,12 @@ RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z);
 TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, float min_z,
                                   float max_z);
 
+// Converts 'range_data' to a proto::RangeData.
+proto::RangeData ToProto(const RangeData& range_data);
+
+// Converts 'proto' to RangeData.
+RangeData FromProto(const proto::RangeData& proto);
+
 }  // namespace sensor
 }  // namespace cartographer
 

cartographer_grpc/framework/rpc.cc

@@ -74,6 +74,8 @@ void Rpc::OnRequest() { handler_->OnRequestInternal(request_.get()); }
 
 void Rpc::OnReadsDone() { handler_->OnReadsDone(); }
 
+void Rpc::OnFinish() { handler_->OnFinish(); }
+
 void Rpc::RequestNextMethodInvocation() {
   // Ask gRPC to notify us when the connection terminates.
   SetRpcEventState(Event::DONE, true);

cartographer_grpc/framework/rpc.h

@@ -62,6 +62,7 @@ class Rpc {
   std::unique_ptr<Rpc> Clone();
   void OnRequest();
   void OnReadsDone();
+  void OnFinish();
   void RequestNextMethodInvocation();
   void RequestStreamingReadIfNeeded();
   void HandleSendQueue();

cartographer_grpc/framework/rpc_handler_interface.h

@@ -33,6 +33,7 @@ class RpcHandlerInterface {
   virtual void OnRequestInternal(
       const ::google::protobuf::Message* request) = 0;
   virtual void OnReadsDone(){};
+  virtual void OnFinish(){};
 };
 
 using RpcHandlerFactory = std::function<std::unique_ptr<RpcHandlerInterface>(

cartographer_grpc/framework/service.cc

@@ -134,6 +134,8 @@ void Service::HandleFinish(Rpc* rpc, bool ok) {
     LOG(ERROR) << "Finish failed";
   }
 
+  rpc->OnFinish();
+
   RemoveIfNotPending(rpc);
 }
 

cartographer_grpc/handlers/receive_local_slam_results_handler.h

@@ -0,0 +1,92 @@
+/*
+ * 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_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_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"
+
+namespace cartographer_grpc {
+namespace handlers {
+
+class ReceiveLocalSlamResultsHandler
+    : public framework::RpcHandler<
+          proto::ReceiveLocalSlamResultsRequest,
+          framework::Stream<proto::ReceiveLocalSlamResultsResponse>> {
+ public:
+  void OnRequest(
+      const proto::ReceiveLocalSlamResultsRequest& request) override {
+    auto writer = GetWriter();
+    MapBuilderServer::SubscriptionId subscription_id =
+        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)));
+                });
+
+    subscription_id_ =
+        cartographer::common::make_unique<MapBuilderServer::SubscriptionId>(
+            subscription_id);
+  }
+
+  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) {
+    auto response = cartographer::common::make_unique<
+        proto::ReceiveLocalSlamResultsResponse>();
+    response->set_trajectory_id(trajectory_id);
+    response->set_timestamp(cartographer::common::ToUniversal(time));
+    *response->mutable_local_pose() =
+        cartographer::transform::ToProto(local_pose);
+    if (range_data) {
+      *response->mutable_range_data() =
+          cartographer::sensor::ToProto(*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);
+    }
+    return response;
+  }
+
+  void OnFinish() override {
+    if (subscription_id_) {
+      GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+          ->UnsubscribeLocalSlamResults(*subscription_id_);
+    }
+  }
+
+ private:
+  std::unique_ptr<MapBuilderServer::SubscriptionId> subscription_id_;
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H

cartographer_grpc/map_builder_server.cc

@@ -22,6 +22,7 @@
 #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/handlers/receive_local_slam_results_handler.h"
 #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
 #include "glog/logging.h"
 
@@ -70,6 +71,18 @@ void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory(
           sensor_data.trajectory_id));
 }
 
+MapBuilderServer::SubscriptionId
+MapBuilderServer::MapBuilderContext::SubscribeLocalSlamResults(
+    int trajectory_id, LocalSlamSubscriptionCallback callback) {
+  return map_builder_server_->SubscribeLocalSlamResults(trajectory_id,
+                                                        callback);
+}
+
+void MapBuilderServer::MapBuilderContext::UnsubscribeLocalSlamResults(
+    const SubscriptionId& subscription_id) {
+  map_builder_server_->UnsubscribeLocalSlamResults(subscription_id);
+}
+
 MapBuilderServer::MapBuilderServer(
     const proto::MapBuilderServerOptions& map_builder_server_options,
     std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder)
@@ -95,6 +108,9 @@ MapBuilderServer::MapBuilderServer(
       "AddFixedFramePoseData");
   server_builder.RegisterHandler<handlers::FinishTrajectoryHandler,
                                  proto::MapBuilderService>("FinishTrajectory");
+  server_builder.RegisterHandler<handlers::ReceiveLocalSlamResultsHandler,
+                                 proto::MapBuilderService>(
+      "ReceiveLocalSlamResults");
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
       cartographer::common::make_unique<MapBuilderContext>(this));

cartographer_grpc/proto/map_builder_service.proto

@@ -14,8 +14,10 @@
 
 syntax = "proto3";
 
+import "cartographer/mapping/proto/pose_graph.proto";
 import "cartographer/mapping/proto/trajectory_builder_options.proto";
 import "cartographer/sensor/proto/sensor.proto";
+import "cartographer/transform/proto/transform.proto";
 import "google/protobuf/empty.proto";
 
 package cartographer_grpc.proto;
@@ -59,6 +61,18 @@ message FinishTrajectoryRequest {
   int32 trajectory_id = 1;
 }
 
+message ReceiveLocalSlamResultsRequest {
+  int32 trajectory_id = 1;
+}
+
+message ReceiveLocalSlamResultsResponse {
+  int32 trajectory_id = 1;
+  int64 timestamp = 2;
+  cartographer.transform.proto.Rigid3d local_pose = 3;
+  cartographer.sensor.proto.RangeData range_data = 4;
+  cartographer.mapping.proto.NodeId node_id = 5;
+}
+
 service MapBuilderService {
   // Starts a new trajectory and returns its index.
   rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@@ -79,6 +93,11 @@ service MapBuilderService {
   rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
       returns (google.protobuf.Empty);
 
+  // Requests the server to send a stream of local SLAM results for the given
+  // 'trajectory_id'.
+  rpc ReceiveLocalSlamResults(ReceiveLocalSlamResultsRequest)
+      returns (stream ReceiveLocalSlamResultsResponse);
+
   // Marks a trajectory corresponding to 'trajectory_id' as finished,
   // i.e. no further sensor data is expected.
   rpc FinishTrajectory(FinishTrajectoryRequest) returns (google.protobuf.Empty);