From 58bc1ced68769db630545efb95a7721803a6b4ae Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Tue, 6 Feb 2018 18:13:31 +0100 Subject: [PATCH] Implement GetLandmarkPoses method. (#888) [RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md) --- cartographer/mapping/pose_graph.h | 1 - cartographer/mapping/pose_graph_interface.h | 4 + cartographer/mapping_2d/pose_graph.cc | 11 +++ cartographer/mapping_2d/pose_graph.h | 2 + cartographer/mapping_3d/pose_graph.cc | 11 +++ cartographer/mapping_3d/pose_graph.h | 3 +- .../handlers/get_landmark_poses_handler.cc | 46 +++++++++++ .../handlers/get_landmark_poses_handler.h | 40 +++++++++ .../get_landmark_poses_handler_test.cc | 81 +++++++++++++++++++ cartographer_grpc/map_builder_server.cc | 2 + cartographer_grpc/mapping/pose_graph_stub.cc | 14 ++++ cartographer_grpc/mapping/pose_graph_stub.h | 2 + .../proto/map_builder_service.proto | 13 +++ cartographer_grpc/testing/handler_test.h | 15 +++- cartographer_grpc/testing/mock_pose_graph.h | 60 ++++++++++++++ 15 files changed, 302 insertions(+), 3 deletions(-) create mode 100644 cartographer_grpc/handlers/get_landmark_poses_handler.cc create mode 100644 cartographer_grpc/handlers/get_landmark_poses_handler.h create mode 100644 cartographer_grpc/handlers/get_landmark_poses_handler_test.cc create mode 100644 cartographer_grpc/testing/mock_pose_graph.h diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 4f647ac..e445ae0 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -37,7 +37,6 @@ #include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/odometry_data.h" -#include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 9bb8649..85f4148 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -22,6 +22,7 @@ #include "cartographer/common/optional.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/submaps.h" +#include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping { @@ -98,6 +99,9 @@ class PoseGraphInterface { // Returns the current optimized trajectory poses. virtual MapById GetTrajectoryNodePoses() = 0; + // Returns the current optimized landmark poses. + virtual std::map GetLandmarkPoses() = 0; + // Checks if the given trajectory is finished. virtual bool IsTrajectoryFinished(int trajectory_id) = 0; diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index 4db95d0..15272ea 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph.cc @@ -606,6 +606,17 @@ PoseGraph::GetTrajectoryNodePoses() { return node_poses; } +std::map PoseGraph::GetLandmarkPoses() { + std::map landmark_poses; + for (const auto& landmark : landmark_nodes_) { + // Landmark without value has not been optimized yet. + if (!landmark.second.global_landmark_pose.has_value()) continue; + landmark_poses[landmark.first] = + landmark.second.global_landmark_pose.value(); + } + return landmark_poses; +} + sensor::MapByTime PoseGraph::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_.imu_data(); diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h index 7ca5397..fd074f0 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/cartographer/mapping_2d/pose_graph.h @@ -116,6 +116,8 @@ class PoseGraph : public mapping::PoseGraph { GetTrajectoryNodes() override EXCLUDES(mutex_); mapping::MapById GetTrajectoryNodePoses() override EXCLUDES(mutex_); + std::map GetLandmarkPoses() override + EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); sensor::MapByTime GetOdometryData() override EXCLUDES(mutex_); diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc index 0c63ac1..f3b0150 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/cartographer/mapping_3d/pose_graph.cc @@ -635,6 +635,17 @@ PoseGraph::GetTrajectoryNodePoses() { return node_poses; } +std::map PoseGraph::GetLandmarkPoses() { + std::map landmark_poses; + for (const auto& landmark : landmark_nodes_) { + // Landmark without value has not been optimized yet. + if (!landmark.second.global_landmark_pose.has_value()) continue; + landmark_poses[landmark.first] = + landmark.second.global_landmark_pose.value(); + } + return landmark_poses; +} + sensor::MapByTime PoseGraph::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_.imu_data(); diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h index b4c5e82..0cd71f0 100644 --- a/cartographer/mapping_3d/pose_graph.h +++ b/cartographer/mapping_3d/pose_graph.h @@ -42,7 +42,6 @@ #include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/point_cloud.h" -#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" namespace cartographer { @@ -116,6 +115,8 @@ class PoseGraph : public mapping::PoseGraph { GetTrajectoryNodes() override EXCLUDES(mutex_); mapping::MapById GetTrajectoryNodePoses() override EXCLUDES(mutex_); + std::map GetLandmarkPoses() override + EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); sensor::MapByTime GetOdometryData() override EXCLUDES(mutex_); diff --git a/cartographer_grpc/handlers/get_landmark_poses_handler.cc b/cartographer_grpc/handlers/get_landmark_poses_handler.cc new file mode 100644 index 0000000..4faa0e8 --- /dev/null +++ b/cartographer_grpc/handlers/get_landmark_poses_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_grpc/handlers/get_landmark_poses_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void GetLandmarkPosesHandler::OnRequest( + const google::protobuf::Empty& request) { + auto landmark_poses = GetContext() + ->map_builder() + .pose_graph() + ->GetLandmarkPoses(); + auto response = + cartographer::common::make_unique(); + for (const auto& landmark_pose : landmark_poses) { + auto* landmark = response->add_landmark_poses(); + landmark->set_landmark_id(landmark_pose.first); + *landmark->mutable_global_pose() = + cartographer::transform::ToProto(landmark_pose.second); + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/get_landmark_poses_handler.h b/cartographer_grpc/handlers/get_landmark_poses_handler.h new file mode 100644 index 0000000..2494ff7 --- /dev/null +++ b/cartographer_grpc/handlers/get_landmark_poses_handler.h @@ -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. + */ + +#ifndef CARTOGRAPHER_GRPC_HANDLERS_GET_LANDMARK_POSES_HANDLER_H +#define CARTOGRAPHER_GRPC_HANDLERS_GET_LANDMARK_POSES_HANDLER_H + +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +class GetLandmarkPosesHandler + : public framework::RpcHandler { + public: + std::string method_name() const override { + return "/cartographer_grpc.proto.MapBuilderService/GetLandmarkPoses"; + } + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_GET_LANDMARK_POSES_HANDLER_H diff --git a/cartographer_grpc/handlers/get_landmark_poses_handler_test.cc b/cartographer_grpc/handlers/get_landmark_poses_handler_test.cc new file mode 100644 index 0000000..3e40479 --- /dev/null +++ b/cartographer_grpc/handlers/get_landmark_poses_handler_test.cc @@ -0,0 +1,81 @@ +/* + * 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_grpc/handlers/get_landmark_poses_handler.h" +#include "cartographer_grpc/testing/handler_test.h" +#include "cartographer_grpc/testing/test_helpers.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer_grpc { +namespace handlers { +namespace { + +using ::cartographer::transform::Rigid3d; +using ::testing::_; +using ::testing::Eq; +using ::testing::Pointee; +using ::testing::Truly; + +const std::string kMessage = R"PROTO( + landmark_poses { + landmark_id: "landmark_1" + global_pose { + translation { + x: 1 y: 2 z: 3 + } + rotation { + w:1 x: 0 y: 0 z: 0 + } + } + } + landmark_poses { + landmark_id: "landmark_2" + global_pose { + translation { + x: 3 y: 2 z: 1 + } + rotation { + w:0 x: 1 y: 0 z: 0 + } + } + } +)PROTO"; + +using GetLandmarkPosesHandlerTest = + testing::HandlerTest; + +TEST_F(GetLandmarkPosesHandlerTest, NoLocalSlamUploader) { + std::map landmark_poses{ + {"landmark_1", Rigid3d(Eigen::Vector3d(1., 2., 3.), + Eigen::Quaterniond(1., 0., 0., 0.))}, + {"landmark_2", Rigid3d(Eigen::Vector3d(3., 2., 1.), + Eigen::Quaterniond(0., 1., 0., 0.))}}; + EXPECT_CALL(*mock_pose_graph_, GetLandmarkPoses()) + .WillOnce(::testing::Return(landmark_poses)); + test_server_->SendWrite(google::protobuf::Empty()); + + proto::GetLandmarkPosesResponse expected_response; + EXPECT_TRUE(google::protobuf::TextFormat::ParseFromString( + kMessage, &expected_response)); + EXPECT_THAT( + test_server_->response(), + ::testing::Truly(testing::BuildProtoPredicateEquals(&expected_response))); +} + +} // namespace +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index 449d873..0caf0ca 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -26,6 +26,7 @@ #include "cartographer_grpc/handlers/finish_trajectory_handler.h" #include "cartographer_grpc/handlers/get_all_submap_poses.h" #include "cartographer_grpc/handlers/get_constraints_handler.h" +#include "cartographer_grpc/handlers/get_landmark_poses_handler.h" #include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h" #include "cartographer_grpc/handlers/get_submap_handler.h" #include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h" @@ -70,6 +71,7 @@ MapBuilderServer::MapBuilderServer( 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_grpc/mapping/pose_graph_stub.cc b/cartographer_grpc/mapping/pose_graph_stub.cc index e4e5bbd..54d24da 100644 --- a/cartographer_grpc/mapping/pose_graph_stub.cc +++ b/cartographer_grpc/mapping/pose_graph_stub.cc @@ -19,6 +19,7 @@ #include "cartographer_grpc/framework/client.h" #include "cartographer_grpc/handlers/get_all_submap_poses.h" #include "cartographer_grpc/handlers/get_constraints_handler.h" +#include "cartographer_grpc/handlers/get_landmark_poses_handler.h" #include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h" #include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h" #include "cartographer_grpc/handlers/run_final_optimization_handler.h" @@ -103,6 +104,19 @@ PoseGraphStub::GetTrajectoryNodePoses() { return node_poses; } +std::map +PoseGraphStub::GetLandmarkPoses() { + google::protobuf::Empty request; + framework::Client client(client_channel_); + CHECK(client.Write(request)); + std::map landmark_poses; + for (const auto& landmark_pose : client.response().landmark_poses()) { + landmark_poses[landmark_pose.landmark_id()] = + cartographer::transform::ToRigid3(landmark_pose.global_pose()); + } + return landmark_poses; +} + bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) { LOG(FATAL) << "Not implemented"; } diff --git a/cartographer_grpc/mapping/pose_graph_stub.h b/cartographer_grpc/mapping/pose_graph_stub.h index 9f5c740..f4e9f42 100644 --- a/cartographer_grpc/mapping/pose_graph_stub.h +++ b/cartographer_grpc/mapping/pose_graph_stub.h @@ -43,6 +43,8 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface { cartographer::mapping::MapById GetTrajectoryNodePoses() override; + std::map GetLandmarkPoses() + override; bool IsTrajectoryFinished(int trajectory_id) override; std::vector constraints() override; cartographer::mapping::proto::PoseGraph ToProto() override; diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index 184e476..49ead22 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/cartographer_grpc/proto/map_builder_service.proto @@ -124,6 +124,15 @@ message GetTrajectoryNodePosesResponse { repeated TrajectoryNodePose node_poses = 1; } +message LandmarkPose { + string landmark_id = 1; + cartographer.transform.proto.Rigid3d global_pose = 2; +} + +message GetLandmarkPosesResponse { + repeated LandmarkPose landmark_poses = 1; +} + message SubmapPose { cartographer.mapping.proto.SubmapId submap_id = 1; int32 submap_version = 2; @@ -202,6 +211,10 @@ service MapBuilderService { rpc GetTrajectoryNodePoses(google.protobuf.Empty) returns (GetTrajectoryNodePosesResponse); + // Returns the current optimized landmark poses. + rpc GetLandmarkPoses(google.protobuf.Empty) + returns (GetLandmarkPosesResponse); + // Returns the current optimized submap poses. rpc GetAllSubmapPoses(google.protobuf.Empty) returns (GetAllSubmapPosesResponse); diff --git a/cartographer_grpc/testing/handler_test.h b/cartographer_grpc/testing/handler_test.h index c0fb773..acef1be 100644 --- a/cartographer_grpc/testing/handler_test.h +++ b/cartographer_grpc/testing/handler_test.h @@ -20,7 +20,9 @@ #include "cartographer/common/make_unique.h" #include "cartographer_grpc/framework/testing/rpc_handler_test_server.h" #include "cartographer_grpc/testing/mock_local_trajectory_uploader.h" +#include "cartographer_grpc/testing/mock_map_builder.h" #include "cartographer_grpc/testing/mock_map_builder_context.h" +#include "cartographer_grpc/testing/mock_pose_graph.h" #include "gtest/gtest.h" namespace cartographer_grpc { @@ -41,6 +43,15 @@ class HandlerTest : public Test { ->template GetUnsynchronizedContext(); mock_local_trajectory_uploader_ = cartographer::common::make_unique(); + mock_map_builder_ = cartographer::common::make_unique(); + mock_pose_graph_ = cartographer::common::make_unique(); + + EXPECT_CALL(*mock_map_builder_context_, map_builder()) + .Times(::testing::AnyNumber()) + .WillRepeatedly(::testing::ReturnPointee(mock_map_builder_.get())); + EXPECT_CALL(*mock_map_builder_, pose_graph()) + .Times(::testing::AnyNumber()) + .WillRepeatedly(Return(mock_pose_graph_.get())); } void SetNoLocalTrajectoryUploader() { @@ -56,8 +67,10 @@ class HandlerTest : public Test { protected: std::unique_ptr> test_server_; - MockMapBuilderContext *mock_map_builder_context_; + MockMapBuilderContext* mock_map_builder_context_; std::unique_ptr mock_local_trajectory_uploader_; + std::unique_ptr mock_map_builder_; + std::unique_ptr mock_pose_graph_; }; } // namespace testing diff --git a/cartographer_grpc/testing/mock_pose_graph.h b/cartographer_grpc/testing/mock_pose_graph.h new file mode 100644 index 0000000..30291d7 --- /dev/null +++ b/cartographer_grpc/testing/mock_pose_graph.h @@ -0,0 +1,60 @@ +/* + * 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_GRPC_TESTING_MOCK_POSE_GRAPH_H +#define CARTOGRAPHER_GRPC_TESTING_MOCK_POSE_GRAPH_H + +#include "cartographer/mapping/pose_graph_interface.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer_grpc { +namespace testing { + +class MockPoseGraph : public cartographer::mapping::PoseGraphInterface { + public: + MockPoseGraph() = default; + ~MockPoseGraph() override = default; + + MOCK_METHOD0(RunFinalOptimization, void()); + MOCK_METHOD0(GetAllSubmapData, + cartographer::mapping::MapById()); + MOCK_METHOD0(GetAllSubmapPoses, + cartographer::mapping::MapById()); + MOCK_METHOD1(GetLocalToGlobalTransform, + cartographer::transform::Rigid3d(int)); + MOCK_METHOD0( + GetTrajectoryNodes, + cartographer::mapping::MapById()); + MOCK_METHOD0(GetTrajectoryNodePoses, + cartographer::mapping::MapById< + cartographer::mapping::NodeId, + cartographer::mapping::TrajectoryNodePose>()); + MOCK_METHOD0(GetLandmarkPoses, + std::map()); + MOCK_METHOD1(IsTrajectoryFinished, bool(int)); + MOCK_METHOD0(constraints, std::vector()); + MOCK_METHOD0(ToProto, cartographer::mapping::proto::PoseGraph()); +}; + +} // namespace testing +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_TESTING_MOCK_POSE_GRAPH_H