Support multiple cam streams from a single spot #1

fmz · 2025-10-02T19:02:00Z

No description provided.

Copilot

Pull Request Overview

This PR introduces multi-camera stream support per Spot robot. It refactors the camera handling and vision pipeline to operate on per-stream identifiers, updates the C API and Unity interop to propagate stream IDs, and adapts the integration test accordingly.

Add SpotCamStream abstraction and refactor VisionPipeline to operate per camera stream
Extend C/Unity APIs to take cam_stream_id and maintain per-robot/per-stream output buffers
Update the integration test to create camera streams, launch pipelines per stream, and read image sets per stream

Reviewed Changes

Copilot reviewed 11 out of 11 changed files in this pull request and generated 12 comments.

Show a summary per file

File	Description
tests/integ-test/integ-test.cpp	Refactors connection/stream lifecycle, launches vision pipeline per stream, and reads images per cam_stream_id
src/vision-pipeline.cpp	Switches from SpotConnection to SpotCamStream and validates tensor shapes against stream-provided shapes
src/unity-cuda-interop.cpp	Adds per-stream texture registration and upload logic; updates function signatures to include cam_stream_id
src/spot-observer.cpp	Moves pipeline management into SpotConnection; adds per-stream APIs; updates image retrieval and pipeline control
src/spot-connection.cpp	Introduces SpotCamStream, creates/destroys streams and pipelines per stream; updates streaming logic and shape handling
src/include/vision-pipeline.h	Updates VisionPipeline to take SpotCamStream instead of SpotConnection
src/include/utils.h	Adds TensorShape with equality and string formatting support
src/include/unity-cuda-interop.h	Updates Unity interop function signatures to include cam_stream_id
src/include/spot-connection.h	Splits SpotConnection and SpotCamStream, adds per-stream lifecycle and pipeline APIs
src/include/model.h	Removes duplicate TensorShape (now in utils.h)
include/spot-observer.h	Updates extern "C" API to include cam_stream_id and wraps in C++ guards

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

Copilot · 2025-10-02T19:07:45Z

tests/integ-test/integ-test.cpp

            if (spot_ids[i] < 0) continue;
-            bool ret = SOb_LaunchVisionPipeline(spot_ids[i], model);
+            // Launch vision pipeline only on the first camera stream
+            bool ret = SOb_LaunchVisionPipeline(spot_ids[i], cam_stream_ids[i][0], model);


The map cam_stream_ids is keyed by spot_id, but here it's indexed by i (robot index). This can access the wrong vector or crash. Use cam_stream_ids[spot_ids[i]][0] instead.

Suggested change

bool ret = SOb_LaunchVisionPipeline(spot_ids[i], cam_stream_ids[i][0], model);

bool ret = SOb_LaunchVisionPipeline(spot_ids[i], cam_stream_ids[spot_ids[i]][0], model);

Copilot · 2025-10-02T19:07:45Z

tests/integ-test/integ-test.cpp

+
+    for (uint32_t cam_bitmask : cam_bitmasks) {
+        uint32_t num_bits_set = __num_set_bits(cam_bitmask);
+        num_images_requested_per_stream.push_back(__num_set_bits(num_bits_set));


num_bits_set is already the count of set bits. Calling __num_set_bits(num_bits_set) computes popcount on a count, producing incorrect values. Replace push_back(__num_set_bits(num_bits_set)) with push_back(num_bits_set).

Suggested change

num_images_requested_per_stream.push_back(__num_set_bits(num_bits_set));

num_images_requested_per_stream.push_back(num_bits_set);

Copilot · 2025-10-02T19:07:45Z

src/spot-observer.cpp

    }
