Add a gRPC version of SetLandmarkPose(). (#1068)

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -24,6 +24,7 @@
 #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/mapping/pose_graph.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
@@ -111,7 +112,13 @@ std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses() {
 
 void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id,
                                     const transform::Rigid3d& global_pose) {
-  LOG(FATAL) << "Not implemented";
+  proto::SetLandmarkPoseRequest request;
+  request.mutable_landmark_pose()->set_landmark_id(landmark_id);
+  *request.mutable_landmark_pose()->mutable_global_pose() =
+      transform::ToProto(global_pose);
+  async_grpc::Client<handlers::SetLandmarkPoseSignature> client(
+      client_channel_);
+  CHECK(client.Write(request));
 }
 
 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {

cartographer/cloud/internal/handlers/set_landmark_pose_handler.cc

@@ -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.
+ */
+
+#include "cartographer/cloud/internal/handlers/set_landmark_pose_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/transform/transform.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer {
+namespace cloud {
+namespace handlers {
+
+void SetLandmarkPoseHandler::OnRequest(
+    const proto::SetLandmarkPoseRequest& request) {
+  GetContext<MapBuilderContextInterface>()
+      ->map_builder()
+      .pose_graph()
+      ->SetLandmarkPose(
+          request.landmark_pose().landmark_id(),
+          transform::ToRigid3(request.landmark_pose().global_pose()));
+  Send(common::make_unique<google::protobuf::Empty>());
+}
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer

cartographer/cloud/internal/handlers/set_landmark_pose_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_ADD_SET_LANDMARK_POSE_HANDLER_H
+#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SET_LANDMARK_POSE_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(
+    SetLandmarkPoseSignature, proto::SetLandmarkPoseRequest,
+    google::protobuf::Empty,
+    "/cartographer.cloud.proto.MapBuilderService/SetLandmarkPose")
+
+class SetLandmarkPoseHandler
+    : public async_grpc::RpcHandler<SetLandmarkPoseSignature> {
+ public:
+  void OnRequest(const proto::SetLandmarkPoseRequest &request) override;
+};
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SET_LANDMARK_POSE_HANDLER_H

cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc

@@ -0,0 +1,63 @@
+/*
+ * 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/set_landmark_pose_handler.h"
+#include "cartographer/cloud/internal/testing/handler_test.h"
+#include "cartographer/cloud/internal/testing/test_helpers.h"
+#include "cartographer/transform/rigid_transform_test_helpers.h"
+#include "google/protobuf/text_format.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace cloud {
+namespace handlers {
+namespace {
+
+const std::string kMessage = R"(
+  landmark_pose {
+    landmark_id: "landmark_1"
+    global_pose {
+      translation {
+        x: 1 y: 2 z: 3
+      }
+      rotation {
+        w: 4 x: 5 y: 6 z: 7
+      }
+    }
+  })";
+
+using SetLandmarkPoseHandlerTest =
+    testing::HandlerTest<SetLandmarkPoseSignature, SetLandmarkPoseHandler>;
+
+TEST_F(SetLandmarkPoseHandlerTest, SetLandmarkPose) {
+  constexpr double kEps = 1e-10;
+  proto::SetLandmarkPoseRequest request;
+  EXPECT_TRUE(
+      google::protobuf::TextFormat::ParseFromString(kMessage, &request));
+  EXPECT_CALL(
+      *mock_pose_graph_,
+      SetLandmarkPose("landmark_1",
+                      transform::IsNearly(
+                          transform::Rigid3d(Eigen::Vector3d(1, 2, 3),
+                                             Eigen::Quaterniond(4, 5, 6, 7)),
+                          kEps)));
+  test_server_->SendWrite(request);
+}
+
+}  // namespace
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer

cartographer/cloud/internal/map_builder_server.cc

@@ -35,6 +35,7 @@
 #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"
+#include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h"
 #include "cartographer/cloud/internal/handlers/write_state_handler.h"
 #include "cartographer/cloud/internal/sensor/serialization.h"
 #include "glog/logging.h"
@@ -81,6 +82,7 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::LoadStateHandler>();
   server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>();
   server_builder.RegisterHandler<handlers::WriteStateHandler>();
+  server_builder.RegisterHandler<handlers::SetLandmarkPoseHandler>();
   grpc_server_ = server_builder.Build();
   if (map_builder_server_options.map_builder_options()
           .use_trajectory_builder_2d()) {

cartographer/cloud/proto/map_builder_service.proto

@@ -126,13 +126,12 @@ message GetTrajectoryNodePosesResponse {
   repeated TrajectoryNodePose node_poses = 1;
 }
 
-message LandmarkPose {
-  string landmark_id = 1;
-  cartographer.transform.proto.Rigid3d global_pose = 2;
+message GetLandmarkPosesResponse {
+  repeated cartographer.mapping.proto.PoseGraph.LandmarkPose landmark_poses = 1;
 }
 
-message GetLandmarkPosesResponse {
-  repeated LandmarkPose landmark_poses = 1;
+message SetLandmarkPoseRequest {
+  cartographer.mapping.proto.PoseGraph.LandmarkPose landmark_pose = 1;
 }
 
 message SubmapPose {
@@ -246,6 +245,9 @@ service MapBuilderService {
   // Returns the list of constraints in the current optimization problem.
   rpc GetConstraints(google.protobuf.Empty) returns (GetConstraintsResponse);
 
+  // Sets a pose for a landmark.
+  rpc SetLandmarkPose(SetLandmarkPoseRequest) returns (google.protobuf.Empty);
+
   // Checks whether the trajectory is finished.
   rpc IsTrajectoryFinished(IsTrajectoryFinishedRequest)
       returns (IsTrajectoryFinishedResponse);

cartographer/mapping/internal/3d/pose_graph_3d_test.cc

@@ -129,7 +129,7 @@ TEST_F(PoseGraph3DTest, BasicSerialization) {
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
       proto.trajectory(0), actual_proto.trajectory(0)));
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
-      proto.landmarks(0), actual_proto.landmarks(0)));
+      proto.landmark_poses(0), actual_proto.landmark_poses(0)));
   EXPECT_TRUE(
       google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto));
 }

cartographer/mapping/internal/testing/test_helpers.cc

@@ -120,9 +120,9 @@ proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id,
   return trajectory;
 }
 
-proto::PoseGraph::Landmark CreateFakeLandmark(
+proto::PoseGraph::LandmarkPose CreateFakeLandmark(
     const std::string& landmark_id, const transform::Rigid3d& global_pose) {
-  proto::PoseGraph::Landmark landmark;
+  proto::PoseGraph::LandmarkPose landmark;
   landmark.set_landmark_id(landmark_id);
   *landmark.mutable_global_pose() = transform::ToProto(global_pose);
   return landmark;
@@ -156,9 +156,9 @@ void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint,
   *pose_graph->add_constraint() = constraint;
 }
 
-void AddToProtoGraph(const proto::PoseGraph::Landmark& landmark,
+void AddToProtoGraph(const proto::PoseGraph::LandmarkPose& landmark,
                      proto::PoseGraph* pose_graph) {
-  *pose_graph->add_landmarks() = landmark;
+  *pose_graph->add_landmark_poses() = landmark;
 }
 
 }  // namespace test

cartographer/mapping/internal/testing/test_helpers.h

@@ -43,7 +43,7 @@ proto::PoseGraph::Constraint CreateFakeConstraint(const proto::Node& node,
 
 proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id,
                                             proto::PoseGraph* pose_graph);
-proto::PoseGraph::Landmark CreateFakeLandmark(
+proto::PoseGraph::LandmarkPose CreateFakeLandmark(
     const std::string& landmark_id, const transform::Rigid3d& global_pose);
 
 void AddToProtoGraph(const proto::Node& node_data,
@@ -55,7 +55,7 @@ void AddToProtoGraph(const proto::Submap& submap_data,
 void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint,
                      proto::PoseGraph* pose_graph);
 
-void AddToProtoGraph(const proto::PoseGraph::Landmark& landmark_node,
+void AddToProtoGraph(const proto::PoseGraph::LandmarkPose& landmark_node,
                      proto::PoseGraph* pose_graph);
 
 }  // namespace test

cartographer/mapping/map_builder.cc

@@ -369,7 +369,7 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
   }
 
   // Set global poses of landmarks.
-  for (const auto& landmark : pose_graph_proto.landmarks()) {
+  for (const auto& landmark : pose_graph_proto.landmark_poses()) {
     pose_graph_->SetLandmarkPose(landmark.landmark_id(),
                                  transform::ToRigid3(landmark.global_pose()));
   }

cartographer/mapping/pose_graph.cc

@@ -156,9 +156,9 @@ proto::PoseGraph PoseGraph::ToProto() {
   }
 
   auto landmarks_copy = GetLandmarkPoses();
-  proto.mutable_landmarks()->Reserve(landmarks_copy.size());
+  proto.mutable_landmark_poses()->Reserve(landmarks_copy.size());
   for (const auto& id_pose : landmarks_copy) {
-    auto* landmark_proto = proto.add_landmarks();
+    auto* landmark_proto = proto.add_landmark_poses();
     landmark_proto->set_landmark_id(id_pose.first);
     *landmark_proto->mutable_global_pose() = transform::ToProto(id_pose.second);
   }

cartographer/mapping/proto/pose_graph.proto

@@ -51,12 +51,12 @@ message PoseGraph {
     Tag tag = 5;
   }
 
-  message Landmark {
+  message LandmarkPose {
     string landmark_id = 1;
     transform.proto.Rigid3d global_pose = 2;
   }
 
   repeated Constraint constraint = 2;
   repeated Trajectory trajectory = 4;
-  repeated Landmark landmarks = 5;
+  repeated LandmarkPose landmark_poses = 5;
 }