diff --git a/cartographer_grpc/handlers/get_local_to_global_transform_handler.h b/cartographer_grpc/handlers/get_local_to_global_transform_handler.h new file mode 100644 index 0000000..6c4c2fc --- /dev/null +++ b/cartographer_grpc/handlers/get_local_to_global_transform_handler.h @@ -0,0 +1,45 @@ +/* + * 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_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H +#define CARTOGRAPHER_GRPC_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H + +namespace cartographer_grpc { +namespace handlers { + +class GetLocalToGlobalTransformHandler + : public framework::RpcHandler { + public: + void OnRequest( + const proto::GetLocalToGlobalTransformRequest& request) override { + auto response = cartographer::common::make_unique< + proto::GetLocalToGlobalTransformResponse>(); + auto local_to_global = + GetContext() + ->map_builder() + .pose_graph() + ->GetLocalToGlobalTransform(request.trajectory_id()); + *response->mutable_local_to_global() = + cartographer::transform::ToProto(local_to_global); + Send(std::move(response)); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index 170b085..7f56c89 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -23,6 +23,7 @@ #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_local_to_global_transform_handler.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" @@ -127,6 +128,9 @@ MapBuilderServer::MapBuilderServer( "GetTrajectoryNodePoses"); server_builder.RegisterHandler("GetAllSubmapPoses"); + server_builder.RegisterHandler( + "GetLocalToGlobalTransform"); 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 af08230..6c9614c 100644 --- a/cartographer_grpc/mapping/pose_graph_stub.cc +++ b/cartographer_grpc/mapping/pose_graph_stub.cc @@ -59,7 +59,13 @@ PoseGraphStub::GetAllSubmapPoses() { cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform( int trajectory_id) { - LOG(FATAL) << "Not implemented"; + grpc::ClientContext client_context; + proto::GetLocalToGlobalTransformRequest request; + request.set_trajectory_id(trajectory_id); + proto::GetLocalToGlobalTransformResponse response; + CHECK(stub_->GetLocalToGlobalTransform(&client_context, request, &response) + .ok()); + return cartographer::transform::ToRigid3(response.local_to_global()); } cartographer::mapping::MapById