[RSDK-1955] add orbslam GetInternalStateStream (#149)

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<GetPointCloudMapStreamResponse> *writer) {
+    return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
+}
+
+::grpc::Status SLAMServiceImpl::GetInternalStateStream(
+    ServerContext *context, const GetInternalStateStreamRequest *request,
+    ServerWriter<GetInternalStateStreamResponse> *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

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<bool> 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<GetPointCloudMapStreamResponse> *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<GetInternalStateStreamResponse> *writer) override;
+
     void ProcessDataOnline(ORB_SLAM3::System *SLAM);
 
     void ProcessDataOffline(ORB_SLAM3::System *SLAM);

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