Offline run

CMakeLists.txt

@@ -27,10 +27,10 @@ if (NOT CUDAToolkit_FOUND)
 endif()
 message(STATUS "CUDAToolkit found: ${CUDAToolkit_VERSION}")
 
-set(SPOT_SDK_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/extern/spot-sdk-install)
+set(SPOT_SDK_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/extern/spot-cpp-sdk) # changed from sdk-install
 set(OpenCV_INCLUDES ${CMAKE_CURRENT_SOURCE_DIR}/extern/opencv/include)
 set(OpenCV_LIBS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/extern/opencv/x64/vc16/lib)
-set(OpenCV_LIBS opencv_world4110.lib)
+set(OpenCV_LIBS opencv_world4120.lib) # change from 4110
 
 # Force protobuf to come from vcpkg, not libtorch
 find_package(Protobuf REQUIRED PATHS "${Protobuf_ROOT}" NO_DEFAULT_PATH)
@@ -51,7 +51,7 @@ endif()
 message(STATUS "Found libtorch: ${TORCH_LIBRARIES}")
 message(STATUS "Torch install prefix: ${TORCH_INSTALL_PREFIX}")
 
-set(ONNX_DIR ${CMAKE_CURRENT_SOURCE_DIR}/extern/onnxruntime-win-x64-gpu-1.22.0)
+set(ONNX_DIR ${CMAKE_CURRENT_SOURCE_DIR}/extern/onnxruntime-win-x64-gpu-1.23.0) # changed from 1.22.0
 # Check if ONNX_DIR exists
 if(NOT EXISTS ${ONNX_DIR})
     message(FATAL_ERROR "ONNX directory not found: ${ONNX_DIR}. Please ensure ONNX Runtime is correctly extracted.")
@@ -182,8 +182,9 @@ if(COPY_DLLS)
     )
 
     # List of required OpenCV DLLs
