From 0e2d28c98fbba6346ed9f678227b48c60a0abc2b Mon Sep 17 00:00:00 2001 From: JohnN193 <92045055+JohnN193@users.noreply.github.com> Date: Wed, 22 Feb 2023 13:33:04 -0500 Subject: [PATCH] [RSDK-1955] add orbslam GetInternalStateStream (#149) --- .../viam-orb-slam3/orbslam_server_v1.cc | 33 +++++++++++++++++++ .../viam-orb-slam3/orbslam_server_v1.h | 32 +++++++++++++++--- .../viam-orb-slam3/orbslam_server_v1_main.cc | 2 +- 3 files changed, 62 insertions(+), 5 deletions(-) diff --git a/slam-libraries/viam-orb-slam3/orbslam_server_v1.cc b/slam-libraries/viam-orb-slam3/orbslam_server_v1.cc index 4e9ab28e..30989e56 100644 --- a/slam-libraries/viam-orb-slam3/orbslam_server_v1.cc +++ b/slam-libraries/viam-orb-slam3/orbslam_server_v1.cc @@ -439,6 +439,39 @@ ::grpc::Status SLAMServiceImpl::GetInternalState( } } +::grpc::Status SLAMServiceImpl::GetPointCloudMapStream( + ServerContext *context, const GetPointCloudMapStreamRequest *request, + ServerWriter *writer) { + return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); +} + +::grpc::Status SLAMServiceImpl::GetInternalStateStream( + ServerContext *context, const GetInternalStateStreamRequest *request, + ServerWriter *writer) { + std::stringbuf buffer; + // deferring reading the osa file in chunks until we run into issues + // with loading the file into memory + bool success = ArchiveSlam(buffer); + if (!success) + return grpc::Status(grpc::StatusCode::UNAVAILABLE, + "SLAM is not yet initialized"); + + std::string internal_state_chunk; + GetInternalStateStreamResponse response; + std::string buffer_str = buffer.str(); + for (int start_index = 0; start_index < buffer_str.size(); + start_index += maximumGRPCByteChunkSize) { + internal_state_chunk = + buffer_str.substr(start_index, maximumGRPCByteChunkSize); + response.set_internal_state_chunk(internal_state_chunk); + bool ok = writer->Write(response); + if (!ok) + return grpc::Status(grpc::StatusCode::UNAVAILABLE, + "error while writing to stream: stream closed"); + } + + return grpc::Status::OK; +} // TODO: This is an antipattern, which only exists b/c: // 1. we only have one class for both the data thread(s) // & GRPC server diff --git a/slam-libraries/viam-orb-slam3/orbslam_server_v1.h b/slam-libraries/viam-orb-slam3/orbslam_server_v1.h index 0fdda59c..1c27b186 100644 --- a/slam-libraries/viam-orb-slam3/orbslam_server_v1.h +++ b/slam-libraries/viam-orb-slam3/orbslam_server_v1.h @@ -12,12 +12,17 @@ #include "service/slam/v1/slam.pb.h" using grpc::ServerContext; +using grpc::ServerWriter; using viam::service::slam::v1::GetInternalStateRequest; using viam::service::slam::v1::GetInternalStateResponse; +using viam::service::slam::v1::GetInternalStateStreamRequest; +using viam::service::slam::v1::GetInternalStateStreamResponse; using viam::service::slam::v1::GetMapRequest; using viam::service::slam::v1::GetMapResponse; using viam::service::slam::v1::GetPointCloudMapRequest; using viam::service::slam::v1::GetPointCloudMapResponse; +using viam::service::slam::v1::GetPointCloudMapStreamRequest; +using viam::service::slam::v1::GetPointCloudMapStreamResponse; using viam::service::slam::v1::GetPositionNewRequest; using viam::service::slam::v1::GetPositionNewResponse; using viam::service::slam::v1::GetPositionRequest; @@ -30,6 +35,10 @@ namespace viam { static const int filenamePrefixLength = 6; static const int checkForShutdownIntervalMicroseconds = 1e5; extern std::atomic b_continue_session; +// Byte limit on unary GRPC calls +static const int maximumGRPCByteLimit = 32 * 1024 * 1024; +// Byte limit for chunks on GRPC, used for streaming apis +static const int maximumGRPCByteChunkSize = 64 * 1024; class SLAMServiceImpl final : public SLAMService::Service { public: @@ -50,20 +59,35 @@ class SLAMServiceImpl final : public SLAMService::Service { // For a given GetPointCloudMapRequest // Returns a GetPointCloudMapResponse containing a sparse - // slam map as Binary PCD - // Map uses z axis is in the direction the camera is facing + // slam map as Binary PCD. The z-axis represents the direction the camera is + // facing at the origin of the map ::grpc::Status GetPointCloudMap( ServerContext *context, const GetPointCloudMapRequest *request, GetPointCloudMapResponse *response) override; // For a given GetInternalStateRequest // Returns a GetInternalStateResponse containing - // the internal state of the SLAM algorithm - // required to continue mapping/localization + // current internal state of the map represented as an ORB-SLAM Atlas(.osa) + // file in chunks of size maximumGRPCByteChunkSize ::grpc::Status GetInternalState( ServerContext *context, const GetInternalStateRequest *request, GetInternalStateResponse *response) override; + // GetPointCloudMap returns a stream containing a sparse + // slam map as Binary PCD. In chunks of size maximumGRPCByteChunkSize. + // The z-axis represents the direction the camera is + // facing at the origin of the map + ::grpc::Status GetPointCloudMapStream( + ServerContext *context, const GetPointCloudMapStreamRequest *request, + ServerWriter *writer) override; + + // GetInternalStateStream returns a stream of the current internal state of + // the map represented as an ORB-SLAM Atlas(.osa) file in chunks of size + // maximumGRPCByteChunkSize + ::grpc::Status GetInternalStateStream( + ServerContext *context, const GetInternalStateStreamRequest *request, + ServerWriter *writer) override; + void ProcessDataOnline(ORB_SLAM3::System *SLAM); void ProcessDataOffline(ORB_SLAM3::System *SLAM); diff --git a/slam-libraries/viam-orb-slam3/orbslam_server_v1_main.cc b/slam-libraries/viam-orb-slam3/orbslam_server_v1_main.cc index 52254fc9..5dfc828b 100644 --- a/slam-libraries/viam-orb-slam3/orbslam_server_v1_main.cc +++ b/slam-libraries/viam-orb-slam3/orbslam_server_v1_main.cc @@ -56,7 +56,7 @@ int main(int argc, char **argv) { // Increasing the gRPC max message size from the default value of 4MB to // 32MB, to match the limit that is set in RDK. This is necessary for // transmitting large pointclouds. - builder.SetMaxSendMessageSize(32 * 1024 * 1024); + builder.SetMaxSendMessageSize(viam::maximumGRPCByteLimit); builder.RegisterService(&slamService); // Start the SLAM gRPC server