From c881fe90cf82dc530ced1772c32e6e92b2520c4c Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 20 Dec 2017 12:42:27 +0100 Subject: [PATCH] Stub receives LocalSlamResults. (#778) [RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md) PAIR=cschuet --- cartographer_grpc/mapping/map_builder_stub.cc | 3 +- .../mapping/trajectory_builder_stub.cc | 44 ++++++++++++++++++- .../mapping/trajectory_builder_stub.h | 15 ++++++- 3 files changed, 59 insertions(+), 3 deletions(-) diff --git a/cartographer_grpc/mapping/map_builder_stub.cc b/cartographer_grpc/mapping/map_builder_stub.cc index 46f1dfa..92cb10a 100644 --- a/cartographer_grpc/mapping/map_builder_stub.cc +++ b/cartographer_grpc/mapping/map_builder_stub.cc @@ -49,7 +49,8 @@ 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_, result.trajectory_id()))); + client_channel_, result.trajectory_id(), + local_slam_result_callback))); return result.trajectory_id(); } diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.cc b/cartographer_grpc/mapping/trajectory_builder_stub.cc index 67f10a4..46aeec1 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.cc +++ b/cartographer_grpc/mapping/trajectory_builder_stub.cc @@ -23,13 +23,30 @@ namespace cartographer_grpc { namespace mapping { TrajectoryBuilderStub::TrajectoryBuilderStub( - std::shared_ptr client_channel, const int trajectory_id) + std::shared_ptr client_channel, const int trajectory_id, + LocalSlamResultCallback local_slam_result_callback) : client_channel_(client_channel), trajectory_id_(trajectory_id) { stub_ = proto::MapBuilderService::NewStub(client_channel_); CHECK(stub_) << "Failed to create stub."; + if (local_slam_result_callback) { + proto::ReceiveLocalSlamResultsRequest request; + request.set_trajectory_id(trajectory_id); + local_slam_result_reader_.client_reader = stub_->ReceiveLocalSlamResults( + &local_slam_result_reader_.client_context, request); + auto* client_reader_ptr = local_slam_result_reader_.client_reader.get(); + local_slam_result_reader_.thread = + cartographer::common::make_unique( + [client_reader_ptr, local_slam_result_callback]() { + RunLocalSlamResultReader(client_reader_ptr, + local_slam_result_callback); + }); + } } TrajectoryBuilderStub::~TrajectoryBuilderStub() { + if (local_slam_result_reader_.thread) { + local_slam_result_reader_.thread->join(); + } if (rangefinder_writer_.client_writer) { CHECK(rangefinder_writer_.client_writer->WritesDone()); CHECK(rangefinder_writer_.client_writer->Finish().ok()); @@ -115,5 +132,30 @@ proto::SensorMetadata TrajectoryBuilderStub::CreateSensorMetadata( return sensor_metadata; } +void TrajectoryBuilderStub::RunLocalSlamResultReader( + grpc::ClientReader* client_reader, + LocalSlamResultCallback local_slam_result_callback) { + proto::ReceiveLocalSlamResultsResponse response; + while (client_reader->Read(&response)) { + int trajectory_id = response.trajectory_id(); + cartographer::common::Time time = + cartographer::common::FromUniversal(response.timestamp()); + cartographer::transform::Rigid3d local_pose = + cartographer::transform::ToRigid3(response.local_pose()); + cartographer::sensor::RangeData range_data = + cartographer::sensor::FromProto(response.range_data()); + auto node_id = + response.has_node_id() + ? cartographer::common::make_unique( + cartographer::mapping::NodeId{ + response.node_id().trajectory_id(), + response.node_id().node_index()}) + : nullptr; + local_slam_result_callback(trajectory_id, time, local_pose, range_data, + std::move(node_id)); + } + client_reader->Finish(); +} + } // namespace mapping } // namespace cartographer_grpc diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.h b/cartographer_grpc/mapping/trajectory_builder_stub.h index 6f6ebaf..09b6fa5 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.h +++ b/cartographer_grpc/mapping/trajectory_builder_stub.h @@ -17,6 +17,8 @@ #ifndef CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_ #define CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_ +#include + #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" #include "grpc++/grpc++.h" @@ -28,7 +30,8 @@ class TrajectoryBuilderStub : public cartographer::mapping::TrajectoryBuilderInterface { public: TrajectoryBuilderStub(std::shared_ptr client_channel, - const int trajectory_id); + const int trajectory_id, + LocalSlamResultCallback local_slam_result_callback); ~TrajectoryBuilderStub() override; TrajectoryBuilderStub(const TrajectoryBuilderStub&) = delete; TrajectoryBuilderStub& operator=(const TrajectoryBuilderStub&) = delete; @@ -52,8 +55,17 @@ class TrajectoryBuilderStub std::unique_ptr> client_writer; google::protobuf::Empty response; }; + struct LocalSlamResultReader { + grpc::ClientContext client_context; + std::unique_ptr> + client_reader; + std::unique_ptr thread; + }; proto::SensorMetadata CreateSensorMetadata(const std::string& sensor_id); + static void RunLocalSlamResultReader( + grpc::ClientReader* client_reader, + LocalSlamResultCallback local_slam_result_callback); std::shared_ptr client_channel_; const int trajectory_id_; @@ -62,6 +74,7 @@ class TrajectoryBuilderStub SensorClientWriter imu_writer_; SensorClientWriter odometry_writer_; SensorClientWriter fixed_frame_writer_; + LocalSlamResultReader local_slam_result_reader_; }; } // namespace mapping