Endpoints for GetTrajectoryStates (#1206)

[RFC=0023](https://github.com/googlecartographer/rfcs/blob/master/text/0023-delete-load.md)

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -22,10 +22,12 @@
 #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/get_trajectory_states_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/cloud/internal/handlers/set_landmark_pose_handler.h"
+#include "cartographer/cloud/internal/mapping/serialization.h"
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
@@ -108,7 +110,16 @@ PoseGraphStub::GetTrajectoryNodePoses() const {
 
 std::map<int, mapping::PoseGraphInterface::TrajectoryState>
 PoseGraphStub::GetTrajectoryStates() const {
-  LOG(FATAL) << "not implemented";
+  google::protobuf::Empty request;
+  async_grpc::Client<handlers::GetTrajectoryStatesSignature> client(
+      client_channel_);
+  CHECK(client.Write(request));
+  std::map<int, mapping::PoseGraphInterface::TrajectoryState>
+      trajectories_state;
+  for (const auto& entry : client.response().trajectories_state()) {
+    trajectories_state[entry.first] = FromProto(entry.second);
+  }
+  return trajectories_state;
 }
 
 std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses()

cartographer/cloud/internal/client_server_test.cc

@@ -297,6 +297,7 @@ TEST_F(ClientServerTest, LocalSlam2D) {
   InitializeRealServer();
   server_->Start();
   InitializeStub();
+  EXPECT_TRUE(stub_->pose_graph()->GetTrajectoryStates().empty());
   int trajectory_id =
       stub_->AddTrajectoryBuilder({kRangeSensorId}, trajectory_builder_options_,
                                   local_slam_result_callback_);
@@ -308,7 +309,12 @@ TEST_F(ClientServerTest, LocalSlam2D) {
     trajectory_stub->AddSensorData(kRangeSensorId.id, measurement);
   }
   WaitForLocalSlamResults(measurements.size());
+  EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id),
+            PoseGraphInterface::TrajectoryState::ACTIVE);
   stub_->FinishTrajectory(trajectory_id);
+  stub_->pose_graph()->RunFinalOptimization();
+  EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id),
+            PoseGraphInterface::TrajectoryState::FINISHED);
   EXPECT_EQ(local_slam_result_poses_.size(), measurements.size());
   EXPECT_NEAR(kTravelDistance,
               (local_slam_result_poses_.back().translation() -
@@ -334,16 +340,15 @@ TEST_F(ClientServerTest, LocalSlamAndDelete2D) {
   }
   WaitForLocalSlamResults(measurements.size());
   stub_->pose_graph()->RunFinalOptimization();
-  // TODO(gaschler): Enable after pending PR has merged.
-  // EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id),
-  //          PoseGraphInterface::TrajectoryState::ACTIVE);
+  EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id),
+            PoseGraphInterface::TrajectoryState::ACTIVE);
   EXPECT_GT(stub_->pose_graph()->GetAllSubmapPoses().size(), 0);
   EXPECT_GT(stub_->pose_graph()->GetTrajectoryNodePoses().size(), 0);
   stub_->FinishTrajectory(trajectory_id);
   stub_->pose_graph()->DeleteTrajectory(trajectory_id);
   stub_->pose_graph()->RunFinalOptimization();
-  // EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id),
-  //          PoseGraphInterface::TrajectoryState::DELETED);
+  EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id),
+            PoseGraphInterface::TrajectoryState::DELETED);
   EXPECT_EQ(stub_->pose_graph()->GetAllSubmapPoses().size(), 0);
   EXPECT_EQ(stub_->pose_graph()->GetTrajectoryNodePoses().size(), 0);
   server_->Shutdown();

cartographer/cloud/internal/handlers/get_trajectory_states_handler.cc

