Implement GetLandmarkPoses method. (#888)
[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)master
parent
9e977daf1d
commit
58bc1ced68
|
@ -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 {
|
||||
|
|
|
@ -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<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() = 0;
|
||||
|
||||
// Returns the current optimized landmark poses.
|
||||
virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses() = 0;
|
||||
|
||||
// Checks if the given trajectory is finished.
|
||||
virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
|
||||
|
||||
|
|
|
@ -606,6 +606,17 @@ PoseGraph::GetTrajectoryNodePoses() {
|
|||
return node_poses;
|
||||
}
|
||||
|
||||
std::map<std::string, transform::Rigid3d> PoseGraph::GetLandmarkPoses() {
|
||||
std::map<std::string, transform::Rigid3d> 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<sensor::ImuData> PoseGraph::GetImuData() {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return optimization_problem_.imu_data();
|
||||
|
|
|
@ -116,6 +116,8 @@ class PoseGraph : public mapping::PoseGraph {
|
|||
GetTrajectoryNodes() override EXCLUDES(mutex_);
|
||||
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
|
||||
GetTrajectoryNodePoses() override EXCLUDES(mutex_);
|
||||
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
|
||||
EXCLUDES(mutex_);
|
||||
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
|
||||
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
|
||||
EXCLUDES(mutex_);
|
||||
|
|
|
@ -635,6 +635,17 @@ PoseGraph::GetTrajectoryNodePoses() {
|
|||
return node_poses;
|
||||
}
|
||||
|
||||
std::map<std::string, transform::Rigid3d> PoseGraph::GetLandmarkPoses() {
|
||||
std::map<std::string, transform::Rigid3d> 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<sensor::ImuData> PoseGraph::GetImuData() {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return optimization_problem_.imu_data();
|
||||
|
|
|
@ -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<mapping::NodeId, mapping::TrajectoryNodePose>
|
||||
GetTrajectoryNodePoses() override EXCLUDES(mutex_);
|
||||
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
|
||||
EXCLUDES(mutex_);
|
||||
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
|
||||
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
|
||||
EXCLUDES(mutex_);
|
||||
|
|
|
@ -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<MapBuilderContextInterface>()
|
||||
->map_builder()
|
||||
.pose_graph()
|
||||
->GetLandmarkPoses();
|
||||
auto response =
|
||||
cartographer::common::make_unique<proto::GetLandmarkPosesResponse>();
|
||||
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
|
|
@ -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<google::protobuf::Empty,
|
||||
proto::GetLandmarkPosesResponse> {
|
||||
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
|
|
@ -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<GetLandmarkPosesHandler>;
|
||||
|
||||
TEST_F(GetLandmarkPosesHandlerTest, NoLocalSlamUploader) {
|
||||
std::map<std::string, Rigid3d> 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
|
|
@ -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<handlers::ReceiveLocalSlamResultsHandler>();
|
||||
server_builder.RegisterHandler<handlers::GetSubmapHandler>();
|
||||
server_builder.RegisterHandler<handlers::GetTrajectoryNodePosesHandler>();
|
||||
server_builder.RegisterHandler<handlers::GetLandmarkPosesHandler>();
|
||||
server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>();
|
||||
server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();
|
||||
server_builder.RegisterHandler<handlers::GetConstraintsHandler>();
|
||||
|
|
|
@ -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<std::string, cartographer::transform::Rigid3d>
|
||||
PoseGraphStub::GetLandmarkPoses() {
|
||||
google::protobuf::Empty request;
|
||||
framework::Client<handlers::GetLandmarkPosesHandler> client(client_channel_);
|
||||
CHECK(client.Write(request));
|
||||
std::map<std::string, cartographer::transform::Rigid3d> 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";
|
||||
}
|
||||
|
|
|
@ -43,6 +43,8 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
|
|||
cartographer::mapping::MapById<cartographer::mapping::NodeId,
|
||||
cartographer::mapping::TrajectoryNodePose>
|
||||
GetTrajectoryNodePoses() override;
|
||||
std::map<std::string, cartographer::transform::Rigid3d> GetLandmarkPoses()
|
||||
override;
|
||||
bool IsTrajectoryFinished(int trajectory_id) override;
|
||||
std::vector<Constraint> constraints() override;
|
||||
cartographer::mapping::proto::PoseGraph ToProto() override;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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<MockMapBuilderContext>();
|
||||
mock_local_trajectory_uploader_ =
|
||||
cartographer::common::make_unique<MockLocalTrajectoryUploader>();
|
||||
mock_map_builder_ = cartographer::common::make_unique<MockMapBuilder>();
|
||||
mock_pose_graph_ = cartographer::common::make_unique<MockPoseGraph>();
|
||||
|
||||
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() {
|
||||
|
@ -58,6 +69,8 @@ class HandlerTest : public Test {
|
|||
test_server_;
|
||||
MockMapBuilderContext* mock_map_builder_context_;
|
||||
std::unique_ptr<MockLocalTrajectoryUploader> mock_local_trajectory_uploader_;
|
||||
std::unique_ptr<MockMapBuilder> mock_map_builder_;
|
||||
std::unique_ptr<MockPoseGraph> mock_pose_graph_;
|
||||
};
|
||||
|
||||
} // namespace testing
|
||||
|
|
|
@ -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<cartographer::mapping::SubmapId,
|
||||
SubmapData>());
|
||||
MOCK_METHOD0(GetAllSubmapPoses,
|
||||
cartographer::mapping::MapById<cartographer::mapping::SubmapId,
|
||||
SubmapPose>());
|
||||
MOCK_METHOD1(GetLocalToGlobalTransform,
|
||||
cartographer::transform::Rigid3d(int));
|
||||
MOCK_METHOD0(
|
||||
GetTrajectoryNodes,
|
||||
cartographer::mapping::MapById<cartographer::mapping::NodeId,
|
||||
cartographer::mapping::TrajectoryNode>());
|
||||
MOCK_METHOD0(GetTrajectoryNodePoses,
|
||||
cartographer::mapping::MapById<
|
||||
cartographer::mapping::NodeId,
|
||||
cartographer::mapping::TrajectoryNodePose>());
|
||||
MOCK_METHOD0(GetLandmarkPoses,
|
||||
std::map<std::string, cartographer::transform::Rigid3d>());
|
||||
MOCK_METHOD1(IsTrajectoryFinished, bool(int));
|
||||
MOCK_METHOD0(constraints, std::vector<Constraint>());
|
||||
MOCK_METHOD0(ToProto, cartographer::mapping::proto::PoseGraph());
|
||||
};
|
||||
|
||||
} // namespace testing
|
||||
} // namespace cartographer_grpc
|
||||
|
||||
#endif // CARTOGRAPHER_GRPC_TESTING_MOCK_POSE_GRAPH_H
|
Loading…
Reference in New Issue