diff --git a/cartographer_grpc/client_server_test.cc b/cartographer_grpc/client_server_test.cc index f4891c8..e2a5496 100644 --- a/cartographer_grpc/client_server_test.cc +++ b/cartographer_grpc/client_server_test.cc @@ -99,12 +99,9 @@ class ClientServerTest : public ::testing::Test { include "map_builder_server.lua" MAP_BUILDER.use_trajectory_builder_2d = true MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0 - MAP_BUILDER_SERVER = { - server_address = "0.0.0.0:50051", - num_event_threads = 1, - num_grpc_threads = 1, - map_builder = MAP_BUILDER, - } + MAP_BUILDER_SERVER.num_event_threads = 1 + MAP_BUILDER_SERVER.num_grpc_threads = 1 + MAP_BUILDER_SERVER.uplink_server_address = "" return MAP_BUILDER_SERVER)text"; auto map_builder_server_parameters = cartographer::mapping::test::ResolveLuaParameters(kMapBuilderServerLua); diff --git a/cartographer_grpc/data_uploader.cc b/cartographer_grpc/data_uploader.cc index c7fa8ce..02f82cc 100644 --- a/cartographer_grpc/data_uploader.cc +++ b/cartographer_grpc/data_uploader.cc @@ -16,8 +16,46 @@ #include "cartographer_grpc/data_uploader.h" +#include "glog/logging.h" + namespace cartographer_grpc { -DataUploader::DataUploader(const std::string &data_uploader) {} +DataUploader::DataUploader(const std::string& server_address) + : client_channel_(grpc::CreateChannel(server_address, + grpc::InsecureChannelCredentials())), + service_stub_(proto::MapBuilderService::NewStub(client_channel_)) {} -} // namespace cartographer_grpc +void DataUploader::AddTrajectory( + int local_trajectory_id, + const std::unordered_set& expected_sensor_ids, + const cartographer::mapping::proto::TrajectoryBuilderOptions& + trajectory_options) { + grpc::ClientContext client_context; + proto::AddTrajectoryRequest request; + proto::AddTrajectoryResponse result; + *request.mutable_trajectory_builder_options() = trajectory_options; + for (const auto& sensor_id : expected_sensor_ids) { + *request.add_expected_sensor_ids() = sensor_id; + } + grpc::Status status = + service_stub_->AddTrajectory(&client_context, request, &result); + CHECK(status.ok()); + CHECK_EQ(local_to_cloud_trajectory_id_map_.count(local_trajectory_id), 0); + local_to_cloud_trajectory_id_map_[local_trajectory_id] = + result.trajectory_id(); +} + +void DataUploader::FinishTrajectory(int local_trajectory_id) { + CHECK_EQ(local_to_cloud_trajectory_id_map_.count(local_trajectory_id), 1); + int cloud_trajectory_id = + local_to_cloud_trajectory_id_map_[local_trajectory_id]; + grpc::ClientContext client_context; + proto::FinishTrajectoryRequest request; + google::protobuf::Empty response; + request.set_trajectory_id(cloud_trajectory_id); + grpc::Status status = + service_stub_->FinishTrajectory(&client_context, request, &response); + CHECK(status.ok()); +} + +} // namespace cartographer_grpc \ No newline at end of file diff --git a/cartographer_grpc/data_uploader.h b/cartographer_grpc/data_uploader.h index 4d00da0..ca26d32 100644 --- a/cartographer_grpc/data_uploader.h +++ b/cartographer_grpc/data_uploader.h @@ -17,13 +17,30 @@ #ifndef CARTOGRAPHER_GRPC_DATA_UPLOADER_H #define CARTOGRAPHER_GRPC_DATA_UPLOADER_H +#include #include +#include + +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" +#include "grpc++/grpc++.h" namespace cartographer_grpc { class DataUploader { public: - DataUploader(const std::string &server_address); + DataUploader(const std::string& server_address); + void AddTrajectory( + int local_trajectory_id, + const std::unordered_set& expected_sensor_ids, + const cartographer::mapping::proto::TrajectoryBuilderOptions& + trajectory_options); + void FinishTrajectory(int local_trajectory_id); + + private: + std::shared_ptr client_channel_; + std::unique_ptr service_stub_; + std::map local_to_cloud_trajectory_id_map_; }; } // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/add_trajectory_handler.h b/cartographer_grpc/handlers/add_trajectory_handler.h index f90a388..e651ff8 100644 --- a/cartographer_grpc/handlers/add_trajectory_handler.h +++ b/cartographer_grpc/handlers/add_trajectory_handler.h @@ -33,14 +33,31 @@ class AddTrajectoryHandler auto local_slam_result_callback = GetUnsynchronizedContext() ->GetLocalSlamResultCallbackForSubscriptions(); + std::unordered_set expected_sensor_ids( + request.expected_sensor_ids().begin(), + request.expected_sensor_ids().end()); const int trajectory_id = GetContext() ->map_builder() - .AddTrajectoryBuilder(std::unordered_set( - request.expected_sensor_ids().begin(), - request.expected_sensor_ids().end()), + .AddTrajectoryBuilder(expected_sensor_ids, request.trajectory_builder_options(), local_slam_result_callback); + if (GetUnsynchronizedContext() + ->data_uploader()) { + auto trajectory_builder_options = request.trajectory_builder_options(); + + // Clear the trajectory builder options to convey to the cloud + // Cartographer instance that does not need to instantiate a + // 'LocalTrajectoryBuilder'. + trajectory_builder_options.clear_trajectory_builder_2d_options(); + trajectory_builder_options.clear_trajectory_builder_3d_options(); + + GetContext() + ->data_uploader() + ->AddTrajectory(trajectory_id, expected_sensor_ids, + trajectory_builder_options); + } + auto response = cartographer::common::make_unique(); response->set_trajectory_id(trajectory_id); diff --git a/cartographer_grpc/handlers/finish_trajectory_handler.h b/cartographer_grpc/handlers/finish_trajectory_handler.h index 93b080a..c97d594 100644 --- a/cartographer_grpc/handlers/finish_trajectory_handler.h +++ b/cartographer_grpc/handlers/finish_trajectory_handler.h @@ -36,6 +36,12 @@ class FinishTrajectoryHandler .FinishTrajectory(request.trajectory_id()); GetUnsynchronizedContext() ->NotifyFinishTrajectory(request.trajectory_id()); + if (GetUnsynchronizedContext() + ->data_uploader()) { + GetContext() + ->data_uploader() + ->FinishTrajectory(request.trajectory_id()); + } Send(cartographer::common::make_unique()); } }; diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index c4cdd97..8cf1951 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -207,6 +207,10 @@ MapBuilderServer::MapBuilderServer( map_builder_server_options.num_grpc_threads()); server_builder.SetNumEventThreads( map_builder_server_options.num_event_threads()); + if (!map_builder_server_options.uplink_server_address().empty()) { + data_uploader_ = cartographer::common::make_unique( + map_builder_server_options.uplink_server_address()); + } server_builder.RegisterHandler("AddTrajectory"); server_builder.RegisterHandlerdata_uploader_.get(); + } template void EnqueueSensorData(int trajectory_id, const std::string& sensor_id, @@ -150,6 +154,7 @@ class MapBuilderServer { int current_subscription_index_ = 0; std::map local_slam_subscriptions_ GUARDED_BY(local_slam_subscriptions_lock_); + std::unique_ptr data_uploader_; }; } // namespace cartographer_grpc diff --git a/cartographer_grpc/map_builder_server_options.cc b/cartographer_grpc/map_builder_server_options.cc index 53fd072..a395dc0 100644 --- a/cartographer_grpc/map_builder_server_options.cc +++ b/cartographer_grpc/map_builder_server_options.cc @@ -34,6 +34,8 @@ proto::MapBuilderServerOptions CreateMapBuilderServerOptions( *map_builder_server_options.mutable_map_builder_options() = cartographer::mapping::CreateMapBuilderOptions( lua_parameter_dictionary->GetDictionary("map_builder").get()); + map_builder_server_options.set_uplink_server_address( + lua_parameter_dictionary->GetString("uplink_server_address")); return map_builder_server_options; } diff --git a/cartographer_grpc/proto/map_builder_server_options.proto b/cartographer_grpc/proto/map_builder_server_options.proto index 9b82b32..a861dbc 100644 --- a/cartographer_grpc/proto/map_builder_server_options.proto +++ b/cartographer_grpc/proto/map_builder_server_options.proto @@ -23,4 +23,5 @@ message MapBuilderServerOptions { int32 num_grpc_threads = 2; int32 num_event_threads = 3; cartographer.mapping.proto.MapBuilderOptions map_builder_options = 4; + string uplink_server_address = 5; } diff --git a/configuration_files/map_builder_server.lua b/configuration_files/map_builder_server.lua index b907eb2..7a5dde6 100644 --- a/configuration_files/map_builder_server.lua +++ b/configuration_files/map_builder_server.lua @@ -15,8 +15,9 @@ include "map_builder.lua" MAP_BUILDER_SERVER = { - server_address = "0.0.0.0:50051", + map_builder = MAP_BUILDER, num_event_threads = 4, num_grpc_threads = 4, - map_builder = MAP_BUILDER, + server_address = "0.0.0.0:50051", + uplink_server_address = "localhost:50052", }