New interop api -- WIP

CMakeLists.txt

@@ -5,6 +5,10 @@ option(BUILD_TESTS "Build the test executable" ON)
 option(INSTALL_TESTS "Install test executables" ON)
 set(PLUGIN_NAME "SpotObserverLib")
 
+if(BUILD_TESTS)
+    enable_testing()
+endif()
+
 # Set default install directory to SpotObserver/install if not specified
 if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT)
     set(CMAKE_INSTALL_PREFIX "${CMAKE_CURRENT_SOURCE_DIR}/install" CACHE PATH "Default install directory" FORCE)
@@ -90,8 +94,26 @@ add_library(${PLUGIN_NAME} SHARED
     src/load_image_to_cuda.cu
 )
 
-# Unity Native-plugin API includes TODO: cleanup
-set(UnityPluginAPI_INCLUDE "C:/Program Files/Unity/Hub/Editor/6000.2.0f1/Editor/Data/PluginAPI")
+if(BUILD_TESTS)
+    target_compile_definitions(${PLUGIN_NAME} PRIVATE SOB_ENABLE_TEST_HOOKS)
+endif()
+
+# Unity Native-plugin API includes
+set(UnityPluginAPI_INCLUDE "" CACHE PATH "Path to Unity Editor/Data/PluginAPI")
+if(NOT UnityPluginAPI_INCLUDE)
+    file(GLOB UNITY_PLUGIN_API_CANDIDATES
+        "C:/Program Files/Unity/Hub/Editor/*/Editor/Data/PluginAPI"
+    )
+    if(UNITY_PLUGIN_API_CANDIDATES)
+        list(SORT UNITY_PLUGIN_API_CANDIDATES)
+        list(REVERSE UNITY_PLUGIN_API_CANDIDATES)
+        list(GET UNITY_PLUGIN_API_CANDIDATES 0 UnityPluginAPI_INCLUDE)
+    endif()
+endif()
+if(NOT UnityPluginAPI_INCLUDE OR NOT EXISTS "${UnityPluginAPI_INCLUDE}")
+    message(FATAL_ERROR "Unity PluginAPI directory not found. Set UnityPluginAPI_INCLUDE to Editor/Data/PluginAPI.")
+endif()
+message(STATUS "Using Unity PluginAPI include: ${UnityPluginAPI_INCLUDE}")
 
 # Include directories
 # TODO: Use target_include_directories instead of include_directories

README.md

