From 02734c296d59e2320cf82aaa549eb928fcf58592 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Tue, 19 Dec 2017 15:11:29 +0100 Subject: [PATCH] Implement ReceiveLocalSlamResultsHandler. (#772) --- cartographer/sensor/proto/sensor.proto | 7 ++ cartographer/sensor/range_data.cc | 31 +++++++ cartographer/sensor/range_data.h | 6 ++ cartographer_grpc/framework/rpc.cc | 2 + cartographer_grpc/framework/rpc.h | 1 + .../framework/rpc_handler_interface.h | 1 + cartographer_grpc/framework/service.cc | 2 + .../receive_local_slam_results_handler.h | 92 +++++++++++++++++++ cartographer_grpc/map_builder_server.cc | 16 ++++ .../proto/map_builder_service.proto | 19 ++++ 10 files changed, 177 insertions(+) create mode 100644 cartographer_grpc/handlers/receive_local_slam_results_handler.h diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 7dcb5c3..ec81fcf 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/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; diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 439fe36..6246df6 100644 --- a/cartographer/sensor/range_data.cc +++ b/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( + transform::ToEigen)); + PointCloud misses; + misses.reserve(proto.misses().size()); + std::transform( + proto.misses().begin(), proto.misses().end(), std::back_inserter(misses), + static_cast( + transform::ToEigen)); + return RangeData{transform::ToEigen(proto.origin()), returns, misses}; +} + } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index 05873db..7439bc7 100644 --- a/cartographer/sensor/range_data.h +++ b/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 diff --git a/cartographer_grpc/framework/rpc.cc b/cartographer_grpc/framework/rpc.cc index 860ea31..e6fc85d 100644 --- a/cartographer_grpc/framework/rpc.cc +++ b/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); diff --git a/cartographer_grpc/framework/rpc.h b/cartographer_grpc/framework/rpc.h index 8f8eb74..acee225 100644 --- a/cartographer_grpc/framework/rpc.h +++ b/cartographer_grpc/framework/rpc.h @@ -62,6 +62,7 @@ class Rpc { std::unique_ptr Clone(); void OnRequest(); void OnReadsDone(); + void OnFinish(); void RequestNextMethodInvocation(); void RequestStreamingReadIfNeeded(); void HandleSendQueue(); diff --git a/cartographer_grpc/framework/rpc_handler_interface.h b/cartographer_grpc/framework/rpc_handler_interface.h index 2979529..8695ef9 100644 --- a/cartographer_grpc/framework/rpc_handler_interface.h +++ b/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( diff --git a/cartographer_grpc/framework/service.cc b/cartographer_grpc/framework/service.cc index d7027e8..69c49ec 100644 --- a/cartographer_grpc/framework/service.cc +++ b/cartographer_grpc/framework/service.cc @@ -134,6 +134,8 @@ void Service::HandleFinish(Rpc* rpc, bool ok) { LOG(ERROR) << "Finish failed"; } + rpc->OnFinish(); + RemoveIfNotPending(rpc); } diff --git a/cartographer_grpc/handlers/receive_local_slam_results_handler.h b/cartographer_grpc/handlers/receive_local_slam_results_handler.h new file mode 100644 index 0000000..6137527 --- /dev/null +++ b/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> { + public: + void OnRequest( + const proto::ReceiveLocalSlamResultsRequest& request) override { + auto writer = GetWriter(); + MapBuilderServer::SubscriptionId subscription_id = + GetUnsynchronizedContext() + ->SubscribeLocalSlamResults( + request.trajectory_id(), + [writer](int trajectory_id, cartographer::common::Time time, + cartographer::transform::Rigid3d local_pose, + std::shared_ptr + range_data, + std::unique_ptr + node_id) { + writer(GenerateResponse(trajectory_id, time, local_pose, + range_data, std::move(node_id))); + }); + + subscription_id_ = + cartographer::common::make_unique( + subscription_id); + } + + static std::unique_ptr + GenerateResponse( + int trajectory_id, cartographer::common::Time time, + const cartographer::transform::Rigid3d& local_pose, + std::shared_ptr range_data, + std::unique_ptr 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() + ->UnsubscribeLocalSlamResults(*subscription_id_); + } + } + + private: + std::unique_ptr subscription_id_; +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index b1f9423..1702d0c 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/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 map_builder) @@ -95,6 +108,9 @@ MapBuilderServer::MapBuilderServer( "AddFixedFramePoseData"); server_builder.RegisterHandler("FinishTrajectory"); + server_builder.RegisterHandler( + "ReceiveLocalSlamResults"); grpc_server_ = server_builder.Build(); grpc_server_->SetExecutionContext( cartographer::common::make_unique(this)); diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index 48a72b9..fca5e29 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/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);