From a35092c20f519f8bc7244fd560ce684a117af33e Mon Sep 17 00:00:00 2001 From: Sebastian Klose Date: Tue, 29 May 2018 22:10:11 +0200 Subject: [PATCH] Mapping state deserializer (#1171) class for de-serializing mapping state stored in the new serialization format [RFC 0021](https://github.com/sebastianklose/rfcs/blob/serialization_format/text/0021-serialization-format.md). Not used yet. Will be hooked up in a separate PR that switches all logic to the new serialization format at once. --- .../cloud/internal/client_server_test.cc | 8 +- cartographer/io/proto_stream_deserializer.cc | 70 +++++++ cartographer/io/proto_stream_deserializer.h | 68 ++++++ .../io/proto_stream_deserializer_test.cc | 197 ++++++++++++++++++ 4 files changed, 337 insertions(+), 6 deletions(-) create mode 100644 cartographer/io/proto_stream_deserializer.cc create mode 100644 cartographer/io/proto_stream_deserializer.h create mode 100644 cartographer/io/proto_stream_deserializer_test.cc diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 073edb4..11bb890 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -129,13 +129,9 @@ class ClientServerTest : public ::testing::Test { auto mock_map_builder = common::make_unique(); mock_map_builder_ = mock_map_builder.get(); mock_pose_graph_ = common::make_unique(); - EXPECT_CALL( - *mock_map_builder_, - pose_graph()) + EXPECT_CALL(*mock_map_builder_, pose_graph()) .WillOnce(::testing::Return(mock_pose_graph_.get())); - EXPECT_CALL( - *mock_pose_graph_, - SetGlobalSlamOptimizationCallback(_)); + EXPECT_CALL(*mock_pose_graph_, SetGlobalSlamOptimizationCallback(_)); server_ = common::make_unique( map_builder_server_options_, std::move(mock_map_builder)); EXPECT_TRUE(server_ != nullptr); diff --git a/cartographer/io/proto_stream_deserializer.cc b/cartographer/io/proto_stream_deserializer.cc new file mode 100644 index 0000000..6b5eee6 --- /dev/null +++ b/cartographer/io/proto_stream_deserializer.cc @@ -0,0 +1,70 @@ +/* + * 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/io/proto_stream_deserializer.h" + +#include "cartographer/io/internal/mapping_state_serialization.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { +namespace { +mapping::proto::SerializationHeader ReadHeaderOrDie( + ProtoStreamReaderInterface* const reader) { + mapping::proto::SerializationHeader header; + CHECK(reader->ReadProto(&header)) << "Failed to read SerializationHeader."; + return header; +} + +bool IsVersionSupported(const mapping::proto::SerializationHeader& header) { + return header.format_version() == kMappingStateSerializationFormatVersion; +} + +} // namespace + +ProtoStreamDeserializer::ProtoStreamDeserializer( + ProtoStreamReaderInterface* const reader) + : reader_(reader), header_(ReadHeaderOrDie(reader)) { + CHECK(IsVersionSupported(header_)) << "Unsupported serialization format \"" + << header_.format_version() << "\""; + + CHECK(ReadNextSerializedData(&pose_graph_)) + << "Serialized stream misses PoseGraph."; + CHECK(pose_graph_.has_pose_graph()) + << "Serialized stream order corrupt. Expecting `PoseGraph` after " + "`SerializationHeader`, but got field tag " + << pose_graph_.data_case(); + + CHECK(ReadNextSerializedData(&all_trajectory_builder_options_)) + << "Serialized stream misses `AllTrajectoryBuilderOptions`."; + CHECK(all_trajectory_builder_options_.has_all_trajectory_builder_options()) + << "Serialized stream order corrupt. Expecting " + "`AllTrajectoryBuilderOptions` after " + "PoseGraph, got field tag " + << all_trajectory_builder_options_.data_case(); + + CHECK_EQ(pose_graph_.pose_graph().trajectory_size(), + all_trajectory_builder_options_.all_trajectory_builder_options() + .options_with_sensor_ids_size()); +} + +bool ProtoStreamDeserializer::ReadNextSerializedData( + mapping::proto::SerializedData* data) { + return reader_->ReadProto(data); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/proto_stream_deserializer.h b/cartographer/io/proto_stream_deserializer.h new file mode 100644 index 0000000..08424a0 --- /dev/null +++ b/cartographer/io/proto_stream_deserializer.h @@ -0,0 +1,68 @@ +/* + * 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_IO_PROTO_STREAM_DESERIALIZER_H_ +#define CARTOGRAPHER_IO_PROTO_STREAM_DESERIALIZER_H_ + +#include "cartographer/io/proto_stream_interface.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/serialization.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" + +namespace cartographer { +namespace io { + +// Helper for deserializing a previously serialized mapping state from a +// proto stream, abstracting away the format parsing logic. +class ProtoStreamDeserializer { + public: + explicit ProtoStreamDeserializer(ProtoStreamReaderInterface* const reader); + + ProtoStreamDeserializer(const ProtoStreamDeserializer&) = delete; + ProtoStreamDeserializer& operator=(const ProtoStreamDeserializer&) = delete; + ProtoStreamDeserializer(ProtoStreamDeserializer&&) = delete; + + mapping::proto::SerializationHeader& header() { return header_; } + + mapping::proto::PoseGraph& pose_graph() { + return *pose_graph_.mutable_pose_graph(); + } + const mapping::proto::PoseGraph& pose_graph() const { + return pose_graph_.pose_graph(); + } + + const mapping::proto::AllTrajectoryBuilderOptions& + all_trajectory_builder_options() { + return all_trajectory_builder_options_.all_trajectory_builder_options(); + } + + // Reads the next `SerializedData` message of the ProtoStream into `data`. + // Returns `true` if the message was successfully read or `false` in case + // there are no-more messages or an error occurred. + bool ReadNextSerializedData(mapping::proto::SerializedData* data); + + private: + ProtoStreamReaderInterface* reader_; + + mapping::proto::SerializationHeader header_; + mapping::proto::SerializedData pose_graph_; + mapping::proto::SerializedData all_trajectory_builder_options_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_PROTO_STREAM_DESERIALIZER_H_ diff --git a/cartographer/io/proto_stream_deserializer_test.cc b/cartographer/io/proto_stream_deserializer_test.cc new file mode 100644 index 0000000..d51e623 --- /dev/null +++ b/cartographer/io/proto_stream_deserializer_test.cc @@ -0,0 +1,197 @@ +/* + * 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 + +#include "cartographer/io/internal/in_memory_proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "google/protobuf/text_format.h" +#include "google/protobuf/util/message_differencer.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +using ::cartographer::common::make_unique; +using ::cartographer::mapping::proto::SerializationHeader; +using ::cartographer::mapping::proto::SerializedData; +using ::google::protobuf::Message; +using ::google::protobuf::util::MessageDifferencer; +using ::testing::Eq; +using ::testing::Not; + +static constexpr char kSerializationHeaderProtoString[] = R"PROTO( + format_version: 1 +)PROTO"; + +static constexpr char kUnsupportedSerializationHeaderProtoString[] = R"PROTO( + format_version: 123 +)PROTO"; + +static constexpr char kPoseGraphProtoString[] = R"PROTO( + pose_graph {} +)PROTO"; + +static constexpr char kAllTrajectoryBuilderOptionsProtoString[] = R"PROTO( + all_trajectory_builder_options {} +)PROTO"; + +static constexpr char kSubmapProtoString[] = R"PROTO( + submap {} +)PROTO"; + +static constexpr char kNodeProtoString[] = R"PROTO( + node {} +)PROTO"; + +static constexpr char kTrajectoryDataProtoString[] = R"PROTO( + trajectory_data {} +)PROTO"; + +static constexpr char kImuDataProtoString[] = R"PROTO( + imu_data {} +)PROTO"; + +static constexpr char kOdometryDataProtoString[] = R"PROTO( + odometry_data {} +)PROTO"; + +static constexpr char kFixedFramePoseDataProtoString[] = R"PROTO( + fixed_frame_pose_data {} +)PROTO"; + +static constexpr char kLandmarkDataProtoString[] = R"PROTO( + landmark_data {} +)PROTO"; + +template +T ProtoFromStringOrDie(const std::string& proto_string) { + T msg; + CHECK(google::protobuf::TextFormat::ParseFromString(proto_string, &msg)); + return msg; +} + +template +std::unique_ptr ProtoUPtrFromStringOrDie(const std::string& proto_string) { + return make_unique(ProtoFromStringOrDie(proto_string)); +} + +std::unique_ptr +CreateInMemoryReaderFromTextProtos() { + std::queue> proto_queue; + + proto_queue.emplace(ProtoUPtrFromStringOrDie( + kSerializationHeaderProtoString)); + proto_queue.emplace( + ProtoUPtrFromStringOrDie(kPoseGraphProtoString)); + proto_queue.emplace(ProtoUPtrFromStringOrDie( + kAllTrajectoryBuilderOptionsProtoString)); + proto_queue.emplace( + ProtoUPtrFromStringOrDie(kSubmapProtoString)); + proto_queue.emplace( + ProtoUPtrFromStringOrDie(kNodeProtoString)); + proto_queue.emplace( + ProtoUPtrFromStringOrDie(kTrajectoryDataProtoString)); + proto_queue.emplace( + ProtoUPtrFromStringOrDie(kImuDataProtoString)); + proto_queue.emplace( + ProtoUPtrFromStringOrDie(kOdometryDataProtoString)); + proto_queue.emplace( + ProtoUPtrFromStringOrDie(kFixedFramePoseDataProtoString)); + proto_queue.emplace( + ProtoUPtrFromStringOrDie(kLandmarkDataProtoString)); + return make_unique(std::move(proto_queue)); +} + +// This test checks if the serialization works. +TEST(ProtoStreamDeserializerTest, WorksOnGoldenTextStream) { + // Load text proto into in_memory_reader. + std::unique_ptr reader = + CreateInMemoryReaderFromTextProtos(); + + io::ProtoStreamDeserializer deserializer(reader.get()); + + EXPECT_TRUE(MessageDifferencer::Equals( + deserializer.header(), ProtoFromStringOrDie( + kSerializationHeaderProtoString))); + + EXPECT_TRUE(MessageDifferencer::Equals( + deserializer.pose_graph(), + ProtoFromStringOrDie(kPoseGraphProtoString) + .pose_graph())); + + EXPECT_TRUE( + MessageDifferencer::Equals(deserializer.all_trajectory_builder_options(), + ProtoFromStringOrDie( + kAllTrajectoryBuilderOptionsProtoString) + .all_trajectory_builder_options())); + + SerializedData serialized_data; + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + // TODO(sebastianklose): Add matcher for protos in common place and use here. + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kSubmapProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, ProtoFromStringOrDie(kNodeProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kTrajectoryDataProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kImuDataProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kOdometryDataProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kFixedFramePoseDataProtoString))); + + EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(MessageDifferencer::Equals( + serialized_data, + ProtoFromStringOrDie(kLandmarkDataProtoString))); + + EXPECT_FALSE(deserializer.ReadNextSerializedData(&serialized_data)); + EXPECT_TRUE(reader->eof()); +} + +TEST(ProtoStreamDeserializerDeathTests, FailsIfVersionNotSupported) { + std::queue> proto_queue; + proto_queue.emplace(ProtoUPtrFromStringOrDie( + kUnsupportedSerializationHeaderProtoString)); + InMemoryProtoStreamReader reader(std::move(proto_queue)); + + EXPECT_DEATH(common::make_unique(&reader), + "Unsupported serialization format"); +} + +} // namespace +} // namespace io +} // namespace cartographer