@@ -0,0 +1,82 @@
+# SpotObserver
+
+A C++ application for observing and interacting with Boston Dynamics Spot robot.
+
+## Project Structure
+
+- `src/main.cpp` - Full implementation using Boston Dynamics SDK
+- `src/main_simple.cpp` - Simplified version without SDK dependencies for testing compilation
+- `extern/spot-cpp-sdk/` - Boston Dynamics Spot C++ SDK submodule
+
+## Building
+
+This requires installing dependencies and builds the full version that can actually communicate with Spot:
+
+#### Prerequisites
+
+1. **Install vcpkg dependencies** (as described in `extern/spot-cpp-sdk/docs/cpp/quickstart.md`):
+   ```bash
+   git clone https://github.com/microsoft/vcpkg
+   cd vcpkg
+   git checkout 3b213864579b6fa686e38715508f7cd41a50900f
+
+   # On Windows:
+   .\bootstrap-vcpkg.bat
+   .\vcpkg.exe install grpc:x64-windows
+   .\vcpkg.exe install eigen3:x64-windows
+   .\vcpkg.exe install cli11:x64-windows
+
+2. **Build with SDK**:
+   ```bash
+   mkdir build
+   cd build
+   cmake .. -DBUILD_WITH_SPOT_SDK=ON -DCMAKE_TOOLCHAIN_FILE=<path_to_vcpkg>/scripts/buildsystems/vcpkg.cmake
+   cmake --build .
+   ```
+
+## Usage
+
+### Simple Version
+```bash
+./SpotObserver username 192.168.80.3 password
+```
+
+### Full Version (with SDK)
+```bash
+./SpotObserver username <spot_ip_address> password
+```
+
+## Features
+
+- **SpotConfig**: Configuration structure for robot connection parameters
+- **LeaseGuard**: RAII wrapper for automatic lease management
+- **Authentication**: Handles robot authentication using username/password
+- **Robot State**: Retrieves and displays robot status information
+- **Lease Management**: Automatically acquires and releases robot control lease
+
+## Code Structure
+
+The main application follows this flow:
+1. Parse command line arguments (username, IP, password)
+2. Create gRPC channel to robot
+3. Authenticate with the robot
+4. Acquire a lease for robot control
+5. Use RAII LeaseGuard to ensure lease is properly released
+6. Query robot state and display information
+7. Clean up and exit
+
+The `LeaseGuard` class ensures that robot leases are properly released even if the program exits unexpectedly, following RAII principles.
+
+## Requirements
+
+- C++17 or later
+- CMake 3.10+
+- For full build: gRPC, Protobuf, Eigen3, CLI11 (via vcpkg)
+
+## Boston Dynamics SDK
+
+This project uses the Boston Dynamics Spot C++ SDK v5.0.0. The SDK is included as a git submodule in the `extern/spot-cpp-sdk` directory.
+
+For detailed information about the SDK, see:
+- [C++ SDK Documentation](extern/spot-cpp-sdk/docs/cpp/README.md)
+- [QuickStart Guide](extern/spot-cpp-sdk/docs/cpp/quickstart.md)

include/spot-observer.h

@@ -18,6 +18,28 @@ typedef void (*SOb_LogCallback)(const char* message);
 
 typedef void* SObModel;
 
+#ifdef SOB_ENABLE_TEST_HOOKS
+typedef bool (UNITY_INTERFACE_API *SOb_TestImageProvider)(
+    int32_t robot_id,
+    int32_t cam_stream_id,
+    int32_t n_images_requested,
+    uint8_t** images,
+    float** depths
+);
+#endif
+
+enum SOb_UnityUploadSourceKind {
+    SOb_UnityUploadSource_RawCameraFrames = 0,
+    SOb_UnityUploadSource_VisionPipelineFrames = 1,
+#ifdef SOB_ENABLE_TEST_HOOKS
+    SOb_UnityUploadSource_TestFrames = 1000,
+#endif
+};
+
+enum {
+    SOb_UnityUploadEventId = 0x534F6255, // "SObU"
+};
+
 enum SpotCamera {
     BACK        = 0x1,
     FRONTLEFT   = 0x2,
@@ -70,6 +92,17 @@ bool UNITY_INTERFACE_API SOb_RegisterUnityReadbackBuffers(
 UNITY_INTERFACE_EXPORT
 bool UNITY_INTERFACE_API SOb_ClearUnityReadbackBuffers(int32_t robot_id);
 
+UNITY_INTERFACE_EXPORT
+void* UNITY_INTERFACE_API SOb_GetRenderEventFunc();
+
+UNITY_INTERFACE_EXPORT
+bool UNITY_INTERFACE_API SOb_EnqueueUnityUpload(int32_t robot_id, int32_t cam_stream_id, int32_t source_kind);
+
+#ifdef SOB_ENABLE_TEST_HOOKS
+UNITY_INTERFACE_EXPORT
+bool UNITY_INTERFACE_API SOb_Test_SetUnityUploadImageProvider(SOb_TestImageProvider provider);
+#endif
+
 UNITY_INTERFACE_EXPORT
 bool UNITY_INTERFACE_API SOb_LaunchVisionPipeline(
     int32_t robot_id,

src/include/unity-cuda-interop.h

@@ -42,6 +42,13 @@ bool registerOutputTextures(
 
 bool uploadNextImageSetToUnity(int32_t robot_id, int32_t cam_stream_id);
 bool uploadNextVisionPipelineImageSetToUnity(int32_t robot_id, int32_t cam_stream_id);
+void* getRenderEventFunc();
+bool enqueueUnityUpload(int32_t robot_id, int32_t cam_stream_id, int32_t source_kind);
 bool clearOutputTextures(int32_t robot_id);
+bool ensureCudaDeviceForD3D12Interop();
+int getD3D12InteropCudaDevice();
+#ifdef SOB_ENABLE_TEST_HOOKS
+bool setTestUnityUploadImageProvider(SOb_TestImageProvider provider);
+#endif
 
-}
+}

src/spot-connection.cpp

@@ -7,6 +7,7 @@
 #include "utils.h"
 #include "dumper.h"
 #include "vision-pipeline.h"
+#include "unity-cuda-interop.h"
 
 #include <opencv2/opencv.hpp>
 
@@ -108,6 +109,10 @@ bool ReaderWriterCBuf::initialize(
     }
 
     // Allocate CUDA memory for circular buffer
+    if (!ensureCudaDeviceForD3D12Interop()) {
+        throw std::runtime_error("ReaderWriterCBuf::initialize: Failed to select D3D12 interop CUDA device");
+    }
+
     size_t total_size_rgb   = max_size_ * n_elems_per_rgb * n_images_per_response_ * sizeof(uint8_t);
     size_t size_depth_per_response = n_elems_per_depth * n_images_per_response_ * sizeof(float);
     size_t total_size_depth = max_size_ * size_depth_per_response;
@@ -359,6 +364,10 @@ SpotCamStream::SpotCamStream(
     , streaming_(false)
 {
     // Create one CUDA stream per SpotConnection and attach it to the buffer.
+    if (!ensureCudaDeviceForD3D12Interop()) {
+        throw std::runtime_error("SpotCamStream::SpotCamStream: Failed to select D3D12 interop CUDA device");
+    }
+
     checkCudaError(
         cudaStreamCreate(&cuda_stream_),
         "cudaStreamCreate for SpotConnection"

src/spot-observer.cpp

@@ -545,6 +545,35 @@ bool UNITY_INTERFACE_API SOb_ClearUnityReadbackBuffers(int32_t robot_id) {
     }
 }
 
+UNITY_INTERFACE_EXPORT
+void* UNITY_INTERFACE_API SOb_GetRenderEventFunc() {
+    return SOb::getRenderEventFunc();
+}
+
+UNITY_INTERFACE_EXPORT
+bool UNITY_INTERFACE_API SOb_EnqueueUnityUpload(int32_t robot_id, int32_t cam_stream_id, int32_t source_kind) {
+    try {
+        bool ret = SOb::enqueueUnityUpload(robot_id, cam_stream_id, source_kind);
+        if (!ret) {
+            SOb::LogMessage("SOb_EnqueueUnityUpload: Failed to enqueue upload for robot ID {} @ cam-stream ID {}",
+                robot_id, cam_stream_id);
+            return false;
+        }
+        return true;
+    } catch (const std::exception& e) {
+        SOb::LogMessage("SOb_EnqueueUnityUpload: Exception while enqueueing upload for robot ID {} @ cam-stream ID {}: {}",
+            robot_id, cam_stream_id, e.what());
+        return false;
+    }
+}
+
+#ifdef SOB_ENABLE_TEST_HOOKS
+UNITY_INTERFACE_EXPORT
+bool UNITY_INTERFACE_API SOb_Test_SetUnityUploadImageProvider(SOb_TestImageProvider provider) {
+    return SOb::setTestUnityUploadImageProvider(provider);
+}
+#endif
+
 UNITY_INTERFACE_EXPORT
 bool UNITY_INTERFACE_API SOb_GetNextImageSet(
     int32_t robot_id,
@@ -602,29 +631,31 @@ bool UNITY_INTERFACE_API SOb_GetNextVisionPipelineImageSet(
 UNITY_INTERFACE_EXPORT
 bool UNITY_INTERFACE_API SOb_PushNextImageSetToUnityBuffers(int32_t robot_id, int32_t cam_stream_id) {
     try {
+        SOb::LogMessage("SOb_PushNextImageSetToUnityBuffers is deprecated; use SOb_EnqueueUnityUpload and GL.IssuePluginEvent.");
         bool ret = SOb::uploadNextImageSetToUnity(robot_id, cam_stream_id);
         if (!ret) {
-            SOb::LogMessage("SOb_PushNextImageSetToUnityBuffers: Failed to upload next image set for robot ID {}", robot_id);
-            return false; // Failed to get images
+            SOb::LogMessage("SOb_PushNextImageSetToUnityBuffers: Failed to enqueue next image set for robot ID {}", robot_id);
+            return false;
         }
         return ret;
     } catch (const std::exception& e) {
-        SOb::LogMessage("SOb_PushNextImageSetToUnityBuffers: Exception while uploading next image set for robot ID {}: {}", robot_id, e.what());
+        SOb::LogMessage("SOb_PushNextImageSetToUnityBuffers: Exception while enqueueing next image set for robot ID {}: {}", robot_id, e.what());
         return false;
     }
 }
 
 UNITY_INTERFACE_EXPORT
 bool UNITY_INTERFACE_API SOb_PushNextVisionPipelineImageSetToUnityBuffers(int32_t robot_id, int32_t cam_stream_id) {
     try {
+        SOb::LogMessage("SOb_PushNextVisionPipelineImageSetToUnityBuffers is deprecated; use SOb_EnqueueUnityUpload and GL.IssuePluginEvent.");
         bool ret = SOb::uploadNextVisionPipelineImageSetToUnity(robot_id, cam_stream_id);
         if (!ret) {
-            SOb::LogMessage("SOb_PushNextVisionPipelineImageSetToUnityBuffers: Failed to upload next vision pipeline image set for robot ID {}", robot_id);
-            return false; // Failed to get images
+            SOb::LogMessage("SOb_PushNextVisionPipelineImageSetToUnityBuffers: Failed to enqueue next vision pipeline image set for robot ID {}", robot_id);
+            return false;
         }
         return ret;
     } catch (const std::exception& e) {
-        SOb::LogMessage("SOb_PushNextVisionPipelineImageSetToUnityBuffers: Exception while uploading next vision pipeline image set for robot ID {}: {}", robot_id, e.what());
+        SOb::LogMessage("SOb_PushNextVisionPipelineImageSetToUnityBuffers: Exception while enqueueing next vision pipeline image set for robot ID {}: {}", robot_id, e.what());
         return false;
     }
 }
@@ -675,4 +706,4 @@ void UNITY_INTERFACE_API SOb_ToggleDebugDumps(const char* dump_path) {
     SOb::LogMessage("Debug dumps enabled successfully");
 }
 
-} // extern "C"
+} // extern "C"