diff --git a/cartographer/cloud/client/map_builder_stub.cc b/cartographer/cloud/client/map_builder_stub.cc index dabe02e..db41de4 100644 --- a/cartographer/cloud/client/map_builder_stub.cc +++ b/cartographer/cloud/client/map_builder_stub.cc @@ -24,6 +24,7 @@ #include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h" #include "cartographer/cloud/internal/handlers/load_state_handler.h" #include "cartographer/cloud/internal/handlers/write_state_handler.h" +#include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h" #include "cartographer/cloud/internal/mapping/serialization.h" #include "cartographer/cloud/internal/sensor/serialization.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" @@ -148,6 +149,22 @@ void MapBuilderStub::SerializeState(bool include_unfinished_submaps, } } +void MapBuilderStub::SerializeStateToFile(bool include_unfinished_submaps, + const std::string& filename) { + if (include_unfinished_submaps) { + LOG(WARNING) << "Serializing unfinished submaps is currently unsupported. " + "Proceeding to write the state without them."; + } + proto::WriteStateToFileRequest request; + request.set_filename(filename); + ::grpc::Status status; + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request, &status)) + << "code: " << status.error_code() + << " reason: " << status.error_message(); +} + std::map MapBuilderStub::LoadState( io::ProtoStreamReaderInterface* reader, const bool load_frozen_state) { async_grpc::Client client(client_channel_); diff --git a/cartographer/cloud/client/map_builder_stub.h b/cartographer/cloud/client/map_builder_stub.h index 733ee56..d2de33c 100644 --- a/cartographer/cloud/client/map_builder_stub.h +++ b/cartographer/cloud/client/map_builder_stub.h @@ -50,6 +50,8 @@ class MapBuilderStub : public mapping::MapBuilderInterface { mapping::proto::SubmapQuery::Response* response) override; void SerializeState(bool include_unfinished_submaps, io::ProtoStreamWriterInterface* writer) override; + void SerializeStateToFile(bool include_unfinished_submaps, + const std::string& filename) override; std::map LoadState(io::ProtoStreamReaderInterface* reader, bool load_frozen_state) override; std::map LoadStateFromFile(const std::string& filename, diff --git a/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc b/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc new file mode 100644 index 0000000..adbbabc --- /dev/null +++ b/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc @@ -0,0 +1,41 @@ +/* + * 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/cloud/internal/handlers/write_state_to_file_handler.h" + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/map_builder_server.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/io/proto_stream.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void WriteStateToFileHandler::OnRequest( + const proto::WriteStateToFileRequest& request) { + if (request.filename().empty()) { + Finish(::grpc::Status(::grpc::INVALID_ARGUMENT, "Filename empty.")); + return; + } + GetContext()->map_builder().SerializeStateToFile( + /*include_unfinished_submaps=*/false, request.filename()); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/cartographer/cloud/internal/handlers/write_state_to_file_handler.h b/cartographer/cloud/internal/handlers/write_state_to_file_handler.h new file mode 100644 index 0000000..7dcbcf3 --- /dev/null +++ b/cartographer/cloud/internal/handlers/write_state_to_file_handler.h @@ -0,0 +1,43 @@ +/* + * 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_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + WriteStateToFileSignature, proto::WriteStateToFileRequest, + google::protobuf::Empty, + "/cartographer.cloud.proto.MapBuilderService/WriteStateToFile") + +class WriteStateToFileHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::WriteStateToFileRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc index dcbbc38..d6d9258 100644 --- a/cartographer/cloud/internal/map_builder_server.cc +++ b/cartographer/cloud/internal/map_builder_server.cc @@ -41,6 +41,7 @@ #include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h" #include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h" #include "cartographer/cloud/internal/handlers/write_state_handler.h" +#include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h" #include "cartographer/cloud/internal/sensor/serialization.h" #include "glog/logging.h" @@ -95,6 +96,7 @@ MapBuilderServer::MapBuilderServer( server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); + server_builder.RegisterHandler(); server_builder.RegisterHandler(); grpc_server_ = server_builder.Build(); if (map_builder_server_options.map_builder_options() diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index ef0bb96..f509b29 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/cartographer/cloud/proto/map_builder_service.proto @@ -228,6 +228,10 @@ message WriteStateResponse { } } +message WriteStateToFileRequest { + string filename = 1; +} + message IsTrajectoryFinishedRequest { int32 trajectory_id = 1; } @@ -339,4 +343,7 @@ service MapBuilderService { // Receives serialized SLAM state data in the order defined by // ProtoStreamWriter. rpc WriteState(google.protobuf.Empty) returns (stream WriteStateResponse); + + // Writes the serialized SLAM state to the host file system. + rpc WriteStateToFile(WriteStateToFileRequest) returns (google.protobuf.Empty); } diff --git a/cartographer/mapping/internal/testing/mock_map_builder.h b/cartographer/mapping/internal/testing/mock_map_builder.h index 1fcdd85..f459137 100644 --- a/cartographer/mapping/internal/testing/mock_map_builder.h +++ b/cartographer/mapping/internal/testing/mock_map_builder.h @@ -48,6 +48,7 @@ class MockMapBuilder : public mapping::MapBuilderInterface { std::string(const mapping::SubmapId &, mapping::proto::SubmapQuery::Response *)); MOCK_METHOD2(SerializeState, void(bool, io::ProtoStreamWriterInterface *)); + MOCK_METHOD2(SerializeStateToFile, void(bool, const std::string &)); MOCK_METHOD2(LoadState, std::map(io::ProtoStreamReaderInterface *, bool)); MOCK_METHOD2(LoadStateFromFile, diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 58e1d79..d5799ec 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -212,6 +212,14 @@ void MapBuilder::SerializeState(bool include_unfinished_submaps, include_unfinished_submaps); } +void MapBuilder::SerializeStateToFile(bool include_unfinished_submaps, + const std::string& filename) { + io::ProtoStreamWriter writer(filename); + io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, &writer, + include_unfinished_submaps); + writer.Close(); +} + std::map MapBuilder::LoadState( io::ProtoStreamReaderInterface* const reader, bool load_frozen_state) { io::ProtoStreamDeserializer deserializer(reader); diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 0249c00..23c609f 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -59,6 +59,9 @@ class MapBuilder : public MapBuilderInterface { void SerializeState(bool include_unfinished_submaps, io::ProtoStreamWriterInterface *writer) override; + void SerializeStateToFile(bool include_unfinished_submaps, + const std::string &filename) override; + std::map LoadState(io::ProtoStreamReaderInterface *reader, bool load_frozen_state) override; diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index f1fceb6..d9ca97b 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -84,6 +84,13 @@ class MapBuilderInterface { virtual void SerializeState(bool include_unfinished_submaps, io::ProtoStreamWriterInterface* writer) = 0; + // Serializes the current state to a proto stream file on the host system. If + // 'include_unfinished_submaps' is set to true, unfinished submaps, i.e. + // submaps that have not yet received all rangefinder data insertions, will + // be included in the serialized state. + virtual void SerializeStateToFile(bool include_unfinished_submaps, + const std::string& filename) = 0; + // Loads the SLAM state from a proto stream. Returns the remapping of new // trajectory_ids. virtual std::map