TrajectoryBuilderStub implementation (#764)

cartographer_grpc/mapping/map_builder_stub.cc

@@ -48,7 +48,7 @@ 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_, service_stub_.get())));
+          client_channel_, result.trajectory_id())));
   return result.trajectory_id();
 }
 

cartographer_grpc/mapping/trajectory_builder_stub.cc

@@ -16,38 +16,99 @@
 
 #include "cartographer_grpc/mapping/trajectory_builder_stub.h"
 
+#include "cartographer_grpc/proto/map_builder_service.pb.h"
 #include "glog/logging.h"
 
 namespace cartographer_grpc {
 namespace mapping {
 
 TrajectoryBuilderStub::TrajectoryBuilderStub(
-    std::shared_ptr<grpc::Channel> client_channel,
+    std::shared_ptr<grpc::Channel> client_channel, const int trajectory_id)
-    proto::MapBuilderService::Stub* stub)
+    : client_channel_(client_channel), trajectory_id_(trajectory_id) {
-    : client_channel_(client_channel), stub_(stub) {}
+  stub_ = proto::MapBuilderService::NewStub(client_channel_);
+  CHECK(stub_) << "Failed to create stub.";
+}
+
+TrajectoryBuilderStub::~TrajectoryBuilderStub() {
+  if (rangefinder_writer_.client_writer) {
+    CHECK(rangefinder_writer_.client_writer->Finish().ok());
+  }
+  if (imu_writer_.client_writer) {
+    CHECK(imu_writer_.client_writer->Finish().ok());
+  }
+  if (odometry_writer_.client_writer) {
+    CHECK(odometry_writer_.client_writer->Finish().ok());
+  }
+  if (fixed_frame_writer_.client_writer) {
+    CHECK(fixed_frame_writer_.client_writer->Finish().ok());
+  }
+}
 
 void TrajectoryBuilderStub::AddSensorData(
     const std::string& sensor_id,
     const cartographer::sensor::TimedPointCloudData& timed_point_cloud_data) {
-  LOG(FATAL) << "Not implemented";
+  if (!rangefinder_writer_.client_writer) {
+    rangefinder_writer_.client_writer = stub_->AddRangefinderData(
+        &rangefinder_writer_.client_context, &rangefinder_writer_.response);
+    CHECK(rangefinder_writer_.client_writer);
+  }
+  proto::AddRangefinderDataRequest request;
+  *request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id);
+  *request.mutable_timed_point_cloud_data() =
+      cartographer::sensor::ToProto(timed_point_cloud_data);
+  rangefinder_writer_.client_writer->Write(request);
 }
 
 void TrajectoryBuilderStub::AddSensorData(
     const std::string& sensor_id,
     const cartographer::sensor::ImuData& imu_data) {
-  LOG(FATAL) << "Not implemented";
+  if (!imu_writer_.client_writer) {
+    imu_writer_.client_writer =
+        stub_->AddImuData(&imu_writer_.client_context, &imu_writer_.response);
+    CHECK(imu_writer_.client_writer);
+  }
+  proto::AddImuDataRequest request;
+  *request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id);
+  *request.mutable_imu_data() = cartographer::sensor::ToProto(imu_data);
+  imu_writer_.client_writer->Write(request);
 }
 
 void TrajectoryBuilderStub::AddSensorData(
     const std::string& sensor_id,
     const cartographer::sensor::OdometryData& odometry_data) {
-  LOG(FATAL) << "Not implemented";
+  if (!odometry_writer_.client_writer) {
+    odometry_writer_.client_writer = stub_->AddOdometryData(
+        &odometry_writer_.client_context, &odometry_writer_.response);
+    CHECK(odometry_writer_.client_writer);
+  }
+  proto::AddOdometryDataRequest request;
+  *request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id);
+  *request.mutable_odometry_data() =
+      cartographer::sensor::ToProto(odometry_data);
+  odometry_writer_.client_writer->Write(request);
 }
 
 void TrajectoryBuilderStub::AddSensorData(
     const std::string& sensor_id,
     const cartographer::sensor::FixedFramePoseData& fixed_frame_pose) {
-  LOG(FATAL) << "Not implemented";
+  if (!fixed_frame_writer_.client_writer) {
+    fixed_frame_writer_.client_writer = stub_->AddFixedFramePoseData(
+        &fixed_frame_writer_.client_context, &fixed_frame_writer_.response);
+    CHECK(fixed_frame_writer_.client_writer);
+  }
+  proto::AddFixedFramePoseDataRequest request;
+  *request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id);
+  *request.mutable_fixed_frame_pose_data() =
+      cartographer::sensor::ToProto(fixed_frame_pose);
+  fixed_frame_writer_.client_writer->Write(request);
+}
+
+proto::SensorMetadata TrajectoryBuilderStub::CreateSensorMetadata(
+    const std::string& sensor_id) {
+  proto::SensorMetadata sensor_metadata;
+  sensor_metadata.set_sensor_id(sensor_id);
+  sensor_metadata.set_trajectory_id(trajectory_id_);
+  return sensor_metadata;
 }
 
 }  // namespace mapping

cartographer_grpc/mapping/trajectory_builder_stub.h

@@ -28,8 +28,8 @@ class TrajectoryBuilderStub
     : public cartographer::mapping::TrajectoryBuilderInterface {
  public:
   TrajectoryBuilderStub(std::shared_ptr<grpc::Channel> client_channel,
-                        proto::MapBuilderService::Stub* stub);
+                        const int trajectory_id);
-
+  ~TrajectoryBuilderStub() override;
   TrajectoryBuilderStub(const TrajectoryBuilderStub&) = delete;
   TrajectoryBuilderStub& operator=(const TrajectoryBuilderStub&) = delete;
 
@@ -46,8 +46,22 @@ class TrajectoryBuilderStub
                          fixed_frame_pose) override;
 
  private:
+  template <typename RequestType>
+  struct SensorClientWriter {
+    grpc::ClientContext client_context;
+    std::unique_ptr<grpc::ClientWriter<RequestType>> client_writer;
+    google::protobuf::Empty response;
+  };
+
+  proto::SensorMetadata CreateSensorMetadata(const std::string& sensor_id);
+
   std::shared_ptr<grpc::Channel> client_channel_;
-  proto::MapBuilderService::Stub* stub_;
+  const int trajectory_id_;
+  std::unique_ptr<proto::MapBuilderService::Stub> stub_;
+  SensorClientWriter<proto::AddRangefinderDataRequest> rangefinder_writer_;
+  SensorClientWriter<proto::AddImuDataRequest> imu_writer_;
+  SensorClientWriter<proto::AddOdometryDataRequest> odometry_writer_;
+  SensorClientWriter<proto::AddFixedFramePoseDataRequest> fixed_frame_writer_;
 };
 
 }  // namespace mapping