diff --git a/cartographer/cloud/client/map_builder_stub.cc b/cartographer/cloud/client/map_builder_stub.cc index a52433b..6ff4550 100644 --- a/cartographer/cloud/client/map_builder_stub.cc +++ b/cartographer/cloud/client/map_builder_stub.cc @@ -153,7 +153,7 @@ void MapBuilderStub::SerializeState(bool include_unfinished_submaps, } } -void MapBuilderStub::SerializeStateToFile(bool include_unfinished_submaps, +bool MapBuilderStub::SerializeStateToFile(bool include_unfinished_submaps, const std::string& filename) { if (include_unfinished_submaps) { LOG(WARNING) << "Serializing unfinished submaps is currently unsupported. " @@ -164,9 +164,12 @@ void MapBuilderStub::SerializeStateToFile(bool include_unfinished_submaps, ::grpc::Status status; async_grpc::Client client( client_channel_); - CHECK(client.Write(request, &status)) - << "code: " << status.error_code() - << " reason: " << status.error_message(); + if (!client.Write(request, &status)) { + LOG(ERROR) << "WriteStateToFileRequest failed - " + << "code: " << status.error_code() + << " reason: " << status.error_message(); + } + return client.response().success(); } std::map MapBuilderStub::LoadState( diff --git a/cartographer/cloud/client/map_builder_stub.h b/cartographer/cloud/client/map_builder_stub.h index d2de33c..5ccfb3a 100644 --- a/cartographer/cloud/client/map_builder_stub.h +++ b/cartographer/cloud/client/map_builder_stub.h @@ -50,7 +50,7 @@ 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, + bool 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/cloud/internal/handlers/write_state_to_file_handler.cc b/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc index adbbabc..178cdd1 100644 --- a/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc +++ b/cartographer/cloud/internal/handlers/write_state_to_file_handler.cc @@ -16,6 +16,7 @@ #include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h" +#include "absl/memory/memory.h" #include "async_grpc/rpc_handler.h" #include "cartographer/cloud/internal/map_builder_context_interface.h" #include "cartographer/cloud/internal/map_builder_server.h" @@ -32,8 +33,14 @@ void WriteStateToFileHandler::OnRequest( Finish(::grpc::Status(::grpc::INVALID_ARGUMENT, "Filename empty.")); return; } - GetContext()->map_builder().SerializeStateToFile( - /*include_unfinished_submaps=*/false, request.filename()); + bool success = + GetContext() + ->map_builder() + .SerializeStateToFile( + /*include_unfinished_submaps=*/false, request.filename()); + auto response = absl::make_unique(); + response->set_success(success); + Send(std::move(response)); } } // namespace handlers diff --git a/cartographer/cloud/internal/handlers/write_state_to_file_handler.h b/cartographer/cloud/internal/handlers/write_state_to_file_handler.h index 7dcbcf3..06a1e16 100644 --- a/cartographer/cloud/internal/handlers/write_state_to_file_handler.h +++ b/cartographer/cloud/internal/handlers/write_state_to_file_handler.h @@ -27,7 +27,7 @@ namespace handlers { DEFINE_HANDLER_SIGNATURE( WriteStateToFileSignature, proto::WriteStateToFileRequest, - google::protobuf::Empty, + proto::WriteStateToFileResponse, "/cartographer.cloud.proto.MapBuilderService/WriteStateToFile") class WriteStateToFileHandler diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index f509b29..4938e3f 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/cartographer/cloud/proto/map_builder_service.proto @@ -232,6 +232,10 @@ message WriteStateToFileRequest { string filename = 1; } +message WriteStateToFileResponse { + bool success = 1; +} + message IsTrajectoryFinishedRequest { int32 trajectory_id = 1; } @@ -345,5 +349,6 @@ service MapBuilderService { 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); + rpc WriteStateToFile(WriteStateToFileRequest) + returns (WriteStateToFileResponse); } diff --git a/cartographer/mapping/internal/testing/mock_map_builder.h b/cartographer/mapping/internal/testing/mock_map_builder.h index f459137..a61319a 100644 --- a/cartographer/mapping/internal/testing/mock_map_builder.h +++ b/cartographer/mapping/internal/testing/mock_map_builder.h @@ -48,7 +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(SerializeStateToFile, bool(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 d5799ec..4c77c8e 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -212,12 +212,12 @@ void MapBuilder::SerializeState(bool include_unfinished_submaps, include_unfinished_submaps); } -void MapBuilder::SerializeStateToFile(bool include_unfinished_submaps, +bool 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(); + return (writer.Close()); } std::map MapBuilder::LoadState( diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 23c609f..72a8e26 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -59,7 +59,7 @@ class MapBuilder : public MapBuilderInterface { void SerializeState(bool include_unfinished_submaps, io::ProtoStreamWriterInterface *writer) override; - void SerializeStateToFile(bool include_unfinished_submaps, + bool SerializeStateToFile(bool include_unfinished_submaps, const std::string &filename) override; std::map LoadState(io::ProtoStreamReaderInterface *reader, diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index d9ca97b..8c1cfb1 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -88,7 +88,8 @@ class MapBuilderInterface { // '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, + // Returns true if the file was successfully written. + virtual bool SerializeStateToFile(bool include_unfinished_submaps, const std::string& filename) = 0; // Loads the SLAM state from a proto stream. Returns the remapping of new