Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h"
00018
00019 #include "absl/memory/memory.h"
00020 #include "async_grpc/rpc_handler.h"
00021 #include "cartographer/cloud/internal/map_builder_context_interface.h"
00022 #include "cartographer/cloud/proto/map_builder_service.pb.h"
00023 #include "cartographer/transform/transform.h"
00024
00025 namespace cartographer {
00026 namespace cloud {
00027 namespace handlers {
00028 namespace {
00029
00030 std::unique_ptr<proto::ReceiveLocalSlamResultsResponse> GenerateResponse(
00031 std::unique_ptr<MapBuilderContextInterface::LocalSlamResult>
00032 local_slam_result) {
00033 auto response = absl::make_unique<proto::ReceiveLocalSlamResultsResponse>();
00034 response->set_trajectory_id(local_slam_result->trajectory_id);
00035 response->set_timestamp(common::ToUniversal(local_slam_result->time));
00036 *response->mutable_local_pose() =
00037 transform::ToProto(local_slam_result->local_pose);
00038 if (local_slam_result->range_data) {
00039 *response->mutable_range_data() =
00040 sensor::ToProto(*local_slam_result->range_data);
00041 }
00042 if (local_slam_result->insertion_result) {
00043 local_slam_result->insertion_result->node_id.ToProto(
00044 response->mutable_insertion_result()->mutable_node_id());
00045 }
00046 return response;
00047 }
00048
00049 }
00050
00051 void ReceiveLocalSlamResultsHandler::OnRequest(
00052 const proto::ReceiveLocalSlamResultsRequest& request) {
00053 auto writer = GetWriter();
00054 MapBuilderContextInterface::LocalSlamSubscriptionId subscription_id =
00055 GetUnsynchronizedContext<MapBuilderContextInterface>()
00056 ->SubscribeLocalSlamResults(
00057 request.trajectory_id(),
00058 [writer](
00059 std::unique_ptr<MapBuilderContextInterface::LocalSlamResult>
00060 local_slam_result) {
00061 if (local_slam_result) {
00062 if (!writer.Write(
00063 GenerateResponse(std::move(local_slam_result)))) {
00064
00065 LOG(INFO) << "Client closed connection.";
00066 return false;
00067 }
00068 } else {
00069
00070
00071 writer.WritesDone();
00072 }
00073 return true;
00074 });
00075
00076 subscription_id_ =
00077 absl::make_unique<MapBuilderContextInterface::LocalSlamSubscriptionId>(
00078 subscription_id);
00079 }
00080
00081 void ReceiveLocalSlamResultsHandler::OnFinish() {
00082 if (subscription_id_) {
00083 GetUnsynchronizedContext<MapBuilderContextInterface>()
00084 ->UnsubscribeLocalSlamResults(*subscription_id_);
00085 }
00086 }
00087
00088 }
00089 }
00090 }