Implement IsTrajectory{Finished,Frozen} in cloud mapping (#1057)

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<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses() {
 }
 
 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
-  LOG(FATAL) << "Not implemented";
+  proto::IsTrajectoryFinishedRequest request;
+  request.set_trajectory_id(trajectory_id);
+  async_grpc::Client<handlers::IsTrajectoryFinishedSignature> 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<handlers::IsTrajectoryFrozenSignature> client(
+      client_channel_);
+  CHECK(client.Write(request));
+  return client.response().is_frozen();
 }
 
 std::map<int, mapping::PoseGraphInterface::TrajectoryData>

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();
 }
 

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<proto::IsTrajectoryFinishedResponse>();
+  response->set_is_finished(
+      GetContext<MapBuilderContextInterface>()
+          ->map_builder()
+          .pose_graph()
+          ->IsTrajectoryFinished(request.trajectory_id()));
+  Send(std::move(response));
+}
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer

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<IsTrajectoryFinishedSignature> {
+ public:
+  void OnRequest(const proto::IsTrajectoryFinishedRequest& request) override;
+};
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H

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<proto::IsTrajectoryFrozenResponse>();
+  response->set_is_frozen(GetContext<MapBuilderContextInterface>()
+                              ->map_builder()
+                              .pose_graph()
+                              ->IsTrajectoryFrozen(request.trajectory_id()));
+  Send(std::move(response));
+}
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer

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<IsTrajectoryFrozenSignature> {
+ public:
+  void OnRequest(const proto::IsTrajectoryFrozenRequest& request) override;
+};
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H

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<handlers::GetAllSubmapPosesHandler>();
   server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();
   server_builder.RegisterHandler<handlers::GetConstraintsHandler>();
+  server_builder.RegisterHandler<handlers::IsTrajectoryFinishedHandler>();
+  server_builder.RegisterHandler<handlers::IsTrajectoryFrozenHandler>();
   server_builder.RegisterHandler<handlers::LoadStateHandler>();
   server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>();
   server_builder.RegisterHandler<handlers::WriteStateHandler>();

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);