From 9e30c1e0cd35bdf4783be3675063eb8d440c9add Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Fri, 19 Jan 2018 17:24:30 +0100 Subject: [PATCH] Send LandmarkData via gRPC. (#831) * Send LandmarkData via gRPC. [RFC PR](https://github.com/googlecartographer/rfcs/pull/18) * Fix the nits. --- .../handlers/add_landmark_data_handler.h | 69 +++++++++++++++++++ cartographer_grpc/map_builder_server.cc | 3 + .../mapping/trajectory_builder_stub.cc | 11 ++- .../mapping/trajectory_builder_stub.h | 1 + .../proto/map_builder_service.proto | 9 +++ cartographer_grpc/sensor/serialization.cc | 9 +++ cartographer_grpc/sensor/serialization.h | 6 ++ 7 files changed, 107 insertions(+), 1 deletion(-) create mode 100644 cartographer_grpc/handlers/add_landmark_data_handler.h diff --git a/cartographer_grpc/handlers/add_landmark_data_handler.h b/cartographer_grpc/handlers/add_landmark_data_handler.h new file mode 100644 index 0000000..a750f84 --- /dev/null +++ b/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, + 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() + ->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() + ->local_trajectory_uploader()) { + auto data_request = + cartographer::common::make_unique(); + sensor::CreateAddLandmarkDataRequest( + request.sensor_metadata().sensor_id(), + request.sensor_metadata().trajectory_id(), request.landmark_data(), + data_request.get()); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueDataRequest(std::move(data_request)); + } + } + + void OnReadsDone() override { + Send(cartographer::common::make_unique()); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index 47161b4..9d4eccd 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/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( "AddFixedFramePoseData"); + server_builder.RegisterHandler("AddLandmarkData"); server_builder.RegisterHandler( "AddLocalSlamResultData"); diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.cc b/cartographer_grpc/mapping/trajectory_builder_stub.cc index 85aaea9..cd1376e 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.cc +++ b/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( diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.h b/cartographer_grpc/mapping/trajectory_builder_stub.h index bf30e3c..a5e324f 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.h +++ b/cartographer_grpc/mapping/trajectory_builder_stub.h @@ -76,6 +76,7 @@ class TrajectoryBuilderStub framework::ClientWriter odometry_writer_; framework::ClientWriter fixed_frame_writer_; + framework::ClientWriter landmark_writer_; LocalSlamResultReader local_slam_result_reader_; }; diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index 351d07b..de107c9 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/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); diff --git a/cartographer_grpc/sensor/serialization.cc b/cartographer_grpc/sensor/serialization.cc index e83106b..8ed1039 100644 --- a/cartographer_grpc/sensor/serialization.cc +++ b/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 diff --git a/cartographer_grpc/sensor/serialization.h b/cartographer_grpc/sensor/serialization.h index c7d7d6d..529b7ad 100644 --- a/cartographer_grpc/sensor/serialization.h +++ b/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