@@ -0,0 +1,46 @@
+/*
+ * 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/get_trajectory_states_handler.h"
+
+#include "async_grpc/rpc_handler.h"
+#include "cartographer/cloud/internal/map_builder_context_interface.h"
+#include "cartographer/cloud/internal/mapping/serialization.h"
+#include "cartographer/cloud/proto/map_builder_service.pb.h"
+#include "cartographer/common/make_unique.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer {
+namespace cloud {
+namespace handlers {
+
+void GetTrajectoryStatesHandler::OnRequest(
+    const google::protobuf::Empty& request) {
+  auto trajectories_state = GetContext<MapBuilderContextInterface>()
+                                ->map_builder()
+                                .pose_graph()
+                                ->GetTrajectoryStates();
+  auto response = common::make_unique<proto::GetTrajectoryStatesResponse>();
+  for (const auto& entry : trajectories_state) {
+    (*response->mutable_trajectories_state())[entry.first] =
+        ToProto(entry.second);
+  }
+  Send(std::move(response));
+}
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer

cartographer/cloud/internal/handlers/get_trajectory_states_handler.h

@@ -0,0 +1,43 @@
+/*
+ * 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_GET_TRAJECTORY_STATES_HANDLER_H
+#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_STATES_HANDLER_H
+
+#include "async_grpc/rpc_handler.h"
+#include "cartographer/cloud/proto/map_builder_service.pb.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer {
+namespace cloud {
+namespace handlers {
+
+DEFINE_HANDLER_SIGNATURE(
+    GetTrajectoryStatesSignature, google::protobuf::Empty,
+    proto::GetTrajectoryStatesResponse,
+    "/cartographer.cloud.proto.MapBuilderService/GetTrajectoryStates")
+
+class GetTrajectoryStatesHandler
+    : public async_grpc::RpcHandler<GetTrajectoryStatesSignature> {
+ public:
+  void OnRequest(const google::protobuf::Empty& request) override;
+};
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_STATES_HANDLER_H

cartographer/cloud/internal/map_builder_server.cc

@@ -31,6 +31,7 @@
 #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/get_trajectory_states_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_from_file_handler.h"
@@ -82,6 +83,7 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::ReceiveLocalSlamResultsHandler>();
   server_builder.RegisterHandler<handlers::GetSubmapHandler>();
   server_builder.RegisterHandler<handlers::GetTrajectoryNodePosesHandler>();
+  server_builder.RegisterHandler<handlers::GetTrajectoryStatesHandler>();
   server_builder.RegisterHandler<handlers::GetLandmarkPosesHandler>();
   server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>();
   server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();

cartographer/cloud/internal/mapping/serialization.cc

@@ -20,6 +20,42 @@
 
 namespace cartographer {
 namespace cloud {
+namespace {
+
+using TrajectoryState =
+    ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
+
+}  // namespace
+
+proto::TrajectoryState ToProto(const TrajectoryState& trajectory_state) {
+  switch (trajectory_state) {
+    case TrajectoryState::ACTIVE:
+      return proto::TrajectoryState::ACTIVE;
+    case TrajectoryState::FINISHED:
+      return proto::TrajectoryState::FINISHED;
+    case TrajectoryState::FROZEN:
+      return proto::TrajectoryState::FROZEN;
+    case TrajectoryState::DELETED:
+      return proto::TrajectoryState::DELETED;
+    default:
+      LOG(FATAL) << "Unknown TrajectoryState";
+  }
+}
+
+TrajectoryState FromProto(const proto::TrajectoryState& proto) {
+  switch (proto) {
+    case proto::TrajectoryState::ACTIVE:
+      return TrajectoryState::ACTIVE;
+    case proto::TrajectoryState::FINISHED:
+      return TrajectoryState::FINISHED;
+    case proto::TrajectoryState::FROZEN:
+      return TrajectoryState::FROZEN;
+    case proto::TrajectoryState::DELETED:
+      return TrajectoryState::DELETED;
+    default:
+      LOG(FATAL) << "Unknown proto::TrajectoryState";
+  }
+}
 
 proto::TrajectoryRemapping ToProto(
     const std::map<int, int>& trajectory_remapping) {

cartographer/cloud/internal/mapping/serialization.h

@@ -18,10 +18,16 @@
 #define CARTOGRAPHER_CLOUD_INTERNAL_MAPPING_SERIALIZATION_H
 
 #include "cartographer/cloud/proto/map_builder_service.pb.h"
+#include "cartographer/mapping/pose_graph_interface.h"
 
 namespace cartographer {
 namespace cloud {
 
+proto::TrajectoryState ToProto(
+    const mapping::PoseGraphInterface::TrajectoryState& trajectory_state);
+mapping::PoseGraphInterface::TrajectoryState FromProto(
+    const proto::TrajectoryState& proto);
+
 proto::TrajectoryRemapping ToProto(
     const std::map<int, int>& trajectory_remapping);
 std::map<int, int> FromProto(const proto::TrajectoryRemapping& proto);

cartographer/cloud/proto/map_builder_service.proto

@@ -33,6 +33,13 @@ enum SensorType {
   LOCAL_SLAM_RESULT = 5;
 }
 
+enum TrajectoryState {
+  ACTIVE = 0;
+  FINISHED = 1;
+  FROZEN = 2;
+  DELETED = 3;
+}
+
 message SensorId {
   string id = 1;
   SensorType type = 2;
@@ -172,6 +179,10 @@ message GetTrajectoryNodePosesResponse {
   repeated TrajectoryNodePose node_poses = 1;
 }
 
+message GetTrajectoryStatesResponse {
+  map<int32 /* trajectory_id */, TrajectoryState> trajectories_state = 1;
+}
+
 message GetLandmarkPosesResponse {
   repeated cartographer.mapping.proto.PoseGraph.LandmarkPose landmark_poses = 1;
 }
@@ -276,6 +287,10 @@ service MapBuilderService {
   rpc GetTrajectoryNodePoses(google.protobuf.Empty)
       returns (GetTrajectoryNodePosesResponse);
 
+  // Returns the states of trajectories.
+  rpc GetTrajectoryStates(google.protobuf.Empty)
+      returns (GetTrajectoryStatesResponse);
+
   // Returns the current optimized landmark poses.
   rpc GetLandmarkPoses(google.protobuf.Empty)
       returns (GetLandmarkPosesResponse);