Alexander Belyaev 2018-02-06 18:13:31 +01:00 committed by GitHub
parent 9e977daf1d
commit 58bc1ced68
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 302 additions and 3 deletions

View File

@ -37,7 +37,6 @@
#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {

View File

@ -22,6 +22,7 @@
#include "cartographer/common/optional.h" #include "cartographer/common/optional.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
@ -98,6 +99,9 @@ class PoseGraphInterface {
// Returns the current optimized trajectory poses. // Returns the current optimized trajectory poses.
virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() = 0; 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. // Checks if the given trajectory is finished.
virtual bool IsTrajectoryFinished(int trajectory_id) = 0; virtual bool IsTrajectoryFinished(int trajectory_id) = 0;

View File

@ -606,6 +606,17 @@ PoseGraph::GetTrajectoryNodePoses() {
return node_poses; 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() { sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data(); return optimization_problem_.imu_data();

View File

@ -116,6 +116,8 @@ class PoseGraph : public mapping::PoseGraph {
GetTrajectoryNodes() override EXCLUDES(mutex_); GetTrajectoryNodes() override EXCLUDES(mutex_);
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() override EXCLUDES(mutex_); 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::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_); EXCLUDES(mutex_);

View File

@ -635,6 +635,17 @@ PoseGraph::GetTrajectoryNodePoses() {
return node_poses; 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() { sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data(); return optimization_problem_.imu_data();

View File

@ -42,7 +42,6 @@
#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
namespace cartographer { namespace cartographer {
@ -116,6 +115,8 @@ class PoseGraph : public mapping::PoseGraph {
GetTrajectoryNodes() override EXCLUDES(mutex_); GetTrajectoryNodes() override EXCLUDES(mutex_);
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() override EXCLUDES(mutex_); 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::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_); EXCLUDES(mutex_);

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -26,6 +26,7 @@
#include "cartographer_grpc/handlers/finish_trajectory_handler.h" #include "cartographer_grpc/handlers/finish_trajectory_handler.h"
#include "cartographer_grpc/handlers/get_all_submap_poses.h" #include "cartographer_grpc/handlers/get_all_submap_poses.h"
#include "cartographer_grpc/handlers/get_constraints_handler.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_local_to_global_transform_handler.h"
#include "cartographer_grpc/handlers/get_submap_handler.h" #include "cartographer_grpc/handlers/get_submap_handler.h"
#include "cartographer_grpc/handlers/get_trajectory_node_poses_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::ReceiveLocalSlamResultsHandler>();
server_builder.RegisterHandler<handlers::GetSubmapHandler>(); server_builder.RegisterHandler<handlers::GetSubmapHandler>();
server_builder.RegisterHandler<handlers::GetTrajectoryNodePosesHandler>(); server_builder.RegisterHandler<handlers::GetTrajectoryNodePosesHandler>();
server_builder.RegisterHandler<handlers::GetLandmarkPosesHandler>();
server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>(); server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>();
server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>(); server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();
server_builder.RegisterHandler<handlers::GetConstraintsHandler>(); server_builder.RegisterHandler<handlers::GetConstraintsHandler>();

View File

@ -19,6 +19,7 @@
#include "cartographer_grpc/framework/client.h" #include "cartographer_grpc/framework/client.h"
#include "cartographer_grpc/handlers/get_all_submap_poses.h" #include "cartographer_grpc/handlers/get_all_submap_poses.h"
#include "cartographer_grpc/handlers/get_constraints_handler.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_local_to_global_transform_handler.h"
#include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h" #include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h"
#include "cartographer_grpc/handlers/run_final_optimization_handler.h" #include "cartographer_grpc/handlers/run_final_optimization_handler.h"
@ -103,6 +104,19 @@ PoseGraphStub::GetTrajectoryNodePoses() {
return node_poses; 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) { bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }

View File

@ -43,6 +43,8 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
cartographer::mapping::MapById<cartographer::mapping::NodeId, cartographer::mapping::MapById<cartographer::mapping::NodeId,
cartographer::mapping::TrajectoryNodePose> cartographer::mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() override; GetTrajectoryNodePoses() override;
std::map<std::string, cartographer::transform::Rigid3d> GetLandmarkPoses()
override;
bool IsTrajectoryFinished(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override;
std::vector<Constraint> constraints() override; std::vector<Constraint> constraints() override;
cartographer::mapping::proto::PoseGraph ToProto() override; cartographer::mapping::proto::PoseGraph ToProto() override;

View File

@ -124,6 +124,15 @@ message GetTrajectoryNodePosesResponse {
repeated TrajectoryNodePose node_poses = 1; 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 { message SubmapPose {
cartographer.mapping.proto.SubmapId submap_id = 1; cartographer.mapping.proto.SubmapId submap_id = 1;
int32 submap_version = 2; int32 submap_version = 2;
@ -202,6 +211,10 @@ service MapBuilderService {
rpc GetTrajectoryNodePoses(google.protobuf.Empty) rpc GetTrajectoryNodePoses(google.protobuf.Empty)
returns (GetTrajectoryNodePosesResponse); returns (GetTrajectoryNodePosesResponse);
// Returns the current optimized landmark poses.
rpc GetLandmarkPoses(google.protobuf.Empty)
returns (GetLandmarkPosesResponse);
// Returns the current optimized submap poses. // Returns the current optimized submap poses.
rpc GetAllSubmapPoses(google.protobuf.Empty) rpc GetAllSubmapPoses(google.protobuf.Empty)
returns (GetAllSubmapPosesResponse); returns (GetAllSubmapPosesResponse);

View File

@ -20,7 +20,9 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer_grpc/framework/testing/rpc_handler_test_server.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_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_map_builder_context.h"
#include "cartographer_grpc/testing/mock_pose_graph.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -41,6 +43,15 @@ class HandlerTest : public Test {
->template GetUnsynchronizedContext<MockMapBuilderContext>(); ->template GetUnsynchronizedContext<MockMapBuilderContext>();
mock_local_trajectory_uploader_ = mock_local_trajectory_uploader_ =
cartographer::common::make_unique<MockLocalTrajectoryUploader>(); 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() { void SetNoLocalTrajectoryUploader() {
@ -58,6 +69,8 @@ class HandlerTest : public Test {
test_server_; test_server_;
MockMapBuilderContext* mock_map_builder_context_; MockMapBuilderContext* mock_map_builder_context_;
std::unique_ptr<MockLocalTrajectoryUploader> mock_local_trajectory_uploader_; std::unique_ptr<MockLocalTrajectoryUploader> mock_local_trajectory_uploader_;
std::unique_ptr<MockMapBuilder> mock_map_builder_;
std::unique_ptr<MockPoseGraph> mock_pose_graph_;
}; };
} // namespace testing } // namespace testing

View File

@ -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