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.master
Sebastian Klose
Wally B. Feed
parent
fbb2835525
commit
a35092c20f
|
|
@ -129,13 +129,9 @@ class ClientServerTest : public ::testing::Test {
|
|||
auto mock_map_builder = common::make_unique<MockMapBuilder>();
|
||||
mock_map_builder_ = mock_map_builder.get();
|
||||
mock_pose_graph_ = common::make_unique<MockPoseGraph>();
|
||||
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<MapBuilderServer>(
|
||||
map_builder_server_options_, std::move(mock_map_builder));
|
||||
EXPECT_TRUE(server_ != nullptr);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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_
|
||||
|
|
@ -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 <memory>
|
||||
|
||||
#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 <typename T>
|
||||
T ProtoFromStringOrDie(const std::string& proto_string) {
|
||||
T msg;
|
||||
CHECK(google::protobuf::TextFormat::ParseFromString(proto_string, &msg));
|
||||
return msg;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
std::unique_ptr<T> ProtoUPtrFromStringOrDie(const std::string& proto_string) {
|
||||
return make_unique<T>(ProtoFromStringOrDie<T>(proto_string));
|
||||
}
|
||||
|
||||
std::unique_ptr<InMemoryProtoStreamReader>
|
||||
CreateInMemoryReaderFromTextProtos() {
|
||||
std::queue<std::unique_ptr<Message>> proto_queue;
|
||||
|
||||
proto_queue.emplace(ProtoUPtrFromStringOrDie<SerializationHeader>(
|
||||
kSerializationHeaderProtoString));
|
||||
proto_queue.emplace(
|
||||
ProtoUPtrFromStringOrDie<SerializedData>(kPoseGraphProtoString));
|
||||
proto_queue.emplace(ProtoUPtrFromStringOrDie<SerializedData>(
|
||||
kAllTrajectoryBuilderOptionsProtoString));
|
||||
proto_queue.emplace(
|
||||
ProtoUPtrFromStringOrDie<SerializedData>(kSubmapProtoString));
|
||||
proto_queue.emplace(
|
||||
ProtoUPtrFromStringOrDie<SerializedData>(kNodeProtoString));
|
||||
proto_queue.emplace(
|
||||
ProtoUPtrFromStringOrDie<SerializedData>(kTrajectoryDataProtoString));
|
||||
proto_queue.emplace(
|
||||
ProtoUPtrFromStringOrDie<SerializedData>(kImuDataProtoString));
|
||||
proto_queue.emplace(
|
||||
ProtoUPtrFromStringOrDie<SerializedData>(kOdometryDataProtoString));
|
||||
proto_queue.emplace(
|
||||
ProtoUPtrFromStringOrDie<SerializedData>(kFixedFramePoseDataProtoString));
|
||||
proto_queue.emplace(
|
||||
ProtoUPtrFromStringOrDie<SerializedData>(kLandmarkDataProtoString));
|
||||
return make_unique<InMemoryProtoStreamReader>(std::move(proto_queue));
|
||||
}
|
||||
|
||||
// This test checks if the serialization works.
|
||||
TEST(ProtoStreamDeserializerTest, WorksOnGoldenTextStream) {
|
||||
// Load text proto into in_memory_reader.
|
||||
std::unique_ptr<InMemoryProtoStreamReader> reader =
|
||||
CreateInMemoryReaderFromTextProtos();
|
||||
|
||||
io::ProtoStreamDeserializer deserializer(reader.get());
|
||||
|
||||
EXPECT_TRUE(MessageDifferencer::Equals(
|
||||
deserializer.header(), ProtoFromStringOrDie<SerializationHeader>(
|
||||
kSerializationHeaderProtoString)));
|
||||
|
||||
EXPECT_TRUE(MessageDifferencer::Equals(
|
||||
deserializer.pose_graph(),
|
||||
ProtoFromStringOrDie<SerializedData>(kPoseGraphProtoString)
|
||||
.pose_graph()));
|
||||
|
||||
EXPECT_TRUE(
|
||||
MessageDifferencer::Equals(deserializer.all_trajectory_builder_options(),
|
||||
ProtoFromStringOrDie<SerializedData>(
|
||||
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<SerializedData>(kSubmapProtoString)));
|
||||
|
||||
EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
|
||||
EXPECT_TRUE(MessageDifferencer::Equals(
|
||||
serialized_data, ProtoFromStringOrDie<SerializedData>(kNodeProtoString)));
|
||||
|
||||
EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
|
||||
EXPECT_TRUE(MessageDifferencer::Equals(
|
||||
serialized_data,
|
||||
ProtoFromStringOrDie<SerializedData>(kTrajectoryDataProtoString)));
|
||||
|
||||
EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
|
||||
EXPECT_TRUE(MessageDifferencer::Equals(
|
||||
serialized_data,
|
||||
ProtoFromStringOrDie<SerializedData>(kImuDataProtoString)));
|
||||
|
||||
EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
|
||||
EXPECT_TRUE(MessageDifferencer::Equals(
|
||||
serialized_data,
|
||||
ProtoFromStringOrDie<SerializedData>(kOdometryDataProtoString)));
|
||||
|
||||
EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
|
||||
EXPECT_TRUE(MessageDifferencer::Equals(
|
||||
serialized_data,
|
||||
ProtoFromStringOrDie<SerializedData>(kFixedFramePoseDataProtoString)));
|
||||
|
||||
EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
|
||||
EXPECT_TRUE(MessageDifferencer::Equals(
|
||||
serialized_data,
|
||||
ProtoFromStringOrDie<SerializedData>(kLandmarkDataProtoString)));
|
||||
|
||||
EXPECT_FALSE(deserializer.ReadNextSerializedData(&serialized_data));
|
||||
EXPECT_TRUE(reader->eof());
|
||||
}
|
||||
|
||||
TEST(ProtoStreamDeserializerDeathTests, FailsIfVersionNotSupported) {
|
||||
std::queue<std::unique_ptr<Message>> proto_queue;
|
||||
proto_queue.emplace(ProtoUPtrFromStringOrDie<SerializationHeader>(
|
||||
kUnsupportedSerializationHeaderProtoString));
|
||||
InMemoryProtoStreamReader reader(std::move(proto_queue));
|
||||
|
||||
EXPECT_DEATH(common::make_unique<ProtoStreamDeserializer>(&reader),
|
||||
"Unsupported serialization format");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace io
|
||||
} // namespace cartographer
|
||||
Loading…
Reference in New Issue