From d240261701a670fa9d8565cdd437803f39cf9e5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Mon, 8 Jan 2018 15:34:00 +0100 Subject: [PATCH] Add GetAllSubmapPoses to gRPC interface. (#791) --- .../handlers/get_all_submap_poses.h | 54 +++++++++++++++++++ cartographer_grpc/map_builder_server.cc | 3 ++ cartographer_grpc/mapping/pose_graph_stub.cc | 18 ++++++- .../proto/map_builder_service.proto | 14 +++++ 4 files changed, 88 insertions(+), 1 deletion(-) create mode 100644 cartographer_grpc/handlers/get_all_submap_poses.h diff --git a/cartographer_grpc/handlers/get_all_submap_poses.h b/cartographer_grpc/handlers/get_all_submap_poses.h new file mode 100644 index 0000000..81d17b1 --- /dev/null +++ b/cartographer_grpc/handlers/get_all_submap_poses.h @@ -0,0 +1,54 @@ +/* + * 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_GET_ALL_SUBMAP_POSES_H +#define CARTOGRAPHER_GRPC_HANDLERS_GET_ALL_SUBMAP_POSES_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 GetAllSubmapPosesHandler + : public framework::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override { + auto submap_poses = GetContext() + ->map_builder() + .pose_graph() + ->GetAllSubmapPoses(); + auto response = + cartographer::common::make_unique(); + for (const auto& submap_id_pose : submap_poses) { + auto* submap_pose = response->add_submap_poses(); + submap_id_pose.id.ToProto(submap_pose->mutable_submap_id()); + submap_pose->set_submap_version(submap_id_pose.data.version); + *submap_pose->mutable_global_pose() = + cartographer::transform::ToProto(submap_id_pose.data.pose); + } + Send(std::move(response)); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_GET_ALL_SUBMAP_POSES_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index 1eef78f..170b085 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/get_all_submap_poses.h" #include "cartographer_grpc/handlers/get_submap_handler.h" #include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h" #include "cartographer_grpc/handlers/receive_local_slam_results_handler.h" @@ -124,6 +125,8 @@ MapBuilderServer::MapBuilderServer( server_builder.RegisterHandler( "GetTrajectoryNodePoses"); + server_builder.RegisterHandler("GetAllSubmapPoses"); grpc_server_ = server_builder.Build(); grpc_server_->SetExecutionContext( cartographer::common::make_unique(this)); diff --git a/cartographer_grpc/mapping/pose_graph_stub.cc b/cartographer_grpc/mapping/pose_graph_stub.cc index 3314338..af08230 100644 --- a/cartographer_grpc/mapping/pose_graph_stub.cc +++ b/cartographer_grpc/mapping/pose_graph_stub.cc @@ -38,7 +38,23 @@ cartographer::mapping::MapById< cartographer::mapping::SubmapId, cartographer::mapping::PoseGraphInterface::SubmapPose> PoseGraphStub::GetAllSubmapPoses() { - LOG(FATAL) << "Not implemented"; + grpc::ClientContext client_context; + google::protobuf::Empty request; + proto::GetAllSubmapPosesResponse response; + stub_->GetAllSubmapPoses(&client_context, request, &response); + cartographer::mapping::MapById< + cartographer::mapping::SubmapId, + cartographer::mapping::PoseGraphInterface::SubmapPose> + submap_poses; + for (const auto& submap_pose : response.submap_poses()) { + submap_poses.Insert( + cartographer::mapping::SubmapId{submap_pose.submap_id().trajectory_id(), + submap_pose.submap_id().submap_index()}, + cartographer::mapping::PoseGraphInterface::SubmapPose{ + submap_pose.submap_version(), + cartographer::transform::ToRigid3(submap_pose.global_pose())}); + } + return submap_poses; } cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform( diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index b1c5fc4..09ea6c6 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/cartographer_grpc/proto/map_builder_service.proto @@ -93,6 +93,16 @@ message GetTrajectoryNodePosesResponse { repeated TrajectoryNodePose node_poses = 1; } +message SubmapPose { + cartographer.mapping.proto.SubmapId submap_id = 1; + int32 submap_version = 2; + cartographer.transform.proto.Rigid3d global_pose = 3; +} + +message GetAllSubmapPosesResponse { + repeated SubmapPose submap_poses = 1; +} + service MapBuilderService { // Starts a new trajectory and returns its index. rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse); @@ -128,4 +138,8 @@ service MapBuilderService { // Returns the current optimized trajectory poses. rpc GetTrajectoryNodePoses(google.protobuf.Empty) returns (GetTrajectoryNodePosesResponse); + + // Returns the current optimized submap poses. + rpc GetAllSubmapPoses(google.protobuf.Empty) + returns (GetAllSubmapPosesResponse); }