+    # changed from 4110
     set(OPENCV_DLLS
-        "opencv_world4110.dll"
+        "opencv_world4120.dll"
     )
 
     set(PLUGIN_DLLS

include/spot-observer.h

@@ -13,6 +13,22 @@
 extern "C" {
 #endif
 
+// Make dummy available to other files in namespace
+// change so all files use getter func?
+namespace SOb {
+ extern bool dummy;
+ extern int taken_from_dummy;
+ extern bool going_up;
+}
+
+// Make avaiable to integ-test
+
+UNITY_INTERFACE_EXPORT
+bool UNITY_INTERFACE_API getDummy();
+UNITY_INTERFACE_EXPORT
+void UNITY_INTERFACE_API setDummy(bool val);
+
+
 // Define a function pointer type for logging
 typedef void (*SOb_LogCallback)(const char* message);
 
@@ -28,6 +44,7 @@ enum SpotCamera {
     NUM_CAMERAS = 0x40,
 };
 
+
 // Unity Stuff
 void UNITY_INTERFACE_EXPORT UNITY_INTERFACE_API UnityPluginLoad(IUnityInterfaces* unity);
 void UNITY_INTERFACE_EXPORT UNITY_INTERFACE_API UnityPluginUnload();

src/spot-connection.cpp

@@ -135,6 +135,7 @@ bool ReaderWriterCBuf::initialize(
  * Push image data to queue (non-blocking, drops oldest if full)
  */
 void ReaderWriterCBuf::push(const google::protobuf::RepeatedPtrField<bosdyn::api::ImageResponse>& responses) {
+    // std::cout << "Dummy: " << (dummy ? "true" : "false") << std::endl;
     using namespace std::chrono;
 
     if (n_elems_per_rgb_ == 0 || n_elems_per_depth_ == 0) {
@@ -159,6 +160,144 @@ void ReaderWriterCBuf::push(const google::protobuf::RepeatedPtrField<bosdyn::api
     int32_t n_rgbs_written = 0;
     int32_t n_depths_written = 0;
 
+    if (dummy) {
+        for (int i = 0; i < 2; i++) {
+        if (going_up && taken_from_dummy >= 100) {
+            taken_from_dummy = 97;
+            going_up = false;
+            LogMessage("Dummy mode: reset taken_from_dummy counter");
+        } else if (!going_up && taken_from_dummy <= 0) {
+            taken_from_dummy = 2;
+            going_up = true;
+            LogMessage("Dummy mode: reset taken_from_dummy counter");
+        }
+
+            cv::Mat cv_img = cv::imread(std::format("..\\..\\saved_images\\spot_rgb{}.png", 1 + (taken_from_dummy/2)), cv::IMREAD_UNCHANGED);
+            cv::cvtColor(cv_img, cv_img, cv::COLOR_BGR2RGB);
+
+            std::vector<cv::Mat> channels;
+            cv::split(cv_img, channels);
+
+            // Create an alpha channel initialized to 255 (fully opaque)
+            cv::Mat alpha_channel = cv::Mat::ones(cv_img.size(), CV_8U) * 255;
+
+            // Add the alpha channel to the vector of channels
+            channels.push_back(alpha_channel);
+
+            // Merge the channels to create a BGRA image
+            cv::merge(channels, cv_img);
+            LogMessage("Dummy mode: read image with size {}x{} and {} channels",
+                    cv_img.cols, cv_img.rows, cv_img.channels());
+
+            // this code can be decoupled from our friend spot so it doesnt need to be repeated
+            size_t image_size = cv_img.cols * cv_img.rows * 4;//(img.pixel_format() == bosdyn::api::Image::PIXEL_FORMAT_RGBA_U8 ? 4 : 3);
+                if (image_size != n_elems_per_rgb_) {
+                    LogMessage("Image size mismatch: expected {}, got {}", n_elems_per_rgb_, image_size);
+                    throw std::runtime_error("ReaderWriterCBuf::push: Image size mismatch");
+                }
+
+                // See if we need to do any cam-specific preprocessing:
+                switch (cameras_[n_rgbs_written]) {
+                case SpotCamera::FRONTLEFT:
+                case SpotCamera::FRONTRIGHT:
+                    break;
+                }
+
+                LogMessage("Copying RGB image to CUDA");
+                checkCudaError(
+                    cudaMemcpyAsync(
+                        rgb_write_ptr,
+                        cv_img.data,
+                        image_size * sizeof(uint8_t),
+                        cudaMemcpyHostToDevice,
+                        cuda_stream_ /* use per-connection stream */
+                    ),
+                    "cudaMemcpyAsync RGB"
+                );
+                LogMessage("Finished copying RGB image to CUDA");
+
+                rgb_write_ptr += n_elems_per_rgb_;
+                n_rgbs_written++;
+
+        // if (going_up) taken_from_dummy++;
+        // else taken_from_dummy--;
+
+        // cv::Mat
+            cv_img = cv::imread(std::format("..\\..\\saved_images\\spot_depth{}.png", 1 + (taken_from_dummy/2)), cv::IMREAD_UNCHANGED);
+
+            // we multiplied depth by 255 when saving, so divide back here
+            cv_img.convertTo(cv_img, CV_32FC1, 1.0/255.0);
+
+            LogMessage("Dummy mode: read depth image with size {}x{}",
+                    cv_img.cols, cv_img.rows);
+
+            int depth_width = cv_img.cols;
+            int depth_height = cv_img.rows;
+
+            size_t depth_size = depth_width * depth_height;
+            if (depth_size != n_elems_per_depth_) {
+                LogMessage("Depth size mismatch: expected {}, got {}", n_elems_per_depth_, depth_size);
+                throw std::runtime_error("ReaderWriterCBuf::push: Depth size mismatch");
+            }
+
+            // LogMessage("Copying depth image of size {} bytes to write pointer at index {}, {:#x} rgb_write_ptr",
+            //            depth_size, write_idx, size_t(depth_write_ptr));
+
+            checkCudaError(
+                cudaMemcpyAsync(
+                    depth_write_ptr,
+                    cv_img.data,
+                    depth_size * sizeof(float),
+                    cudaMemcpyHostToDevice,
+                    cuda_stream_
+                ),
+                "cudaMemcpyAsync DEPTH"
+            );
+
+            // If dumping requires device->host copies on default stream, this ensures correctness:
+            // checkCudaError(cudaStreamSynchronize(cuda_stream_), "sync before debug dump");
+            DumpDepthImageFromCuda(
+                depth_write_ptr,
+                cv_img.cols,
+                cv_img.rows,
+                "pre-depth-cache",
+                n_depths_written + write_idx * n_images_per_response_
+            );
+
+            DumpDepthImageFromCuda(
+                depth_write_ptr,
+                cv_img.cols,
+                cv_img.rows,
+                "post-depth-cache",
+                n_depths_written + write_idx * n_images_per_response_
+            );
+
+            depth_write_ptr += n_elems_per_depth_;
+            depth_cache_ptr += n_elems_per_depth_;
+            n_depths_written++;
+
+        checkCudaError(cudaStreamSynchronize(cuda_stream_), "cudaStreamSynchronize after push");
+
+        // Update indices/flags
+        assert(n_rgbs_written == n_depths_written);
+        // Update the read index to the write index we just wrote to.
+        read_idx_.store(write_idx, std::memory_order_release);
+        new_data_.store(true, std::memory_order_release);
+        LogMessage("ReaderWriterCBuf::push: updating write index from {} to {}",
+                    write_idx, (write_idx + 1) % max_size_);
+        write_idx = (write_idx + 1) % max_size_;
+        write_idx_.store(write_idx, std::memory_order_release);
+        first_run_ = false;
+
+        if (going_up) taken_from_dummy+=2;
+        else taken_from_dummy-=2;
+
+        }
+        return;
+
+
+    }
+
     for (const auto& response : responses) {
         const auto& img = response.shot().image();
 
@@ -411,6 +550,10 @@ bosdyn::api::GetImageRequest SpotCamStream::_createImageRequest(
 
 void SpotCamStream::_startStreamingThread() {
     // Create and start thread
+    if (!image_client_ && !dummy) {
+        std::cerr << "Image client not initialized" << std::endl;
+        return;
+    }
     image_streamer_thread_ = std::make_unique<std::jthread>([this](std::stop_token stop_token) {
         _spotCamReaderThread(stop_token);
     });
@@ -424,6 +567,14 @@ void SpotCamStream::_spotCamReaderThread(std::stop_token stop_token) {
         try {
             auto start = std::chrono::high_resolution_clock::now();
 
+            if (dummy) {
+                google::protobuf::RepeatedPtrField<bosdyn::api::ImageResponse> dummy_responses;
+                image_lifo_.push(dummy_responses);
+                LogMessage("Dummy mode: pushed dummy image responses");
+                std::this_thread::sleep_for(std::chrono::milliseconds(100));
+                continue;
+            }
+
             // Request images from all cameras
             bosdyn::client::GetImageResultType response = image_client_->GetImage(current_request_);
             if (!response.status) {
@@ -493,13 +644,51 @@ bool SpotCamStream::streamCameras(uint32_t cam_mask) {
 
         num_cams_requested = rgb_sources.size();
 
-        current_request_ = _createImageRequest(rgb_sources, depth_sources);
+        if (!dummy) current_request_ = _createImageRequest(rgb_sources, depth_sources);
         if (image_streamer_thread_ != nullptr) {
             _joinStreamingThread();
         }
     }
 
     try {
+        // AATASK: rewrite this part to give needed info from disk images
+        if (dummy) {
+            cv::Mat image = cv::imread(std::format("..\\..\\saved_images\\spot_rgb{}.png", 1), cv::IMREAD_COLOR);
+            cv::Size ref_size = image.size();
+            // Check if the image was loaded successfully
+            if (image.empty()) {
+                std::cerr << "Error: Could not open or find the image at " << std::format("..\\..\\saved_images\\spot_rgb{}.png", 1) << std::endl;
+                return false; // Indicate an error
+            }
+
+            current_rgb_shape_ = TensorShape{
+                size_t(num_cams_requested),
+                4, //(image_responses[0].shot().image().pixel_format() == bosdyn::api::Image::PIXEL_FORMAT_RGBA_U8 ? 4 : 3)
+                size_t(ref_size.height),
+                size_t(ref_size.width)
+            };
+
+            current_depth_shape_ = TensorShape{
+                size_t(num_cams_requested),
+                1,
+                size_t(ref_size.height),
+                size_t(ref_size.width)
+            };
+
+            image_lifo_.initialize(
+                ref_size.width * ref_size.height * 4, // RGBA
+                ref_size.width * ref_size.height,     // Depth
+                camera_order_
+            );
+
+            _startStreamingThread();
+            streaming_ = true;
+            current_cam_mask_ = cam_mask;
+            current_num_cams_ = num_cams_requested;
+            return true;
+
+
+        } 
         constexpr int32_t max_connection_retries = 3;
         // Query the robot for images and fill in image metadata
         // Max 3 retries
@@ -558,6 +747,7 @@ bool SpotCamStream::streamCameras(uint32_t cam_mask) {
                     return false;
                 }
             }
+
 
             // (Re)initialize circular buffer
             image_lifo_.initialize(
@@ -621,6 +811,13 @@ SpotConnection::SpotConnection(
     }
 
     try {
+        if (dummy) {
+            LogMessage("SpotConnection::connect: Dummy mode enabled, skipping actual connection");
+            connected_ = true;
+
+            return;
+        }
+
         // Create robot using ClientSDK
         bosdyn::client::Result<std::unique_ptr<bosdyn::client::Robot>> robot_result = sdk_->CreateRobot(robot_ip);
         if (!robot_result.status) {
@@ -736,6 +933,7 @@ SpotCamStream* SpotConnection::getCamStream(int32_t stream_id) {
 }
 
 bool SpotConnection::createVisionPipeline(MLModel& model, int32_t stream_id) {
+    LogMessage("SpotConnection::createVisionPipeline: Creating vision pipeline for stream ", stream_id);
     SpotCamStream* cam_stream = getCamStream(stream_id);
     if (cam_stream == nullptr || !cam_stream->isStreaming()) {
         LogMessage("SpotConnection::createVisionPipeline: Camera stream {} doesn't exist. "