From 994ab1279b2c5649570200d6b4e9b17f7f3c56e3 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Thu, 30 Apr 2026 13:44:35 +0000 Subject: [PATCH 01/18] Add better stream control errors --- src/camera/uvc_camera.cc | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index bd391c71..65c0d297 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -90,8 +90,9 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, camera_constant_.frame_width.value(), camera_constant_.frame_height.value(), camera_constant_.fps.value()); if (res != 0) { - status = absl::AbortedError("Unable to get stream format for camera: " + - camera_constant.name); + status = absl::AbortedError(fmt::format( + "Unable to get stream control for camera {} with error code {}", + camera_constant.name, res)); return; } uvc_print_stream_ctrl(&ctrl_, stderr); @@ -101,8 +102,9 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, camera_constant.max_frame_size.value_or(ctrl_.dwMaxVideoFrameSize); res = uvc_start_streaming(device_handle_, &ctrl_, callback, this, 0); if (res != 0) { - status = absl::AbortedError("Unable to start streaming for camera: " + - camera_constant.name); + status = absl::AbortedError( + fmt::format("Unable to start stream for camera {} with error code {}", + camera_constant.name, res)); return; } } From 9ef52cb4dbea5e469e45ee22dfc7bfb0386b2b97 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Thu, 30 Apr 2026 13:53:13 +0000 Subject: [PATCH 02/18] Fix logging --- src/camera/multi_camera_source.cc | 15 ++++++++++++++- src/camera/multi_camera_source.h | 1 + 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/camera/multi_camera_source.cc b/src/camera/multi_camera_source.cc index 841c23c4..1c37f80f 100644 --- a/src/camera/multi_camera_source.cc +++ b/src/camera/multi_camera_source.cc @@ -1,5 +1,6 @@ #include "multi_camera_source.h" #include "src/camera/camera.h" +#include "src/camera/write_frame.h" #include "src/utils/log.h" namespace camera { @@ -9,9 +10,18 @@ MultiCameraSource::MultiCameraSource( timestamped_frames_(cameras_.size()), use_all_frames_(use_all_frames) { camera_threads_.reserve(cameras_.size()); + std::string log_path = frc::DataLogManager::GetLogDir(); + LOG(INFO) << "Making overall log folder: " << log_path << " " + << std::filesystem::create_directory(log_path); for (size_t i = 0; i < cameras_.size(); i++) { - camera_threads_.emplace_back([this, i]() -> void { + camera_threads_.emplace_back([this, i, log_path]() -> void { + const std::string camera_log_dest = + fmt::format("{}/{}", log_path, cameras_[i]->GetCameraConstant().name); + LOG(INFO) << "Making camera log folder: " + << std::filesystem::create_directory(camera_log_dest); + int counter = 0; while (true) { + counter++; if (use_all_frames_) { mutex_.lock(); bool frames_used = frames_used_; @@ -27,6 +37,9 @@ MultiCameraSource::MultiCameraSource( timestamped_frames_[i] = timestamped_frame; frames_used_ = false; mutex_.unlock(); + if (counter % log_frequency == 0 && !timestamped_frame.frame.empty()) { + WriteFrame(camera_log_dest, timestamped_frame); + } } }); } diff --git a/src/camera/multi_camera_source.h b/src/camera/multi_camera_source.h index db665913..9839b3c7 100644 --- a/src/camera/multi_camera_source.h +++ b/src/camera/multi_camera_source.h @@ -24,6 +24,7 @@ class MultiCameraSource { std::mutex mutex_; const bool use_all_frames_; bool frames_used_ = true; // only for when use_all_frames_ is true + static constexpr int log_frequency = 50; }; } // namespace camera From f780491ea046b6a45c899318d66fd7990b0187e7 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Thu, 30 Apr 2026 19:35:17 +0000 Subject: [PATCH 03/18] Test remove front camera --- src/unambiguous_second.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/unambiguous_second.cc b/src/unambiguous_second.cc index 9ed04588..b95c2f07 100644 --- a/src/unambiguous_second.cc +++ b/src/unambiguous_second.cc @@ -21,7 +21,7 @@ auto main() -> int { std::vector cameras = { camera_constants.at("second_bot_left"), camera_constants.at("second_bot_right"), - camera_constants.at("second_bot_front"), + // camera_constants.at("second_bot_front"), }; LOG(INFO) << "Loaded constants"; From fb7c821c8f77c88b008f4d24da1d0bc72fa4cfd5 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Thu, 30 Apr 2026 21:12:09 +0000 Subject: [PATCH 04/18] Remove submodule update TODO add back but less chopped --- scripts/build.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/scripts/build.sh b/scripts/build.sh index 7c5446f9..a2ec5f13 100755 --- a/scripts/build.sh +++ b/scripts/build.sh @@ -26,5 +26,6 @@ if [ "$(pwd)" != "/bos" ]; then mkdir -p /bos sudo cp -r constants /bos fi +# git submodule update --init --progress --depth 1 cmake -DENABLE_CLANG_TIDY=OFF -DCMAKE_BUILD_TYPE=Release -B "$BUILD_DIR" -G Ninja . cmake --build "$BUILD_DIR" From 852ccaa934525878ae8467ecb3c2a57f7dfce774 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Fri, 1 May 2026 03:22:54 +0000 Subject: [PATCH 05/18] Should work but not tested on orin bc libs --- src/camera/CMakeLists.txt | 11 +++++-- src/camera/uvc_camera.cc | 65 ++++++++++++++++++++++++++++++++++++--- src/camera/uvc_camera.h | 2 ++ 3 files changed, 72 insertions(+), 6 deletions(-) diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index 8159e96e..bd9ecb4a 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -1,3 +1,10 @@ add_library(camera cscore_streamer.cc disk_camera.cc cv_camera.cc select_camera.cc camera_source.cc write_frame.cc camera_constants.cc multi_camera_source.cc) -target_sources(camera PRIVATE uvc_camera.cc) -target_link_libraries(camera PRIVATE utils absl::status uvc) +target_sources(camera PRIVATE + uvc_camera.cc +) +target_link_libraries(camera PRIVATE + utils + absl::status + uvc + nvjpeg +) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index 65c0d297..90498bb5 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -1,6 +1,8 @@ #include "src/camera/uvc_camera.h" #include #include +#include "/usr/src/jetson_multimedia_api/include/NvBuffer.h" +#include "/usr/src/jetson_multimedia_api/include/NvUtils.h" #include "absl/status/status.h" #include "src/utils/pch.h" @@ -14,9 +16,61 @@ void callback(uvc_frame_t* frame, void* ptr) { ptr_->mutex_.lock(); switch (frame->frame_format) { case UVC_COLOR_FORMAT_MJPEG: { - char* data = static_cast(frame->data); - std::vector buffer(data, data + frame->data_bytes); - ptr_->frame_buffer.frame = cv::imdecode(buffer, UVCCamera::read_type); + NvBuffer* decoded_frame_buffer = nullptr; + uint32_t pixfmt, width, height; + + int ret = ptr_->decoder_->decodeToBuffer( + &decoded_frame_buffer, reinterpret_cast(frame->data), + frame->data_bytes, &pixfmt, &width, &height); + + if (ret != 0 || decoded_frame_buffer == nullptr) { + LOG(WARNING) << "Decode failed for camera " + << ptr_->camera_constant_.name; + ptr_->mutex_.unlock(); + return; + } + + if (decoded_frame_buffer->map() != 0) { + LOG(WARNING) << "Failed to map NvBuffer for camera " + << ptr_->camera_constant_.name; + ptr_->mutex_.unlock(); + return; + } + + if (ptr_->read_type == cv::IMREAD_COLOR) { + cv::Mat yuv_mat(height * 3 / 2, width, CV_8UC1); + + for (uint32_t plane = 0; plane < decoded_frame_buffer->n_planes; + plane++) { + NvBuffer::NvBufferPlane& nv_plane = + decoded_frame_buffer->planes[plane]; + + ret = decoded_frame_buffer->map(); + if (ret != 0) { + LOG(WARNING) << "Failed to map NvBuffer plane " << plane + << " for camera " << ptr_->camera_constant_.name + << " with error code: " << ret; + ptr_->mutex_.unlock(); + return; + } + + uint8_t* src = nv_plane.data; + uint8_t* dst = yuv_mat.data + (plane == 0 ? 0 : width * height); + + for (uint32_t row = 0; row < nv_plane.fmt.height; row++) { + memcpy(dst + row * nv_plane.fmt.width, + src + row * nv_plane.fmt.bytesperpixel * nv_plane.fmt.stride, + nv_plane.fmt.width * nv_plane.fmt.bytesperpixel); + } + } + } else { + NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; + ptr_->frame_buffer.frame = + cv::Mat(height, width, CV_8UC1, luminance.data, + luminance.fmt.stride * luminance.fmt.bytesperpixel) + .clone(); + } + ptr_->mutex_.unlock(); break; } case UVC_COLOR_FORMAT_YUYV: { @@ -56,7 +110,10 @@ void callback(uvc_frame_t* frame, void* ptr) { UVCCamera::UVCCamera(const CameraConstant& camera_constant, absl::Status& status, std::optional log_path) - : camera_constant_(camera_constant), log_path_(std::move(log_path)) { + : camera_constant_(camera_constant), + log_path_(std::move(log_path)), + decoder_( + NvJPEGDecoder::createJPEGDecoder(camera_constant_.name.c_str())) { if (!camera_constant.serial_id.has_value()) { status = absl::InvalidArgumentError(fmt::format( "Must provide a serial id for uvc camera {}", camera_constant.name)); diff --git a/src/camera/uvc_camera.h b/src/camera/uvc_camera.h index 34dafe2a..f141b80a 100644 --- a/src/camera/uvc_camera.h +++ b/src/camera/uvc_camera.h @@ -1,5 +1,6 @@ #pragma once #include +#include "/usr/src/jetson_multimedia_api/include/NvJpegDecoder.h" #include "camera_constants.h" #include "libuvc/libuvc.h" #include "src/camera/camera.h" @@ -29,6 +30,7 @@ class UVCCamera : public ICamera { std::mutex mutex_; int frame_index_; int previous_frame_index_; + NvJPEGDecoder* decoder_; static constexpr cv::ImreadModes read_type = cv::IMREAD_GRAYSCALE; private: From 231f6fc2110386ec2cb29ce75fc110541d6eef54 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Fri, 1 May 2026 03:34:01 +0000 Subject: [PATCH 06/18] Functional fixes --- src/camera/uvc_camera.cc | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index 90498bb5..ddfbab47 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -45,15 +45,6 @@ void callback(uvc_frame_t* frame, void* ptr) { NvBuffer::NvBufferPlane& nv_plane = decoded_frame_buffer->planes[plane]; - ret = decoded_frame_buffer->map(); - if (ret != 0) { - LOG(WARNING) << "Failed to map NvBuffer plane " << plane - << " for camera " << ptr_->camera_constant_.name - << " with error code: " << ret; - ptr_->mutex_.unlock(); - return; - } - uint8_t* src = nv_plane.data; uint8_t* dst = yuv_mat.data + (plane == 0 ? 0 : width * height); @@ -63,6 +54,7 @@ void callback(uvc_frame_t* frame, void* ptr) { nv_plane.fmt.width * nv_plane.fmt.bytesperpixel); } } + ptr_->frame_buffer.frame = std::move(yuv_mat); } else { NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; ptr_->frame_buffer.frame = @@ -71,6 +63,7 @@ void callback(uvc_frame_t* frame, void* ptr) { .clone(); } ptr_->mutex_.unlock(); + decoded_frame_buffer->unmap(); break; } case UVC_COLOR_FORMAT_YUYV: { @@ -207,6 +200,7 @@ UVCCamera::~UVCCamera() { uvc_close(device_handle_); uvc_unref_device(device_); uvc_exit(context_); + delete decoder_; } auto UVCCamera::GetCameraConstant() const -> camera_constant_t { From 026b8805787fb656ce0cb83704ee6616325bc265 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 30 Apr 2026 20:50:25 -0700 Subject: [PATCH 07/18] Build but segfault :( --- constants/camera_constants.json | 10 +++++++++- src/camera/CMakeLists.txt | 14 +++++++++++++- src/unambiguous_first.cc | 5 ++--- 3 files changed, 24 insertions(+), 5 deletions(-) diff --git a/constants/camera_constants.json b/constants/camera_constants.json index b57514a3..f43baf96 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -20,7 +20,15 @@ "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", "intrinsics_path": "/bos/constants/misc/dev_orin_intrinsics.json", "extrinsics_path": "/bos/constants/misc/dev_orin_extrinsics.json", - "name": "dev_orin" + "name": "dev_orin", + "frame_width": 1280, + "frame_height": 800, + "fps": 100.0, + "serial_id": "devorin", + "max_payload_size": 1024, + "stream_ratio": 0.5, + "port": 5803, + "detector_type": "austin_gpu" }, { "name": "main_bot_front", diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index bd9ecb4a..2919529d 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -1,10 +1,22 @@ add_library(camera cscore_streamer.cc disk_camera.cc cv_camera.cc select_camera.cc camera_source.cc write_frame.cc camera_constants.cc multi_camera_source.cc) +set(JETSON_API /usr/src/jetson_multimedia_api) target_sources(camera PRIVATE uvc_camera.cc + ${JETSON_API}/samples/common/classes/NvJpegDecoder.cpp + ${JETSON_API}/samples/common/classes/NvBuffer.cpp + ${JETSON_API}/samples/common/classes/NvElement.cpp + ${JETSON_API}/samples/common/classes/NvElementProfiler.cpp + ${JETSON_API}/samples/common/classes/NvLogging.cpp +) + +target_include_directories(camera PRIVATE + ${JETSON_API}/include + ${JETSON_API}/include/libjpeg-8b + ${JETSON_API}/samples/common/classes ) target_link_libraries(camera PRIVATE utils absl::status uvc - nvjpeg + /usr/lib/aarch64-linux-gnu/tegra/libnvjpeg.so ) diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index 8b4b4425..eaa52283 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -11,7 +11,7 @@ using camera::camera_constants_t; auto main() -> int { - utils::StartNetworktables(9971); + // utils::StartNetworktables(9971); std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); @@ -19,8 +19,7 @@ auto main() -> int { LOG(INFO) << "Loading constants"; std::vector cameras = { - camera_constants.at("main_bot_left"), - camera_constants.at("main_bot_right"), + camera_constants.at("dev_orin"), }; LOG(INFO) << "Loaded constants"; From 19113cc46de80e6d31ba211bb1c81568fcbc86e7 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 30 Apr 2026 21:18:31 -0700 Subject: [PATCH 08/18] Changes but still has memory corruption --- src/camera/uvc_camera.cc | 21 ++++++++++++++++++++- src/unambiguous_first.cc | 4 +++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index ddfbab47..f0fb4dec 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -12,30 +12,37 @@ const cv::Mat UVCCamera::backup_image_ = cv::imread("/bos/constants/dont_worry_about_it.jpg"); void callback(uvc_frame_t* frame, void* ptr) { + LOG(INFO) << "in cb"; auto ptr_ = static_cast(ptr); ptr_->mutex_.lock(); switch (frame->frame_format) { case UVC_COLOR_FORMAT_MJPEG: { + LOG(INFO) << "1"; NvBuffer* decoded_frame_buffer = nullptr; uint32_t pixfmt, width, height; int ret = ptr_->decoder_->decodeToBuffer( &decoded_frame_buffer, reinterpret_cast(frame->data), frame->data_bytes, &pixfmt, &width, &height); + LOG(INFO) << "2"; if (ret != 0 || decoded_frame_buffer == nullptr) { LOG(WARNING) << "Decode failed for camera " << ptr_->camera_constant_.name; - ptr_->mutex_.unlock(); + decoded_frame_buffer->unmap(); return; } + LOG(INFO) << "3"; if (decoded_frame_buffer->map() != 0) { LOG(WARNING) << "Failed to map NvBuffer for camera " << ptr_->camera_constant_.name; ptr_->mutex_.unlock(); + decoded_frame_buffer->unmap(); + delete decoded_frame_buffer; return; } + LOG(INFO) << "4"; if (ptr_->read_type == cv::IMREAD_COLOR) { cv::Mat yuv_mat(height * 3 / 2, width, CV_8UC1); @@ -56,14 +63,18 @@ void callback(uvc_frame_t* frame, void* ptr) { } ptr_->frame_buffer.frame = std::move(yuv_mat); } else { + LOG(INFO) << "5"; NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; ptr_->frame_buffer.frame = cv::Mat(height, width, CV_8UC1, luminance.data, luminance.fmt.stride * luminance.fmt.bytesperpixel) .clone(); + LOG(INFO) << "6"; } + LOG(INFO) << "7"; ptr_->mutex_.unlock(); decoded_frame_buffer->unmap(); + delete decoded_frame_buffer; break; } case UVC_COLOR_FORMAT_YUYV: { @@ -99,6 +110,7 @@ void callback(uvc_frame_t* frame, void* ptr) { .to(); // TODO: Use more accurate timestamp ptr_->frame_index_ = frame->sequence; ptr_->mutex_.unlock(); + LOG(INFO) << "out cb"; } UVCCamera::UVCCamera(const CameraConstant& camera_constant, @@ -107,6 +119,13 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, log_path_(std::move(log_path)), decoder_( NvJPEGDecoder::createJPEGDecoder(camera_constant_.name.c_str())) { + LOG(INFO) << "In constructor"; + if (decoder_ == nullptr) { + status = absl::InternalError("Failed to create JPEG decoder for " + + camera_constant_.name); + return; + } + LOG(INFO) << "After decode"; if (!camera_constant.serial_id.has_value()) { status = absl::InvalidArgumentError(fmt::format( "Must provide a serial id for uvc camera {}", camera_constant.name)); diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index eaa52283..6a4dc57b 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -12,9 +12,11 @@ using camera::camera_constants_t; auto main() -> int { // utils::StartNetworktables(9971); + LOG(INFO) << "Init"; - std::string log_path = frc::DataLogManager::GetLogDir(); + // std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); + LOG(INFO) << "Cam constants"; LOG(INFO) << "Loading constants"; From 156bd6f236b54d57e9ba12c4ae195371730ddcb7 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 1 May 2026 03:52:18 -0700 Subject: [PATCH 09/18] Should have been fixed --- src/camera/uvc_camera.cc | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index f0fb4dec..f0f4866b 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -11,6 +11,13 @@ namespace camera { const cv::Mat UVCCamera::backup_image_ = cv::imread("/bos/constants/dont_worry_about_it.jpg"); +void FreeNvBuffer(NvBuffer* buff) { + if (buff != nullptr) { + buff->unmap(); + delete buff; + } +} + void callback(uvc_frame_t* frame, void* ptr) { LOG(INFO) << "in cb"; auto ptr_ = static_cast(ptr); @@ -28,18 +35,18 @@ void callback(uvc_frame_t* frame, void* ptr) { if (ret != 0 || decoded_frame_buffer == nullptr) { LOG(WARNING) << "Decode failed for camera " - << ptr_->camera_constant_.name; - decoded_frame_buffer->unmap(); + << ptr_->camera_constant_.name << " with code: " << ret; + ptr_->mutex_.unlock(); + FreeNvBuffer(decoded_frame_buffer); return; } LOG(INFO) << "3"; if (decoded_frame_buffer->map() != 0) { LOG(WARNING) << "Failed to map NvBuffer for camera " - << ptr_->camera_constant_.name; + << ptr_->camera_constant_.name << " with code: " << ret; ptr_->mutex_.unlock(); - decoded_frame_buffer->unmap(); - delete decoded_frame_buffer; + FreeNvBuffer(decoded_frame_buffer); return; } LOG(INFO) << "4"; @@ -73,8 +80,7 @@ void callback(uvc_frame_t* frame, void* ptr) { } LOG(INFO) << "7"; ptr_->mutex_.unlock(); - decoded_frame_buffer->unmap(); - delete decoded_frame_buffer; + FreeNvBuffer(decoded_frame_buffer); break; } case UVC_COLOR_FORMAT_YUYV: { From 2c1c21ed6edd88c2a859c5f083161f54120c63de Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 1 May 2026 12:35:22 -0700 Subject: [PATCH 10/18] No longer segfaults on static init --- src/camera/uvc_camera.cc | 7 +++---- src/camera/uvc_camera.h | 1 - src/unambiguous_first.cc | 1 + 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index f0f4866b..ad2262ad 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -8,8 +8,8 @@ namespace camera { -const cv::Mat UVCCamera::backup_image_ = - cv::imread("/bos/constants/dont_worry_about_it.jpg"); +// const cv::Mat UVCCamera::backup_image_ = +// cv::imread("/bos/constants/dont_worry_about_it.jpg"); void FreeNvBuffer(NvBuffer* buff) { if (buff != nullptr) { @@ -68,7 +68,7 @@ void callback(uvc_frame_t* frame, void* ptr) { nv_plane.fmt.width * nv_plane.fmt.bytesperpixel); } } - ptr_->frame_buffer.frame = std::move(yuv_mat); + ptr_->frame_buffer.frame = yuv_mat.clone(); } else { LOG(INFO) << "5"; NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; @@ -191,7 +191,6 @@ auto UVCCamera::GetFrame() -> timestamped_frame_t { } mutex_.lock(); if (frame_buffer.frame.empty()) { - backup_image_.copyTo(copied_timestamped_frame.frame); copied_timestamped_frame.invalid = true; copied_timestamped_frame.timestamp = frc::Timer::GetFPGATimestamp().to(); diff --git a/src/camera/uvc_camera.h b/src/camera/uvc_camera.h index f141b80a..a7e9ec87 100644 --- a/src/camera/uvc_camera.h +++ b/src/camera/uvc_camera.h @@ -21,7 +21,6 @@ class UVCCamera : public ICamera { public: const camera_constant_t camera_constant_; std::optional log_path_; - static const cv::Mat backup_image_; uvc_context_t* context_; uvc_device_t* device_; uvc_device_handle_t* device_handle_; diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index 6a4dc57b..7b760c66 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -13,6 +13,7 @@ using camera::camera_constants_t; auto main() -> int { // utils::StartNetworktables(9971); LOG(INFO) << "Init"; + write(STDERR_FILENO, "ENTER SelectCameraConfig\n", 26); // std::string log_path = frc::DataLogManager::GetLogDir(); camera_constants_t camera_constants = camera::GetCameraConstants(); From 5465bf64c549e8dd890e4caadebfe43ba0aac653 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 1 May 2026 15:10:16 -0700 Subject: [PATCH 11/18] Decode error --- constants/camera_constants.json | 11 +++++------ src/camera/uvc_camera.cc | 6 +++++- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/constants/camera_constants.json b/constants/camera_constants.json index e90bfdc6..10fa7694 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -17,17 +17,16 @@ "name": "mipi1" }, { - "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0", "intrinsics_path": "/bos/constants/misc/dev_orin_intrinsics.json", "extrinsics_path": "/bos/constants/misc/dev_orin_extrinsics.json", "name": "dev_orin", + "serial_id": "devorin", + "backlight": null, "frame_width": 1280, "frame_height": 800, "fps": 100.0, - "serial_id": "devorin", - "max_payload_size": 1024, - "stream_ratio": 0.5, - "port": 5803, + "exposure": null, + "port": 5801, "detector_type": "austin_gpu" }, { @@ -109,4 +108,4 @@ "detector_type": "austin_gpu" } ] -} \ No newline at end of file +} diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index ad2262ad..b8e3ef3e 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -42,7 +42,8 @@ void callback(uvc_frame_t* frame, void* ptr) { } LOG(INFO) << "3"; - if (decoded_frame_buffer->map() != 0) { + ret = decoded_frame_buffer->map(); + if (ret != 0) { LOG(WARNING) << "Failed to map NvBuffer for camera " << ptr_->camera_constant_.name << " with code: " << ret; ptr_->mutex_.unlock(); @@ -220,11 +221,14 @@ auto UVCCamera::Restart() -> void { } UVCCamera::~UVCCamera() { + LOG(INFO) << "Destructor"; uvc_stop_streaming(device_handle_); uvc_close(device_handle_); uvc_unref_device(device_); uvc_exit(context_); + LOG(INFO) << "Decoder deletion"; delete decoder_; + LOG(INFO) << "Deleted"; } auto UVCCamera::GetCameraConstant() const -> camera_constant_t { From 999e95462aa76bd4d7ec0ef1e9b853b031c6d9ab Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Fri, 1 May 2026 22:15:32 +0000 Subject: [PATCH 12/18] Codex attempt --- src/camera/uvc_camera.cc | 113 ++++++++++++++++++++++++++------------- src/camera/uvc_camera.h | 12 ++--- 2 files changed, 82 insertions(+), 43 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index b8e3ef3e..3c935d25 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -1,15 +1,55 @@ #include "src/camera/uvc_camera.h" -#include -#include #include "/usr/src/jetson_multimedia_api/include/NvBuffer.h" -#include "/usr/src/jetson_multimedia_api/include/NvUtils.h" #include "absl/status/status.h" #include "src/utils/pch.h" namespace camera { -// const cv::Mat UVCCamera::backup_image_ = -// cv::imread("/bos/constants/dont_worry_about_it.jpg"); +namespace { + +constexpr unsigned char kJpegStartMarker[2] = {0xFF, 0xD8}; +constexpr unsigned char kJpegEndMarker[2] = {0xFF, 0xD9}; + +auto NormalizeJpegBuffer(const unsigned char* data, size_t size) + -> std::vector { + if (data == nullptr || size < 2) { + return {}; + } + + const unsigned char* begin = data; + const unsigned char* end = data + size; + + const unsigned char* soi = + std::search(begin, end, std::begin(kJpegStartMarker), + std::end(kJpegStartMarker)); + if (soi == end) { + return {}; + } + + const unsigned char* eoi = end; + for (const unsigned char* current = end - 2; current >= soi; --current) { + if (current[0] == kJpegEndMarker[0] && current[1] == kJpegEndMarker[1]) { + eoi = current + 2; + break; + } + if (current == soi) { + break; + } + } + + std::vector normalized(soi, eoi == end ? end : eoi); + if (normalized.size() < 2) { + return {}; + } + + if (eoi == end && (normalized[normalized.size() - 2] != kJpegEndMarker[0] || + normalized[normalized.size() - 1] != kJpegEndMarker[1])) { + normalized.push_back(kJpegEndMarker[0]); + normalized.push_back(kJpegEndMarker[1]); + } + + return normalized; +} void FreeNvBuffer(NvBuffer* buff) { if (buff != nullptr) { @@ -18,39 +58,43 @@ void FreeNvBuffer(NvBuffer* buff) { } } +} // namespace + void callback(uvc_frame_t* frame, void* ptr) { - LOG(INFO) << "in cb"; auto ptr_ = static_cast(ptr); - ptr_->mutex_.lock(); + std::unique_lock lock(ptr_->mutex_); + switch (frame->frame_format) { case UVC_COLOR_FORMAT_MJPEG: { - LOG(INFO) << "1"; NvBuffer* decoded_frame_buffer = nullptr; uint32_t pixfmt, width, height; + std::vector jpeg = + NormalizeJpegBuffer(reinterpret_cast(frame->data), + frame->data_bytes); + if (jpeg.empty()) { + LOG(WARNING) << "Failed to normalize MJPEG frame for camera " + << ptr_->camera_constant_.name; + return; + } int ret = ptr_->decoder_->decodeToBuffer( - &decoded_frame_buffer, reinterpret_cast(frame->data), - frame->data_bytes, &pixfmt, &width, &height); - LOG(INFO) << "2"; + &decoded_frame_buffer, jpeg.data(), jpeg.size(), &pixfmt, &width, + &height); if (ret != 0 || decoded_frame_buffer == nullptr) { LOG(WARNING) << "Decode failed for camera " << ptr_->camera_constant_.name << " with code: " << ret; - ptr_->mutex_.unlock(); FreeNvBuffer(decoded_frame_buffer); return; } - LOG(INFO) << "3"; ret = decoded_frame_buffer->map(); if (ret != 0) { LOG(WARNING) << "Failed to map NvBuffer for camera " << ptr_->camera_constant_.name << " with code: " << ret; - ptr_->mutex_.unlock(); FreeNvBuffer(decoded_frame_buffer); return; } - LOG(INFO) << "4"; if (ptr_->read_type == cv::IMREAD_COLOR) { cv::Mat yuv_mat(height * 3 / 2, width, CV_8UC1); @@ -71,16 +115,12 @@ void callback(uvc_frame_t* frame, void* ptr) { } ptr_->frame_buffer.frame = yuv_mat.clone(); } else { - LOG(INFO) << "5"; NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; ptr_->frame_buffer.frame = cv::Mat(height, width, CV_8UC1, luminance.data, luminance.fmt.stride * luminance.fmt.bytesperpixel) .clone(); - LOG(INFO) << "6"; } - LOG(INFO) << "7"; - ptr_->mutex_.unlock(); FreeNvBuffer(decoded_frame_buffer); break; } @@ -89,16 +129,17 @@ void callback(uvc_frame_t* frame, void* ptr) { if (!bgr) { LOG(WARNING) << "Camera " << ptr_->camera_constant_.name << " failed to allocate "; + return; } uvc_error_t ret = uvc_yuyv2bgr(frame, bgr); if (ret != 0) { LOG(WARNING) << "YUYV failed to convert to BGR"; + uvc_free_frame(bgr); + return; } - IplImage* ipl_image; - ipl_image = - cvCreateImageHeader(cvSize(bgr->width, bgr->height), IPL_DEPTH_8U, 3); - cvSetData(ipl_image, bgr->data, bgr->width * 3); - ptr_->frame_buffer.frame = cv::cvarrToMat(ipl_image, true); + ptr_->frame_buffer.frame = + cv::Mat(bgr->height, bgr->width, CV_8UC3, bgr->data, bgr->width * 3) + .clone(); uvc_free_frame(bgr); break; } @@ -116,8 +157,6 @@ void callback(uvc_frame_t* frame, void* ptr) { frc::Timer::GetFPGATimestamp() .to(); // TODO: Use more accurate timestamp ptr_->frame_index_ = frame->sequence; - ptr_->mutex_.unlock(); - LOG(INFO) << "out cb"; } UVCCamera::UVCCamera(const CameraConstant& camera_constant, @@ -126,13 +165,11 @@ UVCCamera::UVCCamera(const CameraConstant& camera_constant, log_path_(std::move(log_path)), decoder_( NvJPEGDecoder::createJPEGDecoder(camera_constant_.name.c_str())) { - LOG(INFO) << "In constructor"; if (decoder_ == nullptr) { status = absl::InternalError("Failed to create JPEG decoder for " + camera_constant_.name); return; } - LOG(INFO) << "After decode"; if (!camera_constant.serial_id.has_value()) { status = absl::InvalidArgumentError(fmt::format( "Must provide a serial id for uvc camera {}", camera_constant.name)); @@ -190,7 +227,7 @@ auto UVCCamera::GetFrame() -> timestamped_frame_t { while (frame_index_ == previous_frame_index_) { std::this_thread::yield(); } - mutex_.lock(); + std::lock_guard lock(mutex_); if (frame_buffer.frame.empty()) { copied_timestamped_frame.invalid = true; copied_timestamped_frame.timestamp = @@ -200,7 +237,6 @@ auto UVCCamera::GetFrame() -> timestamped_frame_t { copied_timestamped_frame.invalid = frame_buffer.invalid; copied_timestamped_frame.timestamp = frame_buffer.timestamp; } - mutex_.unlock(); previous_frame_index_ = frame_index_; return copied_timestamped_frame; } @@ -221,14 +257,17 @@ auto UVCCamera::Restart() -> void { } UVCCamera::~UVCCamera() { - LOG(INFO) << "Destructor"; - uvc_stop_streaming(device_handle_); - uvc_close(device_handle_); - uvc_unref_device(device_); - uvc_exit(context_); - LOG(INFO) << "Decoder deletion"; + if (device_handle_ != nullptr) { + uvc_stop_streaming(device_handle_); + uvc_close(device_handle_); + } + if (device_ != nullptr) { + uvc_unref_device(device_); + } + if (context_ != nullptr) { + uvc_exit(context_); + } delete decoder_; - LOG(INFO) << "Deleted"; } auto UVCCamera::GetCameraConstant() const -> camera_constant_t { diff --git a/src/camera/uvc_camera.h b/src/camera/uvc_camera.h index a7e9ec87..808b658a 100644 --- a/src/camera/uvc_camera.h +++ b/src/camera/uvc_camera.h @@ -21,15 +21,15 @@ class UVCCamera : public ICamera { public: const camera_constant_t camera_constant_; std::optional log_path_; - uvc_context_t* context_; - uvc_device_t* device_; - uvc_device_handle_t* device_handle_; + uvc_context_t* context_ = nullptr; + uvc_device_t* device_ = nullptr; + uvc_device_handle_t* device_handle_ = nullptr; timestamped_frame_t frame_buffer; uvc_stream_ctrl_t ctrl_; std::mutex mutex_; - int frame_index_; - int previous_frame_index_; - NvJPEGDecoder* decoder_; + int frame_index_ = 0; + int previous_frame_index_ = 0; + NvJPEGDecoder* decoder_ = nullptr; static constexpr cv::ImreadModes read_type = cv::IMREAD_GRAYSCALE; private: From 9925bf01bf183346acddee95e147c836f165df05 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 1 May 2026 15:19:44 -0700 Subject: [PATCH 13/18] Fixed, map was unnecessary --- src/camera/uvc_camera.cc | 25 +++++++------------------ 1 file changed, 7 insertions(+), 18 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index 3c935d25..f41d71a7 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -19,9 +19,8 @@ auto NormalizeJpegBuffer(const unsigned char* data, size_t size) const unsigned char* begin = data; const unsigned char* end = data + size; - const unsigned char* soi = - std::search(begin, end, std::begin(kJpegStartMarker), - std::end(kJpegStartMarker)); + const unsigned char* soi = std::search( + begin, end, std::begin(kJpegStartMarker), std::end(kJpegStartMarker)); if (soi == end) { return {}; } @@ -53,7 +52,6 @@ auto NormalizeJpegBuffer(const unsigned char* data, size_t size) void FreeNvBuffer(NvBuffer* buff) { if (buff != nullptr) { - buff->unmap(); delete buff; } } @@ -68,18 +66,17 @@ void callback(uvc_frame_t* frame, void* ptr) { case UVC_COLOR_FORMAT_MJPEG: { NvBuffer* decoded_frame_buffer = nullptr; uint32_t pixfmt, width, height; - std::vector jpeg = - NormalizeJpegBuffer(reinterpret_cast(frame->data), - frame->data_bytes); + std::vector jpeg = NormalizeJpegBuffer( + reinterpret_cast(frame->data), frame->data_bytes); if (jpeg.empty()) { LOG(WARNING) << "Failed to normalize MJPEG frame for camera " << ptr_->camera_constant_.name; return; } - int ret = ptr_->decoder_->decodeToBuffer( - &decoded_frame_buffer, jpeg.data(), jpeg.size(), &pixfmt, &width, - &height); + int ret = + ptr_->decoder_->decodeToBuffer(&decoded_frame_buffer, jpeg.data(), + jpeg.size(), &pixfmt, &width, &height); if (ret != 0 || decoded_frame_buffer == nullptr) { LOG(WARNING) << "Decode failed for camera " @@ -88,14 +85,6 @@ void callback(uvc_frame_t* frame, void* ptr) { return; } - ret = decoded_frame_buffer->map(); - if (ret != 0) { - LOG(WARNING) << "Failed to map NvBuffer for camera " - << ptr_->camera_constant_.name << " with code: " << ret; - FreeNvBuffer(decoded_frame_buffer); - return; - } - if (ptr_->read_type == cv::IMREAD_COLOR) { cv::Mat yuv_mat(height * 3 / 2, width, CV_8UC1); From 2d36c6ad40b9ac6b598fc2110eac70769bcf7c17 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Fri, 1 May 2026 22:55:25 +0000 Subject: [PATCH 14/18] Start cleanup (untested) --- src/camera/multi_camera_source.cc | 15 +------------- src/camera/multi_camera_source.h | 1 - src/camera/uvc_camera.cc | 33 ++++++++++++++++++------------- 3 files changed, 20 insertions(+), 29 deletions(-) diff --git a/src/camera/multi_camera_source.cc b/src/camera/multi_camera_source.cc index 1c37f80f..841c23c4 100644 --- a/src/camera/multi_camera_source.cc +++ b/src/camera/multi_camera_source.cc @@ -1,6 +1,5 @@ #include "multi_camera_source.h" #include "src/camera/camera.h" -#include "src/camera/write_frame.h" #include "src/utils/log.h" namespace camera { @@ -10,18 +9,9 @@ MultiCameraSource::MultiCameraSource( timestamped_frames_(cameras_.size()), use_all_frames_(use_all_frames) { camera_threads_.reserve(cameras_.size()); - std::string log_path = frc::DataLogManager::GetLogDir(); - LOG(INFO) << "Making overall log folder: " << log_path << " " - << std::filesystem::create_directory(log_path); for (size_t i = 0; i < cameras_.size(); i++) { - camera_threads_.emplace_back([this, i, log_path]() -> void { - const std::string camera_log_dest = - fmt::format("{}/{}", log_path, cameras_[i]->GetCameraConstant().name); - LOG(INFO) << "Making camera log folder: " - << std::filesystem::create_directory(camera_log_dest); - int counter = 0; + camera_threads_.emplace_back([this, i]() -> void { while (true) { - counter++; if (use_all_frames_) { mutex_.lock(); bool frames_used = frames_used_; @@ -37,9 +27,6 @@ MultiCameraSource::MultiCameraSource( timestamped_frames_[i] = timestamped_frame; frames_used_ = false; mutex_.unlock(); - if (counter % log_frequency == 0 && !timestamped_frame.frame.empty()) { - WriteFrame(camera_log_dest, timestamped_frame); - } } }); } diff --git a/src/camera/multi_camera_source.h b/src/camera/multi_camera_source.h index 9839b3c7..db665913 100644 --- a/src/camera/multi_camera_source.h +++ b/src/camera/multi_camera_source.h @@ -24,7 +24,6 @@ class MultiCameraSource { std::mutex mutex_; const bool use_all_frames_; bool frames_used_ = true; // only for when use_all_frames_ is true - static constexpr int log_frequency = 50; }; } // namespace camera diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index f41d71a7..fa798652 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -7,8 +7,8 @@ namespace camera { namespace { -constexpr unsigned char kJpegStartMarker[2] = {0xFF, 0xD8}; -constexpr unsigned char kJpegEndMarker[2] = {0xFF, 0xD9}; +constexpr std::array kJpegStartMarker = {0xFF, 0xD8}; +constexpr std::array kJpegEndMarker = {0xFF, 0xD9}; auto NormalizeJpegBuffer(const unsigned char* data, size_t size) -> std::vector { @@ -16,33 +16,38 @@ auto NormalizeJpegBuffer(const unsigned char* data, size_t size) return {}; } - const unsigned char* begin = data; - const unsigned char* end = data + size; + const unsigned char* full_frame_begin = data; + const unsigned char* full_frame_end = data + size; - const unsigned char* soi = std::search( - begin, end, std::begin(kJpegStartMarker), std::end(kJpegStartMarker)); - if (soi == end) { + const unsigned char* start_of_image = + std::search(full_frame_begin, full_frame_end, + std::begin(kJpegStartMarker), std::end(kJpegStartMarker)); + if (start_of_image == full_frame_end) { return {}; } - const unsigned char* eoi = end; - for (const unsigned char* current = end - 2; current >= soi; --current) { + const unsigned char* end_of_image = full_frame_end; + for (const unsigned char* current = full_frame_end - 2; + current >= start_of_image; --current) { if (current[0] == kJpegEndMarker[0] && current[1] == kJpegEndMarker[1]) { - eoi = current + 2; + end_of_image = current + 2; break; } - if (current == soi) { + if (current == start_of_image) { break; } } - std::vector normalized(soi, eoi == end ? end : eoi); + std::vector normalized( + start_of_image, + end_of_image == full_frame_end ? full_frame_end : end_of_image); if (normalized.size() < 2) { return {}; } - if (eoi == end && (normalized[normalized.size() - 2] != kJpegEndMarker[0] || - normalized[normalized.size() - 1] != kJpegEndMarker[1])) { + if (end_of_image == full_frame_end && + (normalized[normalized.size() - 2] != kJpegEndMarker[0] || + normalized[normalized.size() - 1] != kJpegEndMarker[1])) { normalized.push_back(kJpegEndMarker[0]); normalized.push_back(kJpegEndMarker[1]); } From d063d8082af99b598aacc4173f89f944762f0627 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 1 May 2026 16:23:31 -0700 Subject: [PATCH 15/18] Cleanup tested --- src/camera/uvc_camera.cc | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index fa798652..1830af9d 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -10,7 +10,10 @@ namespace { constexpr std::array kJpegStartMarker = {0xFF, 0xD8}; constexpr std::array kJpegEndMarker = {0xFF, 0xD9}; -auto NormalizeJpegBuffer(const unsigned char* data, size_t size) +// Necessary because sometimes the size of the end sequence is variable, usually 5-7 bytes +// Example on home orin with usb camera: Img end was shifted0xffff60106574 vs 0xffff60106578 +// Doesn't seem to happen for image start but it is left in for safety +auto RemoveImagePackaging(const unsigned char* data, size_t size) -> std::vector { if (data == nullptr || size < 2) { return {}; @@ -38,20 +41,11 @@ auto NormalizeJpegBuffer(const unsigned char* data, size_t size) } } - std::vector normalized( - start_of_image, - end_of_image == full_frame_end ? full_frame_end : end_of_image); + std::vector normalized(start_of_image, end_of_image); if (normalized.size() < 2) { return {}; } - if (end_of_image == full_frame_end && - (normalized[normalized.size() - 2] != kJpegEndMarker[0] || - normalized[normalized.size() - 1] != kJpegEndMarker[1])) { - normalized.push_back(kJpegEndMarker[0]); - normalized.push_back(kJpegEndMarker[1]); - } - return normalized; } @@ -71,7 +65,7 @@ void callback(uvc_frame_t* frame, void* ptr) { case UVC_COLOR_FORMAT_MJPEG: { NvBuffer* decoded_frame_buffer = nullptr; uint32_t pixfmt, width, height; - std::vector jpeg = NormalizeJpegBuffer( + std::vector jpeg = RemoveImagePackaging( reinterpret_cast(frame->data), frame->data_bytes); if (jpeg.empty()) { LOG(WARNING) << "Failed to normalize MJPEG frame for camera " From 1c0b28b22a8c4f8367628286c7531cf30648a414 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 1 May 2026 16:43:55 -0700 Subject: [PATCH 16/18] Add yuyv/2-channel functionality --- src/camera/uvc_camera.cc | 91 ++++++++++++++++++----- src/camera/uvc_camera.h | 2 +- src/localization/gpu_apriltag_detector.cc | 34 ++++++--- src/localization/gpu_apriltag_detector.h | 2 +- 4 files changed, 100 insertions(+), 29 deletions(-) diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index 1830af9d..aa0fa40f 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -57,6 +57,78 @@ void FreeNvBuffer(NvBuffer* buff) { } // namespace +inline auto CopyPlaneRow(const NvBuffer::NvBufferPlane& plane, uint32_t row) + -> const uint8_t* { + return plane.data + row * plane.fmt.stride * plane.fmt.bytesperpixel; +} + +auto BuildYuyvMat(const NvBuffer& decoded_frame_buffer, uint32_t width, + uint32_t height) -> cv::Mat { + if (width % 2 != 0) { + return {}; + } + + cv::Mat yuyv(height, width, CV_8UC2); + const auto& y_plane = decoded_frame_buffer.planes[0]; + + if (decoded_frame_buffer.n_planes == 1 && y_plane.fmt.bytesperpixel == 2) { + for (uint32_t row = 0; row < height; ++row) { + memcpy(yuyv.ptr(row), CopyPlaneRow(y_plane, row), width * 2); + } + return yuyv; + } + + if (decoded_frame_buffer.n_planes < 2) { + return {}; + } + + const auto& u_or_uv_plane = decoded_frame_buffer.planes[1]; + const NvBuffer::NvBufferPlane* v_plane = decoded_frame_buffer.n_planes >= 3 + ? &decoded_frame_buffer.planes[2] + : nullptr; + + const uint32_t chroma_height = u_or_uv_plane.fmt.height; + const uint32_t chroma_width = u_or_uv_plane.fmt.width; + const bool has_separate_u_v_planes = v_plane != nullptr; + const bool chroma_is_horizontally_subsampled = chroma_width * 2 <= width; + const bool chroma_is_vertically_subsampled = chroma_height < height; + + for (uint32_t row = 0; row < height; ++row) { + const uint8_t* y_src = CopyPlaneRow(y_plane, row); + const uint32_t chroma_row = chroma_is_vertically_subsampled ? row / 2 : row; + const uint8_t* u_src = CopyPlaneRow(u_or_uv_plane, chroma_row); + const uint8_t* v_src = + has_separate_u_v_planes ? CopyPlaneRow(*v_plane, chroma_row) : nullptr; + uint8_t* dst = yuyv.ptr(row); + + for (uint32_t col = 0; col < width; col += 2) { + const uint32_t chroma_col = + chroma_is_horizontally_subsampled ? col / 2 : col; + const uint8_t y0 = y_src[col * y_plane.fmt.bytesperpixel]; + const uint8_t y1 = + y_src[(std::min(col + 1, width - 1)) * y_plane.fmt.bytesperpixel]; + + uint8_t u = 0; + uint8_t v = 0; + if (has_separate_u_v_planes) { + u = u_src[chroma_col * u_or_uv_plane.fmt.bytesperpixel]; + v = v_src[chroma_col * v_plane->fmt.bytesperpixel]; + } else { + const uint32_t uv_index = chroma_col * u_or_uv_plane.fmt.bytesperpixel; + u = u_src[uv_index]; + v = u_src[uv_index + 1]; + } + + dst[col * 2] = y0; + dst[col * 2 + 1] = u; + dst[col * 2 + 2] = y1; + dst[col * 2 + 3] = v; + } + } + + return yuyv; +} + void callback(uvc_frame_t* frame, void* ptr) { auto ptr_ = static_cast(ptr); std::unique_lock lock(ptr_->mutex_); @@ -85,23 +157,8 @@ void callback(uvc_frame_t* frame, void* ptr) { } if (ptr_->read_type == cv::IMREAD_COLOR) { - cv::Mat yuv_mat(height * 3 / 2, width, CV_8UC1); - - for (uint32_t plane = 0; plane < decoded_frame_buffer->n_planes; - plane++) { - NvBuffer::NvBufferPlane& nv_plane = - decoded_frame_buffer->planes[plane]; - - uint8_t* src = nv_plane.data; - uint8_t* dst = yuv_mat.data + (plane == 0 ? 0 : width * height); - - for (uint32_t row = 0; row < nv_plane.fmt.height; row++) { - memcpy(dst + row * nv_plane.fmt.width, - src + row * nv_plane.fmt.bytesperpixel * nv_plane.fmt.stride, - nv_plane.fmt.width * nv_plane.fmt.bytesperpixel); - } - } - ptr_->frame_buffer.frame = yuv_mat.clone(); + ptr_->frame_buffer.frame = + BuildYuyvMat(*decoded_frame_buffer, width, height); } else { NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; ptr_->frame_buffer.frame = diff --git a/src/camera/uvc_camera.h b/src/camera/uvc_camera.h index 808b658a..cf2836b9 100644 --- a/src/camera/uvc_camera.h +++ b/src/camera/uvc_camera.h @@ -30,7 +30,7 @@ class UVCCamera : public ICamera { int frame_index_ = 0; int previous_frame_index_ = 0; NvJPEGDecoder* decoder_ = nullptr; - static constexpr cv::ImreadModes read_type = cv::IMREAD_GRAYSCALE; + static constexpr cv::ImreadModes read_type = cv::IMREAD_COLOR; private: auto StartCamera(uvc_stream_ctrl_t ctrl) -> void; diff --git a/src/localization/gpu_apriltag_detector.cc b/src/localization/gpu_apriltag_detector.cc index 5902a025..07c0069b 100644 --- a/src/localization/gpu_apriltag_detector.cc +++ b/src/localization/gpu_apriltag_detector.cc @@ -53,16 +53,30 @@ auto GPUAprilTagDetector::GetTagDetections( CHECK(!timestamped_frame.frame.empty()); try { absl::Status detection_status; - if (image_format == vision::ImageFormat::MONO8) { - CHECK(timestamped_frame.frame.channels() == 1); - detection_status = - gpu_detector_->Detect(timestamped_frame.frame.data, nullptr); - } else { // TODO allow other formats than YUY2 - CHECK(timestamped_frame.frame.channels() == 3); - cv::Mat gray; - cv::cvtColor(timestamped_frame.frame, gray, cv::COLOR_BGR2YUV_YUY2); - cv::Mat mat_ = ToMat(gray); - detection_status = gpu_detector_->Detect(mat_.data, nullptr); + switch (image_format) { + case vision::ImageFormat::MONO8: { + CHECK(timestamped_frame.frame.channels() == 1); + detection_status = + gpu_detector_->Detect(timestamped_frame.frame.data, nullptr); + break; + } + case vision::ImageFormat::BGR8: { // TODO allow other formats than YUY2 + CHECK(timestamped_frame.frame.channels() == 3); + cv::Mat yuyv_mat; + cv::cvtColor(timestamped_frame.frame, yuyv_mat, cv::COLOR_BGR2YUV_YUY2); + cv::Mat mat_ = ToMat(yuyv_mat); + detection_status = gpu_detector_->Detect(mat_.data, nullptr); + break; + } + case vision::ImageFormat::YUYV422: { + LOG(INFO) << "Ch: " << timestamped_frame.frame.channels(); + CHECK(timestamped_frame.frame.channels() == 2); + detection_status = + gpu_detector_->Detect(timestamped_frame.frame.data, nullptr); + break; + } + default: + LOG(WARNING) << "Unsupported image format"; } if (!detection_status.ok()) { // LOG(WARNING) << "Gpu detector failed! Error: " diff --git a/src/localization/gpu_apriltag_detector.h b/src/localization/gpu_apriltag_detector.h index 088cbfa7..3cf26487 100644 --- a/src/localization/gpu_apriltag_detector.h +++ b/src/localization/gpu_apriltag_detector.h @@ -24,6 +24,6 @@ class GPUAprilTagDetector : public IAprilTagDetector { apriltag_detector_t* apriltag_detector_; std::unique_ptr gpu_detector_; static constexpr vision::ImageFormat image_format = - vision::ImageFormat::MONO8; + vision::ImageFormat::YUYV422; }; } // namespace localization From d508d60c13cb9c8111cca716bccf41aa739aa291 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 2 May 2026 06:56:20 -0700 Subject: [PATCH 17/18] Runs but crashes on streaming --- src/camera/cscore_streamer.cc | 40 +++++++++-- src/camera/uvc_camera.cc | 129 +++++++++++++++------------------- 2 files changed, 91 insertions(+), 78 deletions(-) diff --git a/src/camera/cscore_streamer.cc b/src/camera/cscore_streamer.cc index ec5a7d7d..db67a788 100644 --- a/src/camera/cscore_streamer.cc +++ b/src/camera/cscore_streamer.cc @@ -1,6 +1,36 @@ #include "cscore_streamer.h" namespace camera { +auto NormalizeForStreaming(const cv::Mat& mat) -> cv::Mat { + if (mat.empty()) { + return {}; + } + + switch (mat.channels()) { + case 1: { + cv::Mat bgr; + cv::cvtColor(mat, bgr, cv::COLOR_GRAY2BGR); + return bgr; + } + case 2: { + cv::Mat bgr; + cv::cvtColor(mat, bgr, cv::COLOR_YUV2BGR_YUY2); + return bgr; + } + case 3: + return mat; + case 4: { + cv::Mat bgr; + cv::cvtColor(mat, bgr, cv::COLOR_BGRA2BGR); + return bgr; + } + default: + std::cerr << "Unsupported frame channel count for CSCore: " + << mat.channels(); + return {}; + } +} + CscoreStreamer::CscoreStreamer(const std::string& name, uint port, uint fps, uint width, uint height, bool verbose) : width_(width), height_(height) { @@ -25,11 +55,13 @@ CscoreStreamer::CscoreStreamer(const std::string& name, uint port, uint fps, verbose) {} void CscoreStreamer::WriteFrame(const cv::Mat& mat) { - cv::Mat frame; - cv::resize(mat, frame, cv::Size(width_, height_)); - if (frame.channels() == 4) { - cv::cvtColor(frame, frame, cv::COLOR_BGRA2BGR); + cv::Mat normalized = NormalizeForStreaming(mat); + if (normalized.empty()) { + return; } + + cv::Mat frame; + cv::resize(normalized, frame, cv::Size(width_, height_)); source_.PutFrame(frame); } } // namespace camera diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index aa0fa40f..ead11b51 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -5,8 +5,6 @@ namespace camera { -namespace { - constexpr std::array kJpegStartMarker = {0xFF, 0xD8}; constexpr std::array kJpegEndMarker = {0xFF, 0xD9}; @@ -55,81 +53,29 @@ void FreeNvBuffer(NvBuffer* buff) { } } -} // namespace - -inline auto CopyPlaneRow(const NvBuffer::NvBufferPlane& plane, uint32_t row) - -> const uint8_t* { - return plane.data + row * plane.fmt.stride * plane.fmt.bytesperpixel; -} - -auto BuildYuyvMat(const NvBuffer& decoded_frame_buffer, uint32_t width, - uint32_t height) -> cv::Mat { - if (width % 2 != 0) { - return {}; - } - - cv::Mat yuyv(height, width, CV_8UC2); - const auto& y_plane = decoded_frame_buffer.planes[0]; - - if (decoded_frame_buffer.n_planes == 1 && y_plane.fmt.bytesperpixel == 2) { - for (uint32_t row = 0; row < height; ++row) { - memcpy(yuyv.ptr(row), CopyPlaneRow(y_plane, row), width * 2); - } - return yuyv; - } - - if (decoded_frame_buffer.n_planes < 2) { - return {}; - } - - const auto& u_or_uv_plane = decoded_frame_buffer.planes[1]; - const NvBuffer::NvBufferPlane* v_plane = decoded_frame_buffer.n_planes >= 3 - ? &decoded_frame_buffer.planes[2] - : nullptr; - - const uint32_t chroma_height = u_or_uv_plane.fmt.height; - const uint32_t chroma_width = u_or_uv_plane.fmt.width; - const bool has_separate_u_v_planes = v_plane != nullptr; - const bool chroma_is_horizontally_subsampled = chroma_width * 2 <= width; - const bool chroma_is_vertically_subsampled = chroma_height < height; - - for (uint32_t row = 0; row < height; ++row) { - const uint8_t* y_src = CopyPlaneRow(y_plane, row); - const uint32_t chroma_row = chroma_is_vertically_subsampled ? row / 2 : row; - const uint8_t* u_src = CopyPlaneRow(u_or_uv_plane, chroma_row); - const uint8_t* v_src = - has_separate_u_v_planes ? CopyPlaneRow(*v_plane, chroma_row) : nullptr; - uint8_t* dst = yuyv.ptr(row); - - for (uint32_t col = 0; col < width; col += 2) { - const uint32_t chroma_col = - chroma_is_horizontally_subsampled ? col / 2 : col; - const uint8_t y0 = y_src[col * y_plane.fmt.bytesperpixel]; - const uint8_t y1 = - y_src[(std::min(col + 1, width - 1)) * y_plane.fmt.bytesperpixel]; - - uint8_t u = 0; - uint8_t v = 0; - if (has_separate_u_v_planes) { - u = u_src[chroma_col * u_or_uv_plane.fmt.bytesperpixel]; - v = v_src[chroma_col * v_plane->fmt.bytesperpixel]; - } else { - const uint32_t uv_index = chroma_col * u_or_uv_plane.fmt.bytesperpixel; - u = u_src[uv_index]; - v = u_src[uv_index + 1]; - } - - dst[col * 2] = y0; - dst[col * 2 + 1] = u; - dst[col * 2 + 2] = y1; - dst[col * 2 + 3] = v; - } +void LogPlaneFormat(const NvBuffer* buffer) { + for (int i = 0; i < buffer->n_planes; i++) { + std::cout << buffer->planes[0].fmt.height << " " + << buffer->planes[0].fmt.width << " " + << buffer->planes[0].fmt.stride << " " + << buffer->planes[0].fmt.bytesperpixel << std::endl; } +} - return yuyv; +void LogPotentialBufferFormat(const NvBuffer* buffer, + NvBuffer::NvBufferPlaneFormat& format, + uint32_t raw_pixfmt, uint32_t width, + uint32_t height) { + uint32_t num_planes; + buffer->fill_buffer_plane_format(&num_planes, &format, width, height, + raw_pixfmt); + std::cout << format.height << " " << format.width << " " << format.stride + << " " << format.bytesperpixel << " with fmt: " << raw_pixfmt + << std::endl; } void callback(uvc_frame_t* frame, void* ptr) { + LOG(INFO) << "Hallo cb"; auto ptr_ = static_cast(ptr); std::unique_lock lock(ptr_->mutex_); @@ -149,6 +95,8 @@ void callback(uvc_frame_t* frame, void* ptr) { ptr_->decoder_->decodeToBuffer(&decoded_frame_buffer, jpeg.data(), jpeg.size(), &pixfmt, &width, &height); + std::cout << "Pixfmt: " << pixfmt << std::endl; + if (ret != 0 || decoded_frame_buffer == nullptr) { LOG(WARNING) << "Decode failed for camera " << ptr_->camera_constant_.name << " with code: " << ret; @@ -157,8 +105,41 @@ void callback(uvc_frame_t* frame, void* ptr) { } if (ptr_->read_type == cv::IMREAD_COLOR) { - ptr_->frame_buffer.frame = - BuildYuyvMat(*decoded_frame_buffer, width, height); + // TODO support a reasonable number of formats other than yuyv422M + if (pixfmt != V4L2_PIX_FMT_YUV422M) { + LOG(WARNING) << "Didn't receive multiplanar format, instead got " + << pixfmt; + std::cout << "Possible Formats: " << V4L2_PIX_FMT_NV12M << " " + << V4L2_PIX_FMT_YUV32 << " " << V4L2_PIX_FMT_YUV420 << " " + << V4L2_PIX_FMT_YUV420M << " " + << " " << V4L2_PIX_FMT_YUV422P << " " << std::endl; + return; + } + + const NvBuffer::NvBufferPlane& y_plane = + decoded_frame_buffer->planes[0]; + const NvBuffer::NvBufferPlane& u_plane = + decoded_frame_buffer->planes[1]; + const NvBuffer::NvBufferPlane& v_plane = + decoded_frame_buffer->planes[2]; + const uint32_t y_stride = + y_plane.fmt.stride * y_plane.fmt.bytesperpixel; + const uint32_t uv_stride = y_stride / 2; + cv::Mat yuyv422_interleaved(height, width, CV_8UC2); + for (size_t row = 0; row < height; row++) { + cv::Vec2b* row_ptr = yuyv422_interleaved.ptr(row); + uint8_t* y_ptr = y_plane.data + row * y_stride; + uint8_t* u_ptr = u_plane.data + row * uv_stride; + uint8_t* v_ptr = v_plane.data + row * uv_stride; + for (size_t col = 0; col < width; col += 2) { + row_ptr[col][0] = y_ptr[col]; + row_ptr[col][1] = u_ptr[col / 2]; + row_ptr[col + 1][0] = y_ptr[col + 1]; + row_ptr[col + 1][1] = v_ptr[col / 2]; + } + } + + ptr_->frame_buffer.frame = std::move(yuyv422_interleaved); } else { NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; ptr_->frame_buffer.frame = From 3f0839b920d15b0f249582310d4ef49db0b8261d Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 2 May 2026 07:37:30 -0700 Subject: [PATCH 18/18] debug --- src/camera/cscore_streamer.cc | 4 ++++ src/camera/uvc_camera.cc | 7 +++++++ src/test/integration_test/apriltag_detect_test.cc | 4 ++++ 3 files changed, 15 insertions(+) diff --git a/src/camera/cscore_streamer.cc b/src/camera/cscore_streamer.cc index db67a788..3b2983c6 100644 --- a/src/camera/cscore_streamer.cc +++ b/src/camera/cscore_streamer.cc @@ -55,6 +55,7 @@ CscoreStreamer::CscoreStreamer(const std::string& name, uint port, uint fps, verbose) {} void CscoreStreamer::WriteFrame(const cv::Mat& mat) { + std::cout << "WRITING FRAME"; cv::Mat normalized = NormalizeForStreaming(mat); if (normalized.empty()) { return; @@ -63,5 +64,8 @@ void CscoreStreamer::WriteFrame(const cv::Mat& mat) { cv::Mat frame; cv::resize(normalized, frame, cv::Size(width_, height_)); source_.PutFrame(frame); + std::cout << "WROTE FRAME"; + std::this_thread::sleep_for(std::chrono::duration(1)); + std::cout << "End sleep"; } } // namespace camera diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index ead11b51..145a47cc 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -139,7 +139,14 @@ void callback(uvc_frame_t* frame, void* ptr) { } } + // cv::Mat rgb; + // std::cout << "1" << std::endl; + // cv::cvtColor(yuyv422_interleaved, rgb, cv::COLOR_YUV2BGR_YUY2); + // std::cout << "2" << std::endl; + // cv::imwrite("frame" + std::to_string(frame->sequence) + ".jpg", rgb); + // std::cout << "3" << std::endl; ptr_->frame_buffer.frame = std::move(yuyv422_interleaved); + // std::cout << "4" << std::endl; } else { NvBuffer::NvBufferPlane& luminance = decoded_frame_buffer->planes[0]; ptr_->frame_buffer.frame = diff --git a/src/test/integration_test/apriltag_detect_test.cc b/src/test/integration_test/apriltag_detect_test.cc index a3e41839..24ffdc13 100644 --- a/src/test/integration_test/apriltag_detect_test.cc +++ b/src/test/integration_test/apriltag_detect_test.cc @@ -57,7 +57,11 @@ auto main(int argc, char* argv[]) -> int { LOG(INFO) << position_estimate; } + LOG(INFO) << "Pre conversion"; timestamped_frame.frame.copyTo(display_frame); + cv::cvtColor(display_frame, display_frame, cv::COLOR_YUV2BGR_YUY2); + LOG(INFO) << "Post conversion"; + for (auto& tag_detection : tag_detections) { for (auto& corner : tag_detection.corners) { cv::circle(display_frame, corner, 10, cv::Scalar(0, 0, 255));