diff --git a/CMakeLists.txt b/CMakeLists.txt index 534999b..ff7ef57 100644 --- a/CMakeLists.txt +++ b/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) # 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) # 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.") @@ -183,7 +183,7 @@ if(COPY_DLLS) # List of required OpenCV DLLs set(OPENCV_DLLS - "opencv_world4110.dll" + "opencv_world4120.dll" ) set(PLUGIN_DLLS diff --git a/include/spot-observer.h b/include/spot-observer.h index f619474..be5510f 100644 --- a/include/spot-observer.h +++ b/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 available 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(); diff --git a/saved_images/spot_depth1.png b/saved_images/spot_depth1.png new file mode 100644 index 0000000..1699aae Binary files /dev/null and b/saved_images/spot_depth1.png differ diff --git a/saved_images/spot_depth10.png b/saved_images/spot_depth10.png new file mode 100644 index 0000000..65a8cff Binary files /dev/null and b/saved_images/spot_depth10.png differ diff --git a/saved_images/spot_depth11.png b/saved_images/spot_depth11.png new file mode 100644 index 0000000..99b9032 Binary files /dev/null and b/saved_images/spot_depth11.png differ diff --git a/saved_images/spot_depth12.png b/saved_images/spot_depth12.png new file mode 100644 index 0000000..c05653a Binary files /dev/null and b/saved_images/spot_depth12.png differ diff --git a/saved_images/spot_depth13.png b/saved_images/spot_depth13.png new file mode 100644 index 0000000..c34136d Binary files /dev/null and b/saved_images/spot_depth13.png differ diff --git a/saved_images/spot_depth14.png b/saved_images/spot_depth14.png new file mode 100644 index 0000000..e1453ba Binary files /dev/null and b/saved_images/spot_depth14.png differ diff --git a/saved_images/spot_depth15.png b/saved_images/spot_depth15.png new file mode 100644 index 0000000..d85a809 Binary files /dev/null and b/saved_images/spot_depth15.png differ diff --git a/saved_images/spot_depth16.png b/saved_images/spot_depth16.png new file mode 100644 index 0000000..c86802d Binary files /dev/null and b/saved_images/spot_depth16.png differ diff --git a/saved_images/spot_depth17.png b/saved_images/spot_depth17.png new file mode 100644 index 0000000..b2aa13b Binary files /dev/null and b/saved_images/spot_depth17.png differ diff --git a/saved_images/spot_depth18.png b/saved_images/spot_depth18.png new file mode 100644 index 0000000..c75fb3c Binary files /dev/null and b/saved_images/spot_depth18.png differ diff --git a/saved_images/spot_depth19.png b/saved_images/spot_depth19.png new file mode 100644 index 0000000..ea18f90 Binary files /dev/null and b/saved_images/spot_depth19.png differ diff --git a/saved_images/spot_depth2.png b/saved_images/spot_depth2.png new file mode 100644 index 0000000..0d0fa54 Binary files /dev/null and b/saved_images/spot_depth2.png differ diff --git a/saved_images/spot_depth20.png b/saved_images/spot_depth20.png new file mode 100644 index 0000000..71dd44f Binary files /dev/null and b/saved_images/spot_depth20.png differ diff --git a/saved_images/spot_depth21.png b/saved_images/spot_depth21.png new file mode 100644 index 0000000..1e8d383 Binary files /dev/null and b/saved_images/spot_depth21.png differ diff --git a/saved_images/spot_depth22.png b/saved_images/spot_depth22.png new file mode 100644 index 0000000..4422bbe Binary files /dev/null and b/saved_images/spot_depth22.png differ diff --git a/saved_images/spot_depth23.png b/saved_images/spot_depth23.png new file mode 100644 index 0000000..4a226a2 Binary files /dev/null and b/saved_images/spot_depth23.png differ diff --git a/saved_images/spot_depth24.png b/saved_images/spot_depth24.png new file mode 100644 index 0000000..e866e1f Binary files /dev/null and b/saved_images/spot_depth24.png differ diff --git a/saved_images/spot_depth25.png b/saved_images/spot_depth25.png new file mode 100644 index 0000000..665c728 Binary files /dev/null and b/saved_images/spot_depth25.png differ diff --git a/saved_images/spot_depth26.png b/saved_images/spot_depth26.png new file mode 100644 index 0000000..d20ac94 Binary files /dev/null and b/saved_images/spot_depth26.png differ diff --git a/saved_images/spot_depth27.png b/saved_images/spot_depth27.png new file mode 100644 index 0000000..6cca73d Binary files /dev/null and b/saved_images/spot_depth27.png differ diff --git a/saved_images/spot_depth28.png b/saved_images/spot_depth28.png new file mode 100644 index 0000000..6df55b8 Binary files /dev/null and b/saved_images/spot_depth28.png differ diff --git a/saved_images/spot_depth29.png b/saved_images/spot_depth29.png new file mode 100644 index 0000000..f4e7e28 Binary files /dev/null and b/saved_images/spot_depth29.png differ diff --git a/saved_images/spot_depth3.png b/saved_images/spot_depth3.png new file mode 100644 index 0000000..5e3e863 Binary files /dev/null and b/saved_images/spot_depth3.png differ diff --git a/saved_images/spot_depth30.png b/saved_images/spot_depth30.png new file mode 100644 index 0000000..11c1558 Binary files /dev/null and b/saved_images/spot_depth30.png differ diff --git a/saved_images/spot_depth31.png b/saved_images/spot_depth31.png new file mode 100644 index 0000000..c154312 Binary files /dev/null and b/saved_images/spot_depth31.png differ diff --git a/saved_images/spot_depth32.png b/saved_images/spot_depth32.png new file mode 100644 index 0000000..0132ae9 Binary files /dev/null and b/saved_images/spot_depth32.png differ diff --git a/saved_images/spot_depth33.png b/saved_images/spot_depth33.png new file mode 100644 index 0000000..4560e97 Binary files /dev/null and b/saved_images/spot_depth33.png differ diff --git a/saved_images/spot_depth34.png b/saved_images/spot_depth34.png new file mode 100644 index 0000000..b4e95c6 Binary files /dev/null and b/saved_images/spot_depth34.png differ diff --git a/saved_images/spot_depth35.png b/saved_images/spot_depth35.png new file mode 100644 index 0000000..44be67a Binary files /dev/null and b/saved_images/spot_depth35.png differ diff --git a/saved_images/spot_depth36.png b/saved_images/spot_depth36.png new file mode 100644 index 0000000..7965b1e Binary files /dev/null and b/saved_images/spot_depth36.png differ diff --git a/saved_images/spot_depth37.png b/saved_images/spot_depth37.png new file mode 100644 index 0000000..063b1c9 Binary files /dev/null and b/saved_images/spot_depth37.png differ diff --git a/saved_images/spot_depth38.png b/saved_images/spot_depth38.png new file mode 100644 index 0000000..128ef52 Binary files /dev/null and b/saved_images/spot_depth38.png differ diff --git a/saved_images/spot_depth39.png b/saved_images/spot_depth39.png new file mode 100644 index 0000000..647776f Binary files /dev/null and b/saved_images/spot_depth39.png differ diff --git a/saved_images/spot_depth4.png b/saved_images/spot_depth4.png new file mode 100644 index 0000000..98c5515 Binary files /dev/null and b/saved_images/spot_depth4.png differ diff --git a/saved_images/spot_depth40.png b/saved_images/spot_depth40.png new file mode 100644 index 0000000..9386b42 Binary files /dev/null and b/saved_images/spot_depth40.png differ diff --git a/saved_images/spot_depth41.png b/saved_images/spot_depth41.png new file mode 100644 index 0000000..c10393e Binary files /dev/null and b/saved_images/spot_depth41.png differ diff --git a/saved_images/spot_depth42.png b/saved_images/spot_depth42.png new file mode 100644 index 0000000..7711ba5 Binary files /dev/null and b/saved_images/spot_depth42.png differ diff --git a/saved_images/spot_depth43.png b/saved_images/spot_depth43.png new file mode 100644 index 0000000..478f1b5 Binary files /dev/null and b/saved_images/spot_depth43.png differ diff --git a/saved_images/spot_depth44.png b/saved_images/spot_depth44.png new file mode 100644 index 0000000..99cd15a Binary files /dev/null and b/saved_images/spot_depth44.png differ diff --git a/saved_images/spot_depth45.png b/saved_images/spot_depth45.png new file mode 100644 index 0000000..6d822a6 Binary files /dev/null and b/saved_images/spot_depth45.png differ diff --git a/saved_images/spot_depth46.png b/saved_images/spot_depth46.png new file mode 100644 index 0000000..16b7665 Binary files /dev/null and b/saved_images/spot_depth46.png differ diff --git a/saved_images/spot_depth47.png b/saved_images/spot_depth47.png new file mode 100644 index 0000000..98aac33 Binary files /dev/null and b/saved_images/spot_depth47.png differ diff --git a/saved_images/spot_depth48.png b/saved_images/spot_depth48.png new file mode 100644 index 0000000..e03b59c Binary files /dev/null and b/saved_images/spot_depth48.png differ diff --git a/saved_images/spot_depth49.png b/saved_images/spot_depth49.png new file mode 100644 index 0000000..113266c Binary files /dev/null and b/saved_images/spot_depth49.png differ diff --git a/saved_images/spot_depth5.png b/saved_images/spot_depth5.png new file mode 100644 index 0000000..52ab002 Binary files /dev/null and b/saved_images/spot_depth5.png differ diff --git a/saved_images/spot_depth50.png b/saved_images/spot_depth50.png new file mode 100644 index 0000000..8445efa Binary files /dev/null and b/saved_images/spot_depth50.png differ diff --git a/saved_images/spot_depth6.png b/saved_images/spot_depth6.png new file mode 100644 index 0000000..372e453 Binary files /dev/null and b/saved_images/spot_depth6.png differ diff --git a/saved_images/spot_depth7.png b/saved_images/spot_depth7.png new file mode 100644 index 0000000..88914c4 Binary files /dev/null and b/saved_images/spot_depth7.png differ diff --git a/saved_images/spot_depth8.png b/saved_images/spot_depth8.png new file mode 100644 index 0000000..96de746 Binary files /dev/null and b/saved_images/spot_depth8.png differ diff --git a/saved_images/spot_depth9.png b/saved_images/spot_depth9.png new file mode 100644 index 0000000..138e0cc Binary files /dev/null and b/saved_images/spot_depth9.png differ diff --git a/saved_images/spot_rgb1.png b/saved_images/spot_rgb1.png new file mode 100644 index 0000000..87d5beb Binary files /dev/null and b/saved_images/spot_rgb1.png differ diff --git a/saved_images/spot_rgb10.png b/saved_images/spot_rgb10.png new file mode 100644 index 0000000..0cefc64 Binary files /dev/null and b/saved_images/spot_rgb10.png differ diff --git a/saved_images/spot_rgb11.png b/saved_images/spot_rgb11.png new file mode 100644 index 0000000..436aa81 Binary files /dev/null and b/saved_images/spot_rgb11.png differ diff --git a/saved_images/spot_rgb12.png b/saved_images/spot_rgb12.png new file mode 100644 index 0000000..9ec624b Binary files /dev/null and b/saved_images/spot_rgb12.png differ diff --git a/saved_images/spot_rgb13.png b/saved_images/spot_rgb13.png new file mode 100644 index 0000000..ae6a3a3 Binary files /dev/null and b/saved_images/spot_rgb13.png differ diff --git a/saved_images/spot_rgb14.png b/saved_images/spot_rgb14.png new file mode 100644 index 0000000..eb2de2a Binary files /dev/null and b/saved_images/spot_rgb14.png differ diff --git a/saved_images/spot_rgb15.png b/saved_images/spot_rgb15.png new file mode 100644 index 0000000..0a83e69 Binary files /dev/null and b/saved_images/spot_rgb15.png differ diff --git a/saved_images/spot_rgb16.png b/saved_images/spot_rgb16.png new file mode 100644 index 0000000..3f8b4da Binary files /dev/null and b/saved_images/spot_rgb16.png differ diff --git a/saved_images/spot_rgb17.png b/saved_images/spot_rgb17.png new file mode 100644 index 0000000..924c211 Binary files /dev/null and b/saved_images/spot_rgb17.png differ diff --git a/saved_images/spot_rgb18.png b/saved_images/spot_rgb18.png new file mode 100644 index 0000000..1d8b255 Binary files /dev/null and b/saved_images/spot_rgb18.png differ diff --git a/saved_images/spot_rgb19.png b/saved_images/spot_rgb19.png new file mode 100644 index 0000000..2284f84 Binary files /dev/null and b/saved_images/spot_rgb19.png differ diff --git a/saved_images/spot_rgb2.png b/saved_images/spot_rgb2.png new file mode 100644 index 0000000..fd3f126 Binary files /dev/null and b/saved_images/spot_rgb2.png differ diff --git a/saved_images/spot_rgb20.png b/saved_images/spot_rgb20.png new file mode 100644 index 0000000..1c82870 Binary files /dev/null and b/saved_images/spot_rgb20.png differ diff --git a/saved_images/spot_rgb21.png b/saved_images/spot_rgb21.png new file mode 100644 index 0000000..bcd438a Binary files /dev/null and b/saved_images/spot_rgb21.png differ diff --git a/saved_images/spot_rgb22.png b/saved_images/spot_rgb22.png new file mode 100644 index 0000000..a01ae63 Binary files /dev/null and b/saved_images/spot_rgb22.png differ diff --git a/saved_images/spot_rgb23.png b/saved_images/spot_rgb23.png new file mode 100644 index 0000000..239014c Binary files /dev/null and b/saved_images/spot_rgb23.png differ diff --git a/saved_images/spot_rgb24.png b/saved_images/spot_rgb24.png new file mode 100644 index 0000000..364d4d5 Binary files /dev/null and b/saved_images/spot_rgb24.png differ diff --git a/saved_images/spot_rgb25.png b/saved_images/spot_rgb25.png new file mode 100644 index 0000000..5de7aaf Binary files /dev/null and b/saved_images/spot_rgb25.png differ diff --git a/saved_images/spot_rgb26.png b/saved_images/spot_rgb26.png new file mode 100644 index 0000000..e721f12 Binary files /dev/null and b/saved_images/spot_rgb26.png differ diff --git a/saved_images/spot_rgb27.png b/saved_images/spot_rgb27.png new file mode 100644 index 0000000..d74789b Binary files /dev/null and b/saved_images/spot_rgb27.png differ diff --git a/saved_images/spot_rgb28.png b/saved_images/spot_rgb28.png new file mode 100644 index 0000000..a9ada54 Binary files /dev/null and b/saved_images/spot_rgb28.png differ diff --git a/saved_images/spot_rgb29.png b/saved_images/spot_rgb29.png new file mode 100644 index 0000000..de050b9 Binary files /dev/null and b/saved_images/spot_rgb29.png differ diff --git a/saved_images/spot_rgb3.png b/saved_images/spot_rgb3.png new file mode 100644 index 0000000..f557605 Binary files /dev/null and b/saved_images/spot_rgb3.png differ diff --git a/saved_images/spot_rgb30.png b/saved_images/spot_rgb30.png new file mode 100644 index 0000000..e3cf1a3 Binary files /dev/null and b/saved_images/spot_rgb30.png differ diff --git a/saved_images/spot_rgb31.png b/saved_images/spot_rgb31.png new file mode 100644 index 0000000..95069d9 Binary files /dev/null and b/saved_images/spot_rgb31.png differ diff --git a/saved_images/spot_rgb32.png b/saved_images/spot_rgb32.png new file mode 100644 index 0000000..ef68abb Binary files /dev/null and b/saved_images/spot_rgb32.png differ diff --git a/saved_images/spot_rgb33.png b/saved_images/spot_rgb33.png new file mode 100644 index 0000000..882e75b Binary files /dev/null and b/saved_images/spot_rgb33.png differ diff --git a/saved_images/spot_rgb34.png b/saved_images/spot_rgb34.png new file mode 100644 index 0000000..599125c Binary files /dev/null and b/saved_images/spot_rgb34.png differ diff --git a/saved_images/spot_rgb35.png b/saved_images/spot_rgb35.png new file mode 100644 index 0000000..c153c83 Binary files /dev/null and b/saved_images/spot_rgb35.png differ diff --git a/saved_images/spot_rgb36.png b/saved_images/spot_rgb36.png new file mode 100644 index 0000000..e6f9a2d Binary files /dev/null and b/saved_images/spot_rgb36.png differ diff --git a/saved_images/spot_rgb37.png b/saved_images/spot_rgb37.png new file mode 100644 index 0000000..550d82a Binary files /dev/null and b/saved_images/spot_rgb37.png differ diff --git a/saved_images/spot_rgb38.png b/saved_images/spot_rgb38.png new file mode 100644 index 0000000..bffa928 Binary files /dev/null and b/saved_images/spot_rgb38.png differ diff --git a/saved_images/spot_rgb39.png b/saved_images/spot_rgb39.png new file mode 100644 index 0000000..67eccdf Binary files /dev/null and b/saved_images/spot_rgb39.png differ diff --git a/saved_images/spot_rgb4.png b/saved_images/spot_rgb4.png new file mode 100644 index 0000000..c5259d7 Binary files /dev/null and b/saved_images/spot_rgb4.png differ diff --git a/saved_images/spot_rgb40.png b/saved_images/spot_rgb40.png new file mode 100644 index 0000000..701c74c Binary files /dev/null and b/saved_images/spot_rgb40.png differ diff --git a/saved_images/spot_rgb41.png b/saved_images/spot_rgb41.png new file mode 100644 index 0000000..340d2d0 Binary files /dev/null and b/saved_images/spot_rgb41.png differ diff --git a/saved_images/spot_rgb42.png b/saved_images/spot_rgb42.png new file mode 100644 index 0000000..9c4c011 Binary files /dev/null and b/saved_images/spot_rgb42.png differ diff --git a/saved_images/spot_rgb43.png b/saved_images/spot_rgb43.png new file mode 100644 index 0000000..e2623b1 Binary files /dev/null and b/saved_images/spot_rgb43.png differ diff --git a/saved_images/spot_rgb44.png b/saved_images/spot_rgb44.png new file mode 100644 index 0000000..fc1daf5 Binary files /dev/null and b/saved_images/spot_rgb44.png differ diff --git a/saved_images/spot_rgb45.png b/saved_images/spot_rgb45.png new file mode 100644 index 0000000..5164dde Binary files /dev/null and b/saved_images/spot_rgb45.png differ diff --git a/saved_images/spot_rgb46.png b/saved_images/spot_rgb46.png new file mode 100644 index 0000000..7c3dd7a Binary files /dev/null and b/saved_images/spot_rgb46.png differ diff --git a/saved_images/spot_rgb47.png b/saved_images/spot_rgb47.png new file mode 100644 index 0000000..0506917 Binary files /dev/null and b/saved_images/spot_rgb47.png differ diff --git a/saved_images/spot_rgb48.png b/saved_images/spot_rgb48.png new file mode 100644 index 0000000..bb06f6b Binary files /dev/null and b/saved_images/spot_rgb48.png differ diff --git a/saved_images/spot_rgb49.png b/saved_images/spot_rgb49.png new file mode 100644 index 0000000..d3a7a71 Binary files /dev/null and b/saved_images/spot_rgb49.png differ diff --git a/saved_images/spot_rgb5.png b/saved_images/spot_rgb5.png new file mode 100644 index 0000000..b95b8f7 Binary files /dev/null and b/saved_images/spot_rgb5.png differ diff --git a/saved_images/spot_rgb50.png b/saved_images/spot_rgb50.png new file mode 100644 index 0000000..73f036a Binary files /dev/null and b/saved_images/spot_rgb50.png differ diff --git a/saved_images/spot_rgb6.png b/saved_images/spot_rgb6.png new file mode 100644 index 0000000..0f7a5d6 Binary files /dev/null and b/saved_images/spot_rgb6.png differ diff --git a/saved_images/spot_rgb7.png b/saved_images/spot_rgb7.png new file mode 100644 index 0000000..d872f11 Binary files /dev/null and b/saved_images/spot_rgb7.png differ diff --git a/saved_images/spot_rgb8.png b/saved_images/spot_rgb8.png new file mode 100644 index 0000000..647401f Binary files /dev/null and b/saved_images/spot_rgb8.png differ diff --git a/saved_images/spot_rgb9.png b/saved_images/spot_rgb9.png new file mode 100644 index 0000000..b801620 Binary files /dev/null and b/saved_images/spot_rgb9.png differ diff --git a/src/spot-connection.cpp b/src/spot-connection.cpp index 6b2d20b..6de9fec 100644 --- a/src/spot-connection.cpp +++ b/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& 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= 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 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([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 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> 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. " diff --git a/src/spot-observer.cpp b/src/spot-observer.cpp index c0fed3c..7bbf57b 100644 --- a/src/spot-observer.cpp +++ b/src/spot-observer.cpp @@ -17,13 +17,16 @@ namespace SOb { // Global state +bool dummy = false; +int taken_from_dummy = 0; +bool going_up = true; // Function pointer for Unity logging callback LogCallback unityLogCallback = nullptr; bool logging_enabled = true; // Map to hold robot connections by ID -static int32_t __next_robot_id = 0; // Incremental ID for each robot connection +static int32_t __next_robot_id = 1; // Incremental ID for each robot connection (dummy is 0) static std::unordered_map> __robot_connections; // Keeping track of loaded models @@ -91,7 +94,33 @@ BOOL APIENTRY DllMain(HMODULE hModule, DWORD ul_reason_for_call, LPVOID lpReserv } static int32_t ConnectToSpot(const std::string& robot_ip, const std::string& username, const std::string& password) { + // AATASK: if dummy create one connection with robot id 0 + // AATASK: make neater try { + if (SOb::dummy) { + if (__robot_connections.find(0) == __robot_connections.end()) { + auto [it, inserted] = __robot_connections.try_emplace(0, std::make_unique(robot_ip, username, password)); + if (!inserted) { + LogMessage("SOb::ConnectToSpot: Failed to connect to robot {}", robot_ip); + return -1; + } + return 0; + // if (inserted) { + // bool success = it->second.connect(robot_ip, username, password); + // if (!success) { + // __robot_connections.erase(it); + // LogMessage("SOb::ConnectToSpot: Failed to connect to dummy connection"); + // return -1; + // } + // LogMessage("SOb::ConnectToSpot: Created dummy SpotConnection with ID 0"); + // return 0; + // } + } else { + LogMessage("SOb::ConnectToSpot: Dummy SpotConnection already exists with ID 0"); + return -1; + } + } + int32_t robot_id = __next_robot_id; __next_robot_id++; auto [it, inserted] = @@ -290,6 +319,16 @@ static void unloadModel(SObModel model) { extern "C" { +UNITY_INTERFACE_EXPORT +bool UNITY_INTERFACE_API getDummy() { + return SOb::dummy; +} + +UNITY_INTERFACE_EXPORT +void UNITY_INTERFACE_API setDummy(bool val) { + SOb::dummy = val; +} + void UNITY_INTERFACE_EXPORT UNITY_INTERFACE_API UnityPluginLoad(IUnityInterfaces* unity) { SOb::LogMessage("SpotObserver Plugin Load!"); try { @@ -312,6 +351,10 @@ void UNITY_INTERFACE_EXPORT UNITY_INTERFACE_API UnityPluginUnload() { UNITY_INTERFACE_EXPORT int32_t UNITY_INTERFACE_API SOb_ConnectToSpot(const char* robot_ip, const char* username, const char* password) { try { + if (SOb::dummy) { + return SOb::ConnectToSpot("dummy", "dummy", "dummy"); + } + SOb::SetDLLDirectory(); if (!robot_ip || !username || !password) { @@ -323,7 +366,7 @@ int32_t UNITY_INTERFACE_API SOb_ConnectToSpot(const char* robot_ip, const char* const std::string username_str = username; const std::string password_str = password; - return SOb::ConnectToSpot(robot_ip_str, username_str, password_str); + return SOb::ConnectToSpot(robot_ip_str, username_str, password_str); } catch (const std::exception& e) { SOb::LogMessage("SOb_ConnectToSpot: Exception while connecting to robot {}: {}", robot_ip ? robot_ip : "null", e.what()); return -1; // Return -1 on error @@ -379,6 +422,7 @@ bool UNITY_INTERFACE_API SOb_DestroyCameraStream(int32_t robot_id, int32_t cam_s return false; // Robot ID not found } + // AATASK: if dummy then still stream try { if (it->second->removeCamStream(cam_stream_id)) { SOb::LogMessage("SOb_DestroyCameraStream: Successfully destroyed camera stream {} for robot ID {}", cam_stream_id, robot_id); @@ -396,6 +440,7 @@ bool UNITY_INTERFACE_API SOb_DestroyCameraStream(int32_t robot_id, int32_t cam_s UNITY_INTERFACE_EXPORT SObModel UNITY_INTERFACE_API SOb_LoadModel(const char* modelPath, const char* backend) { + std::cout << "Using dummy:" << SOb::dummy << std::endl; if (!modelPath || !backend) { SOb::LogMessage("SOb_LoadModel: Invalid null pointer parameters"); return nullptr; @@ -430,12 +475,17 @@ UNITY_INTERFACE_EXPORT bool UNITY_INTERFACE_API SOb_LaunchVisionPipeline(int32_t robot_id, int32_t cam_stream_id, SObModel model) { using namespace SOb; try { + + LogMessage("SOb_LaunchVisionPipeline: Launching vision pipeline for robot ID {} on stream ID {}", robot_id, cam_stream_id); + auto robot_it = __robot_connections.find(robot_id); if (robot_it == __robot_connections.end()) { LogMessage("SOb_LaunchVisionPipeline: Robot ID {} not found", robot_id); return false; } + LogMessage("SOb_LaunchVisionPipeline: Launching vision pipeline for robot ID {} on stream ID {}", robot_id, cam_stream_id); + if (!robot_it->second->isConnected()) { LogMessage("SOb_LaunchVisionPipeline: Robot ID {} must be connected", robot_id); return false; @@ -446,6 +496,8 @@ bool UNITY_INTERFACE_API SOb_LaunchVisionPipeline(int32_t robot_id, int32_t cam_ return false; } + + // Cast the model to the proper type auto ml_model = reinterpret_cast(model); @@ -581,6 +633,7 @@ bool UNITY_INTERFACE_API SOb_GetNextVisionPipelineImageSet( uint8_t** images, float** depths ) { + // AATASK: if robot_id == -1, use dummy images try { if (!images || !depths) { SOb::LogMessage("SOb_GetNextVisionPipelineImageSet: Invalid null pointer parameters (images or depths)"); diff --git a/tests/integ-test/integ-test.cpp b/tests/integ-test/integ-test.cpp index d5f94d4..2169790 100644 --- a/tests/integ-test/integ-test.cpp +++ b/tests/integ-test/integ-test.cpp @@ -19,7 +19,9 @@ static int32_t connect_to_spot( const std::string& username, const std::string& password ) { - std::cout << "Connecting to Spot robot at " << robot_ip << " with user " << username << std::endl; + if (getDummy()) { + std::cout << "Using dummy connection" << std::endl; + } else std::cout << "Connecting to Spot robot at " << robot_ip << " with user " << username << std::endl; int32_t spot_id = SOb_ConnectToSpot(robot_ip.c_str(), username.c_str(), password.c_str()); if (spot_id < 0) { std::cerr << "Failed to connect to Spot robot" << std::endl; @@ -53,6 +55,8 @@ static int32_t disconnect_from_spots(const int32_t spot_ids[], size_t num_spots) return 0; } + + int main(int argc, char* argv[]) { using namespace std::chrono; @@ -71,7 +75,7 @@ int main(int argc, char* argv[]) { std::unordered_map> cam_stream_ids; - std::vector cam_bitmasks = {FRONTRIGHT | FRONTLEFT, HAND }; + std::vector cam_bitmasks = {FRONTRIGHT | FRONTLEFT, HAND }; // rm hand for vision? // FRONTRIGHT | FRONTLEFT for (size_t i = 0; i < 2; i++) { if (robot_ips[i] == "0") { spot_ids[i] = -1; @@ -83,7 +87,29 @@ int main(int argc, char* argv[]) { } } } - std::cout << "Connected to Spot robots with IDs: " << spot_ids[0] << ", " << spot_ids[1] << std::endl; + + setDummy(true); + int dummy_id; + // bool run_on_dummy_images = true; + for (size_t i = 0; i < 2; i++) { + if (spot_ids[i] >= 0) { + setDummy(false); + break; + } + } + if (!getDummy()) { + std::cout << "Connected to Spot robots with IDs: " << spot_ids[0] << ", " << spot_ids[1] << std::endl; + } + else { + std::cout << "No Spot robots connected so running on dummy images." << std::endl; + // run the connect function here anyway to create a dummy connection + dummy_id = connect_to_spot(robot_ips[0], username, password); + if (dummy_id < 0) { + std::cerr << "Failed to create dummy connection" << std::endl; + return -1; + } + // save first 10 images from each feed to disk + } for (size_t i = 0; i < 2; i++) { if (spot_ids[i] < 0) continue; @@ -98,6 +124,22 @@ int main(int argc, char* argv[]) { } } + if (getDummy()) { + for (uint32_t cam_bitmask : cam_bitmasks) { + if (cam_bitmask != (FRONTRIGHT | FRONTLEFT)) { + std::cout << "Skipping non-front cameras for dummy connection." << std::endl; + continue; + } + int32_t cam_stream_id = start_cam_stream(dummy_id, cam_bitmask); + + if (cam_stream_id < 0) { + SOb_DisconnectFromSpot(dummy_id); + return -1; + } + cam_stream_ids[dummy_id].push_back(cam_stream_id); + } + } + // TODO: Setup a listener for ctrl-c to gracefully stop the connection // std::cout << "Press Ctrl-C to stop reading camera feeds..." << std::endl; @@ -130,8 +172,22 @@ int main(int argc, char* argv[]) { } std::cout << "Vision pipeline launched on robot " << i << std::endl; } + + if (getDummy()) { + std::cout << "Launching vision pipeline on dummy connection." << std::endl; + bool ret = SOb_LaunchVisionPipeline(dummy_id, cam_stream_ids[dummy_id][0], model); + if (!ret) { + std::cerr << "Failed to launch vision pipeline on dummy connection" << std::endl; + cv::destroyAllWindows(); + SOb_DisconnectFromSpot(dummy_id); + SOb_UnloadModel(model); + return -1; + } + std::cout << "Vision pipeline launched on dummy connection" << std::endl; + } } + // Initialized output pointers std::vector num_images_requested_per_stream; std::vector images; @@ -148,6 +204,13 @@ int main(int argc, char* argv[]) { std::vector depth_cpu_buffer(640 * 480); bool new_images = false; + int writingimages[5][5]; // how many images to write to disk for dummy usage + // initialize every val in writingimages to 20 + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + writingimages[i][j] = 20; + } + } time_point start_time = high_resolution_clock::now(); bool exit_requested = false; while (!exit_requested) { @@ -163,10 +226,27 @@ int main(int argc, char* argv[]) { for (int32_t spot = 0; spot < 2; spot++) { int32_t spot_id = spot_ids[spot]; - if (spot_id < 0) { + if (getDummy()) { + spot_id = dummy_id; + } + if (spot_id < 0 && !getDummy()) { std::cout << "Skipping Spot " << spot << " as it is not connected." << std::endl; continue; } + if (getDummy() && spot == 1) { + continue; + } + + /* else if (getDummy() && spot == 0) { + if (using_vision_pipeline) { + if (!SOb_GetNextVisionPipelineImageSet(dummy_id, int32_t(num_images_requested), images, depths)) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + continue; + } + } + } */ + + for (int32_t stream = 0; stream < cam_stream_ids[spot_id].size(); stream++) { int32_t cam_stream_id = cam_stream_ids[spot_id][stream]; if (cam_stream_id < 0) { @@ -191,6 +271,7 @@ int main(int argc, char* argv[]) { } new_images = true; for (uint32_t i = 0; i < num_images_requested; i++) { + std::cout << "Reading Spot " << spot << " Stream " << stream << " Image " << i << std::endl; cudaMemcpyAsync( image_cpu_buffer.data(), images_set[i], @@ -213,6 +294,16 @@ int main(int argc, char* argv[]) { cv::imshow("SPOT " + std::to_string(spot) + " Stream " + std::to_string(stream) + " RGB" + std::to_string(i), image); cv::imshow("SPOT " + std::to_string(spot) + " Stream " + std::to_string(stream) + " Depth" + std::to_string(i), depth); + + // save images to disk for dummy usage with vision pipeline + // if (!using_vision_pipeline && !getDummy() && writingimages[stream][i] > 0) { + // std::string img_filename = std::format("..\\..\\saved_imagesv2\\spot_rgb_stream{}_index{}_image{}.png", stream, i, writingimages[stream][i]); + // std::string depth_filename = std::format("..\\..\\saved_imagesv2\\spot_depth_stream{}_index{}_image{}.png", stream, i, writingimages[stream][i]); + // cv::imwrite(img_filename, image); + // cv::imwrite(depth_filename, depth * 255); + // std::cout << "Wrote " << img_filename << " and " << depth_filename << std::endl; + // writingimages[stream][i]--; + // } } if (cv::waitKey(1) == 'q') { exit_requested = true;