diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index fc9b577..70bb921 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/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 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 client( + client_channel_); + CHECK(client.Write(request)); } bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) { diff --git a/cartographer/cloud/internal/handlers/set_landmark_pose_handler.cc b/cartographer/cloud/internal/handlers/set_landmark_pose_handler.cc new file mode 100644 index 0000000..ad457d3 --- /dev/null +++ b/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() + ->map_builder() + .pose_graph() + ->SetLandmarkPose( + request.landmark_pose().landmark_id(), + transform::ToRigid3(request.landmark_pose().global_pose())); + Send(common::make_unique()); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/cartographer/cloud/internal/handlers/set_landmark_pose_handler.h b/cartographer/cloud/internal/handlers/set_landmark_pose_handler.h new file mode 100644 index 0000000..9fea303 --- /dev/null +++ b/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 { + 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 diff --git a/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc b/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc new file mode 100644 index 0000000..c1f1094 --- /dev/null +++ b/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; + +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 diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc index 5a4ee06..ec6d6ba 100644 --- a/cartographer/cloud/internal/map_builder_server.cc +++ b/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(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); + server_builder.RegisterHandler(); grpc_server_ = server_builder.Build(); if (map_builder_server_options.map_builder_options() .use_trajectory_builder_2d()) { diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index 29e8205..e5461c2 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/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); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index c94f72a..a2ea75e 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/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)); } diff --git a/cartographer/mapping/internal/testing/test_helpers.cc b/cartographer/mapping/internal/testing/test_helpers.cc index 5890e7a..a104fb6 100644 --- a/cartographer/mapping/internal/testing/test_helpers.cc +++ b/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 diff --git a/cartographer/mapping/internal/testing/test_helpers.h b/cartographer/mapping/internal/testing/test_helpers.h index 255e9b0..5b0f2d1 100644 --- a/cartographer/mapping/internal/testing/test_helpers.h +++ b/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 diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 8f74c0b..ead1931 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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())); } diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index 9750fab..e5b843b 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/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); } diff --git a/cartographer/mapping/proto/pose_graph.proto b/cartographer/mapping/proto/pose_graph.proto index d34ee1d..76c10b8 100644 --- a/cartographer/mapping/proto/pose_graph.proto +++ b/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; }