From f83d0a6ed635484ffad6afaefcfe295493b5319e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Thu, 12 Apr 2018 12:37:04 +0200 Subject: [PATCH] Implement IsTrajectory{Finished,Frozen} in cloud mapping (#1057) --- .../cloud/internal/client/pose_graph_stub.cc | 16 ++++++- .../cloud/internal/client_server_test.cc | 3 ++ .../is_trajectory_finished_handler.cc | 41 ++++++++++++++++++ .../handlers/is_trajectory_finished_handler.h | 42 +++++++++++++++++++ .../handlers/is_trajectory_frozen_handler.cc | 40 ++++++++++++++++++ .../handlers/is_trajectory_frozen_handler.h | 42 +++++++++++++++++++ .../cloud/internal/map_builder_server.cc | 4 ++ .../cloud/proto/map_builder_service.proto | 24 +++++++++++ 8 files changed, 210 insertions(+), 2 deletions(-) create mode 100644 cartographer/cloud/internal/handlers/is_trajectory_finished_handler.cc create mode 100644 cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h create mode 100644 cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.cc create mode 100644 cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index bbbce1a..6356473 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -21,6 +21,8 @@ #include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h" #include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h" #include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h" +#include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" +#include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" #include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/transform/transform.h" @@ -108,11 +110,21 @@ std::map PoseGraphStub::GetLandmarkPoses() { } bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) { - LOG(FATAL) << "Not implemented"; + proto::IsTrajectoryFinishedRequest request; + request.set_trajectory_id(trajectory_id); + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + return client.response().is_finished(); } bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) { - LOG(FATAL) << "Not implemented"; + proto::IsTrajectoryFrozenRequest request; + request.set_trajectory_id(trajectory_id); + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + return client.response().is_frozen(); } std::map diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 38deb23..c91f95d 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -140,7 +140,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilder) { InitializeStub(); int trajectory_id = stub_->AddTrajectoryBuilder( {kImuSensorId}, trajectory_builder_options_, nullptr); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id)); stub_->FinishTrajectory(trajectory_id); + EXPECT_TRUE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id)); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(trajectory_id)); server_->Shutdown(); } diff --git a/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.cc b/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.cc new file mode 100644 index 0000000..8537b9b --- /dev/null +++ b/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.cc @@ -0,0 +1,41 @@ +/* + * 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. + */ + +#include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/common/make_unique.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void IsTrajectoryFinishedHandler::OnRequest( + const proto::IsTrajectoryFinishedRequest& request) { + auto response = common::make_unique(); + response->set_is_finished( + GetContext() + ->map_builder() + .pose_graph() + ->IsTrajectoryFinished(request.trajectory_id())); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h b/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h new file mode 100644 index 0000000..5595a20 --- /dev/null +++ b/cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h @@ -0,0 +1,42 @@ +/* + * 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_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + IsTrajectoryFinishedSignature, proto::IsTrajectoryFinishedRequest, + proto::IsTrajectoryFinishedResponse, + "/cartographer.cloud.proto.MapBuilderService/IsTrajectoryFinished") + +class IsTrajectoryFinishedHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::IsTrajectoryFinishedRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H diff --git a/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.cc b/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.cc new file mode 100644 index 0000000..ede3175 --- /dev/null +++ b/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.cc @@ -0,0 +1,40 @@ +/* + * 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. + */ + +#include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/common/make_unique.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void IsTrajectoryFrozenHandler::OnRequest( + const proto::IsTrajectoryFrozenRequest& request) { + auto response = common::make_unique(); + response->set_is_frozen(GetContext() + ->map_builder() + .pose_graph() + ->IsTrajectoryFrozen(request.trajectory_id())); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h b/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h new file mode 100644 index 0000000..6a5d892 --- /dev/null +++ b/cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h @@ -0,0 +1,42 @@ +/* + * 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_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + IsTrajectoryFrozenSignature, proto::IsTrajectoryFrozenRequest, + proto::IsTrajectoryFrozenResponse, + "/cartographer.cloud.proto.MapBuilderService/IsTrajectoryFrozen") + +class IsTrajectoryFrozenHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::IsTrajectoryFrozenRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc index 3d66068..7fcf4be 100644 --- a/cartographer/cloud/internal/map_builder_server.cc +++ b/cartographer/cloud/internal/map_builder_server.cc @@ -30,6 +30,8 @@ #include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h" #include "cartographer/cloud/internal/handlers/get_submap_handler.h" #include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h" +#include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" +#include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" #include "cartographer/cloud/internal/handlers/load_state_handler.h" #include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h" #include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h" @@ -74,6 +76,8 @@ MapBuilderServer::MapBuilderServer( server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); + server_builder.RegisterHandler(); + server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index 4486e97..29e8205 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/cartographer/cloud/proto/map_builder_service.proto @@ -171,6 +171,22 @@ message WriteStateResponse { } } +message IsTrajectoryFinishedRequest { + int32 trajectory_id = 1; +} + +message IsTrajectoryFinishedResponse { + bool is_finished = 1; +} + +message IsTrajectoryFrozenRequest { + int32 trajectory_id = 1; +} + +message IsTrajectoryFrozenResponse { + bool is_frozen = 1; +} + service MapBuilderService { // Starts a new trajectory and returns its index. rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse); @@ -230,6 +246,14 @@ service MapBuilderService { // Returns the list of constraints in the current optimization problem. rpc GetConstraints(google.protobuf.Empty) returns (GetConstraintsResponse); + // Checks whether the trajectory is finished. + rpc IsTrajectoryFinished(IsTrajectoryFinishedRequest) + returns (IsTrajectoryFinishedResponse); + + // Checks whether the trajectory is frozen. + rpc IsTrajectoryFrozen(IsTrajectoryFrozenRequest) + returns (IsTrajectoryFrozenResponse); + // Requests a PoseGraph to call RunFinalOptimization. rpc RunFinalOptimization(google.protobuf.Empty) returns (google.protobuf.Empty);