-    if (n_images_requested <= 0 || n_images_requested > robot.getCurrentNumCams()) {
+
+    if (n_images_requested <= 0 || n_images_requested > stream->getCurrentCamMask()) {


Bounds check uses the bitmask (getCurrentCamMask) instead of the number of cameras. Compare against stream->getCurrentNumCams() to validate n_images_requested.

Suggested change

if (n_images_requested <= 0 || n_images_requested > stream->getCurrentCamMask()) {

if (n_images_requested <= 0 || n_images_requested > stream->getCurrentNumCams()) {

Copilot · 2025-10-02T19:07:46Z

src/spot-observer.cpp

+        return false;
+    }
+
+    VisionPipeline* pipeline = robot.getVisionPipeline(cam_stream_id);


Potential null dereference: if getVisionPipeline returns nullptr, calling pipeline->isRunning() will crash. Add a nullptr check before isRunning().

Suggested change

VisionPipeline* pipeline = robot.getVisionPipeline(cam_stream_id);

VisionPipeline* pipeline = robot.getVisionPipeline(cam_stream_id);

if (!pipeline) {

LogMessage("SOb_GetNextVisionPipelineImageSet: Vision pipeline not found for robot ID {} and cam_stream_id {}", robot_id, cam_stream_id);

return false;

}

Copilot · 2025-10-02T19:07:46Z

src/spot-observer.cpp

    if (it == SOb::__robot_connections.end()) {
-        SOb::LogMessage("SOb_ReadCameraFeeds: Robot ID {} not found", robot_id);
+        SOb::LogMessage("SOb_CreateCameraStream: Robot ID {} not found", robot_id);
        return false; // Robot ID not found


Function returns int32_t but returns false (0) on error, which can be misinterpreted as a valid stream id. Return -1 for the 'not found' case to match other error paths.

Suggested change

return false; // Robot ID not found

return -1; // Robot ID not found

Copilot · 2025-10-02T19:07:47Z

src/spot-connection.cpp

+        if (!it->second->streamCameras(cam_mask)) {
+            LogMessage("SpotConnection::createCamStream: Failed to start streaming cameras with mask {:#x}",
+                       cam_mask);
+            return false;
+        }


createCamStream returns int32_t but returns false (0) on error. Return -1 for error paths to avoid ambiguity with a valid stream id.

Copilot · 2025-10-02T19:07:47Z

src/spot-connection.cpp

+    cam_streams_.clear();
+    vision_pipelines_.clear();


Destructing cam_streams_ before vision_pipelines_ can leave VisionPipeline instances holding dangling references to SpotCamStream during stop()/destruction. Stop and clear vision_pipelines_ first, then clear cam_streams_.

Suggested change

cam_streams_.clear();

vision_pipelines_.clear();

vision_pipelines_.clear();

cam_streams_.clear();

Copilot · 2025-10-02T19:07:48Z

src/spot-connection.cpp

+    LogMessage("SpotConnection::connect: Created CUDA stream {:#x} and attached to buffer",
+               size_t(cuda_stream_));


These log messages are emitted from SpotCamStream, but reference SpotConnection, which can confuse diagnostics. Update the text to reference SpotCamStream.

Copilot · 2025-10-02T19:07:48Z

src/spot-connection.cpp

        checkCudaError(cudaStreamDestroy(cuda_stream_), "cudaStreamDestroy for SpotConnection");
        cuda_stream_ = nullptr;
-        LogMessage("Destroyed CUDA stream for SpotConnection");
+        LogMessage("SpotConnection::~SpotConnection: Destroyed CUDA stream");


These log messages are emitted from SpotCamStream, but reference SpotConnection, which can confuse diagnostics. Update the text to reference SpotCamStream.

Copilot · 2025-10-02T19:07:48Z

src/spot-observer.cpp

+
    } catch (const std::exception& e) {
-        LogMessage("SOb_StopVisionPipeline: Exception while stopping vision pipeline for robot ID {}: {}", robot_id, e.what());
+        LogMessage("SOb_StopVisionPipeline: Exception while launching vision pipeline for robot ID {} @ stream-ID: {}",


The log message mentions 'launching' in StopVisionPipeline. Replace 'launching' with 'stopping' to reflect the actual operation.

Suggested change

LogMessage("SOb_StopVisionPipeline: Exception while launching vision pipeline for robot ID {} @ stream-ID: {}",

LogMessage("SOb_StopVisionPipeline: Exception while stopping vision pipeline for robot ID {} @ stream-ID: {}",

Copilot

Pull Request Overview

Copilot reviewed 12 out of 12 changed files in this pull request and generated 11 comments.

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

Copilot · 2025-10-03T23:02:46Z

src/spot-connection.cpp

+int32_t SpotConnection::createCamStream(uint32_t cam_mask) {
+    if (!connected_) {
+        LogMessage("SpotConnection::createCamStream: Not connected to robot");
+        return false;
+    }


The function returns int32_t but uses 'return false;' (value 0) on failure; calling code/tests treat any value < 0 as failure, so a failure may be misinterpreted as success, and 0 could be a valid stream ID later. Return a negative error code (e.g. -1) consistently and ensure failed insertion/stream start cleans up the partially added entry.

Copilot · 2025-10-03T23:02:46Z

src/spot-connection.cpp

+        if (!it->second->streamCameras(cam_mask)) {
+            LogMessage("SpotConnection::createCamStream: Failed to start streaming cameras with mask {:#x}",
+                       cam_mask);
+            return false;
+        }


The function returns int32_t but uses 'return false;' (value 0) on failure; calling code/tests treat any value < 0 as failure, so a failure may be misinterpreted as success, and 0 could be a valid stream ID later. Return a negative error code (e.g. -1) consistently and ensure failed insertion/stream start cleans up the partially added entry.

Copilot · 2025-10-03T23:02:47Z

src/spot-connection.cpp

+        if (cam_streams_.find(cam_mask) != cam_streams_.end()) {
+            LogMessage("SpotConnection::createCamStream: Note that a camera stream with mask {:#x} already exists",
+                       cam_mask);


cam_streams_ is keyed by stream_id, not by cam_mask, so this lookup will almost never detect an existing stream for the same camera mask and provides a false sense of duplication checking. To detect duplicates, either maintain a separate map keyed by cam_mask or iterate cam_streams_ and compare each stream's current_cam_mask_.

Suggested change

if (cam_streams_.find(cam_mask) != cam_streams_.end()) {

LogMessage("SpotConnection::createCamStream: Note that a camera stream with mask {:#x} already exists",

cam_mask);

bool duplicate_found = false;

for (const auto& [id, stream] : cam_streams_) {

if (stream && stream->current_cam_mask_ == cam_mask) {

LogMessage("SpotConnection::createCamStream: Note that a camera stream with mask {:#x} already exists",

cam_mask);

duplicate_found = true;

break;

}

Copilot · 2025-10-03T23:02:47Z

src/spot-connection.cpp

+        if (!it->second->streamCameras(cam_mask)) {
+            LogMessage("SpotConnection::createCamStream: Failed to start streaming cameras with mask {:#x}",
+                       cam_mask);
+            return false;


On streaming failure the newly inserted (but non-functional) SpotCamStream remains in cam_streams_, leaving stale state. Erase the entry before returning (and return a negative error code) to avoid leaking an uninitialized stream object.

Suggested change

return false;

cam_streams_.erase(it);

return -1;

Copilot · 2025-10-03T23:02:47Z

src/include/spot-connection.h

+
+    bool connected_;
+
+    int32_t next_stream_id_{0xee1};


Magic number 0xee1 as the initial stream ID reduces clarity and may collide with caller assumptions; replace with 0 or a named constexpr (e.g. kInitialStreamId) with a comment explaining any non-zero start.

Suggested change

int32_t next_stream_id_{0xee1};

// Initial stream ID. If changed from 0, document the reason here.

static constexpr int32_t kInitialStreamId = 0xee1; // Non-zero start to avoid reserved IDs (document reason if needed)

int32_t next_stream_id_{kInitialStreamId};

Copilot · 2025-10-03T23:02:48Z

src/spot-connection.cpp

+            LogMessage("Copying RGB image of size {} bytes to write pointer at index {}, {:#x} rgb_write_ptr",
+                       image_size, write_idx, size_t(rgb_write_ptr));


Per-image verbose logging plus unconditional dumping (DumpRGBImageFromCuda) in the hot path will drastically reduce throughput and increase I/O load when debug dumps are enabled; wrap these in a conditional (e.g. debug/trace flag) or remove for release builds.

Copilot · 2025-10-03T23:02:48Z

src/spot-connection.cpp

+            DumpRGBImageFromCuda(
+                rgb_write_ptr,
+                cv_img.cols,
+                cv_img.rows,
+                cv_img.depth(),
+                "rgb",
+                n_rgbs_written + write_idx * n_images_per_response_
+            );


Per-image verbose logging plus unconditional dumping (DumpRGBImageFromCuda) in the hot path will drastically reduce throughput and increase I/O load when debug dumps are enabled; wrap these in a conditional (e.g. debug/trace flag) or remove for release builds.

Copilot · 2025-10-03T23:02:48Z

src/spot-connection.cpp

+            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));


Per-image verbose logging plus unconditional dumping (DumpRGBImageFromCuda) in the hot path will drastically reduce throughput and increase I/O load when debug dumps are enabled; wrap these in a conditional (e.g. debug/trace flag) or remove for release builds.

Copilot · 2025-10-03T23:02:49Z

tests/integ-test/integ-test.cpp

    std::string password  = argv[4];

-    //SOb_ToggleDebugDumps("./spot_dump");
+    SOb_ToggleDebugDumps("./spot_dump");


Enabling debug dumps unconditionally in the integration test will produce substantial disk I/O and may mask performance regressions; gate this behind a command-line flag or environment variable so normal test runs stay fast.

Copilot · 2025-10-03T23:02:49Z

src/unity-cuda-interop.cpp

    // Grab the latest image set using the provided function
    uint8_t* rgb_images[NUM_CAMERAS];
    float* depth_images[NUM_CAMERAS];
-    bool ret = getImageSetFunc(robot_id, num_textures, rgb_images, depth_images);
+    bool ret = getImageSetFunc(robot_id, cam_stream_id, num_textures, rgb_images, depth_images);


num_textures is size_t but implicitly converts to int32_t parameter; add an explicit static_cast<int32_t>(num_textures) and (optionally) a bounds check to prevent silent truncation if ever > INT32_MAX.

Copilot

Pull Request Overview

Copilot reviewed 13 out of 13 changed files in this pull request and generated 7 comments.

Comments suppressed due to low confidence (1)

src/spot-connection.cpp:1

The streaming thread is never started: _startStreamingThread() is not called in streamCameras, so after the initial probe request no further images will arrive. Call startStreamingThread() after successfully validating image sizes and before setting streaming = true (or immediately after setting it).

//

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

Copilot · 2025-10-04T20:37:21Z

tests/integ-test/integ-test.cpp

+                    cudaMemcpyAsync(
+                        image_cpu_buffer.data(),
+                        images_set[i],
+                        4 * 640 * 480,
+                        cudaMemcpyDeviceToHost
+                    );
+                    cudaMemcpyAsync(
+                        depth_cpu_buffer.data(),
+                        depths_set[i],
+                        640 * 480 * sizeof(float),
+                        cudaMemcpyDeviceToHost
+                    );
+                    cudaStreamSynchronize(0); // Wait for the copy to complete
+
+                    cv::Mat image(480, 640, CV_8UC4, image_cpu_buffer.data());
+                    cv::Mat depth(480, 640, CV_32FC1, depth_cpu_buffer.data());
+
+                    cv::cvtColor(image, image, cv::COLOR_RGBA2BGR);


Asynchronous cudaMemcpyAsync calls are issued on the default stream but the code immediately constructs cv::Mat and processes the data without synchronizing; this can lead to reading incomplete data. Insert a cudaStreamSynchronize(0) (or use a dedicated stream handle) after the copies and before using image_cpu_buffer/depth_cpu_buffer.

Copilot · 2025-10-04T20:37:21Z

tests/integ-test/integ-test.cpp

+static int32_t start_cam_stream(int32_t spot_id, uint32_t cam_bitmask) {
+    int32_t cam_stream_id = SOb_CreateCameraStream(spot_id, cam_bitmask);
+    if (cam_stream_id < 0) {
        std::cerr << "Failed to start reading camera feeds" << std::endl;
        SOb_DisconnectFromSpot(spot_id);
        return -1;
    }
+    std::cout << "Started camera stream with ID: " << cam_stream_id << std::endl;
+    return cam_stream_id;


On failure you disconnect the robot but the caller (main) retains the original spot_id and later calls disconnect_from_spots, causing a second disconnect attempt on an already-closed connection. Either (a) do not disconnect here and let the caller handle cleanup, or (b) update the caller to mark spot_ids[i] = -1 when start_cam_stream returns < 0.

Copilot · 2025-10-04T20:37:21Z

src/spot-connection.cpp

-bool SpotConnection::streamCameras(uint32_t cam_mask) {
-    if (!connected_) {
-        LogMessage("SpotConnection::streamCameras: Not connected to robot");
+bool SpotCamStream::streamCameras(uint32_t cam_mask) {


The streaming thread is never started: _startStreamingThread() is not called in streamCameras, so after the initial probe request no further images will arrive. Call startStreamingThread() after successfully validating image sizes and before setting streaming = true (or immediately after setting it).

Copilot · 2025-10-04T20:37:21Z

src/spot-connection.cpp

            DumpDepthImageFromCuda(
                depth_write_ptr,
                cv_img.cols,
                cv_img.rows,
-                "post-depth-cache",
+                "depth",
                n_depths_written + write_idx * n_images_per_response_
            );


Per-image logging plus unconditional DumpRGBImageFromCuda / DumpDepthImageFromCuda inside the hot push loop will significantly degrade throughput and generate large I/O. Gate these calls behind a debug flag or remove them for production paths.

Copilot · 2025-10-04T20:37:22Z

tests/integ-test-dx12/integ-test-dx12.cpp

+                    ThrowIfFailed(robot_resources[spot_id][stream][i].image_texture->Map(
+                        0, &image_read_range, reinterpret_cast<void**>(&image_data)
+                    ), "Map image readback");
+
+                    ThrowIfFailed(robot_resources[spot_id][stream][i].depth_texture->Map(
+                        0, &depth_read_range, reinterpret_cast<void**>(&depth_data)
+                    ), "Map depth readback");


[nitpick] Mapping and unmapping each resource every frame per image can be expensive; if these are readback buffers whose contents you fully overwrite, consider persistent mapping (Map once at creation and reuse pointers) to reduce CPU overhead.

Copilot · 2025-10-04T20:37:22Z

tests/integ-test-dx12/integ-test-dx12.cpp

+                    robot_resources[spot_id][stream][i].image_texture->Unmap(0, &no_write);
+                    robot_resources[spot_id][stream][i].depth_texture->Unmap(0, &no_write);


[nitpick] Mapping and unmapping each resource every frame per image can be expensive; if these are readback buffers whose contents you fully overwrite, consider persistent mapping (Map once at creation and reuse pointers) to reduce CPU overhead.

Copilot · 2025-10-04T20:37:22Z

src/unity-cuda-interop.cpp

+    s_OutputRGBTextures[robot_id][cam_stream_id].push_back(rgb_resource);
+    s_OutputDepthTextures[robot_id][cam_stream_id].push_back(depth_resource);


[nitpick] Using push_back without ensuring insertion order matches camera bit positions (cam_bit) can cause mismatches if registration order differs; consider indexing by the actual camera bit (e.g., position derived from least significant bit index) to make retrieval order deterministic.

fmz requested a review from Copilot October 2, 2025 19:02

Copilot AI reviewed Oct 2, 2025

View reviewed changes

fmz added 2 commits October 3, 2025 12:47

Support multiple cam streams from a single spot

0a48260

Minor fixes

6d222ba

fmz force-pushed the cam-streams branch from 343e0eb to 6d222ba Compare October 3, 2025 22:59

fmz requested a review from Copilot October 3, 2025 23:00

Copilot AI reviewed Oct 3, 2025

View reviewed changes

fmz added 3 commits October 4, 2025 16:24

integ-test works

abc6de6

integ-test-dx12 works (vibecoded)

0aa4c78

Reinstate the dumps

2f0316d

fmz requested a review from Copilot October 4, 2025 20:34

Copilot AI reviewed Oct 4, 2025

View reviewed changes

fmz added 2 commits October 10, 2025 14:43

various fixes

ad3fad1

Bug? fixes?

b265e41

	bool ret = SOb_LaunchVisionPipeline(spot_ids[i], cam_stream_ids[i][0], model);
	bool ret = SOb_LaunchVisionPipeline(spot_ids[i], cam_stream_ids[spot_ids[i]][0], model);

	num_images_requested_per_stream.push_back(__num_set_bits(num_bits_set));
	num_images_requested_per_stream.push_back(num_bits_set);

	if (n_images_requested <= 0 \|\| n_images_requested > stream->getCurrentCamMask()) {
	if (n_images_requested <= 0 \|\| n_images_requested > stream->getCurrentNumCams()) {

	return false; // Robot ID not found
	return -1; // Robot ID not found

		LogMessage("SpotConnection::connect: Created CUDA stream {:#x} and attached to buffer",
		size_t(cuda_stream_));

	LogMessage("SOb_StopVisionPipeline: Exception while launching vision pipeline for robot ID {} @ stream-ID: {}",
	LogMessage("SOb_StopVisionPipeline: Exception while stopping vision pipeline for robot ID {} @ stream-ID: {}",

-    int32_t next_stream_id_{0xee1};
+    // Initial stream ID. If changed from 0, document the reason here.
+    static constexpr int32_t kInitialStreamId = 0xee1; // Non-zero start to avoid reserved IDs (document reason if needed)
+    int32_t next_stream_id_{kInitialStreamId};

		LogMessage("Copying RGB image of size {} bytes to write pointer at index {}, {:#x} rgb_write_ptr",
		image_size, write_idx, size_t(rgb_write_ptr));

		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));

		robot_resources[spot_id][stream][i].image_texture->Unmap(0, &no_write);
		robot_resources[spot_id][stream][i].depth_texture->Unmap(0, &no_write);

		s_OutputRGBTextures[robot_id][cam_stream_id].push_back(rgb_resource);
		s_OutputDepthTextures[robot_id][cam_stream_id].push_back(depth_resource);

Support multiple cam streams from a single spot #1

Are you sure you want to change the base?

Support multiple cam streams from a single spot #1

Uh oh!

Conversation

fmz commented Oct 2, 2025

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 2, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Oct 3, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Uh oh!