diff --git a/constants/camera_constants.json b/constants/camera_constants.json index 6305002c..10fa7694 100644 --- a/constants/camera_constants.json +++ b/constants/camera_constants.json @@ -17,10 +17,17 @@ "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" + "name": "dev_orin", + "serial_id": "devorin", + "backlight": null, + "frame_width": 1280, + "frame_height": 800, + "fps": 100.0, + "exposure": null, + "port": 5801, + "detector_type": "austin_gpu" }, { "name": "main_bot_front", @@ -101,4 +108,4 @@ "detector_type": "austin_gpu" } ] -} \ No newline at end of file +} 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" diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index 8159e96e..2919529d 100644 --- a/src/camera/CMakeLists.txt +++ b/src/camera/CMakeLists.txt @@ -1,3 +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) -target_sources(camera PRIVATE uvc_camera.cc) -target_link_libraries(camera PRIVATE utils absl::status uvc) +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 + /usr/lib/aarch64-linux-gnu/tegra/libnvjpeg.so +) diff --git a/src/camera/cscore_streamer.cc b/src/camera/cscore_streamer.cc index ec5a7d7d..3b2983c6 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,17 @@ 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); + std::cout << "WRITING FRAME"; + cv::Mat normalized = NormalizeForStreaming(mat); + if (normalized.empty()) { + return; } + + 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 bd391c71..145a47cc 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -1,22 +1,160 @@ #include "src/camera/uvc_camera.h" -#include -#include +#include "/usr/src/jetson_multimedia_api/include/NvBuffer.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"); +constexpr std::array kJpegStartMarker = {0xFF, 0xD8}; +constexpr std::array kJpegEndMarker = {0xFF, 0xD9}; + +// 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 {}; + } + + const unsigned char* full_frame_begin = data; + const unsigned char* full_frame_end = data + size; + + 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* 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]) { + end_of_image = current + 2; + break; + } + if (current == start_of_image) { + break; + } + } + + std::vector normalized(start_of_image, end_of_image); + if (normalized.size() < 2) { + return {}; + } + + return normalized; +} + +void FreeNvBuffer(NvBuffer* buff) { + if (buff != nullptr) { + delete buff; + } +} + +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; + } +} + +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); - ptr_->mutex_.lock(); + std::unique_lock lock(ptr_->mutex_); + 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; + std::vector jpeg = RemoveImagePackaging( + 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); + + 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; + FreeNvBuffer(decoded_frame_buffer); + return; + } + + if (ptr_->read_type == cv::IMREAD_COLOR) { + // 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]; + } + } + + // 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 = + cv::Mat(height, width, CV_8UC1, luminance.data, + luminance.fmt.stride * luminance.fmt.bytesperpixel) + .clone(); + } + FreeNvBuffer(decoded_frame_buffer); break; } case UVC_COLOR_FORMAT_YUYV: { @@ -24,16 +162,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; } @@ -51,12 +190,19 @@ 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(); } 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 (decoder_ == nullptr) { + status = absl::InternalError("Failed to create JPEG decoder for " + + camera_constant_.name); + return; + } if (!camera_constant.serial_id.has_value()) { status = absl::InvalidArgumentError(fmt::format( "Must provide a serial id for uvc camera {}", camera_constant.name)); @@ -90,8 +236,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 +248,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; } } @@ -112,9 +260,8 @@ 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()) { - backup_image_.copyTo(copied_timestamped_frame.frame); copied_timestamped_frame.invalid = true; copied_timestamped_frame.timestamp = frc::Timer::GetFPGATimestamp().to(); @@ -123,7 +270,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; } @@ -144,10 +290,17 @@ auto UVCCamera::Restart() -> void { } UVCCamera::~UVCCamera() { - uvc_stop_streaming(device_handle_); - uvc_close(device_handle_); - uvc_unref_device(device_); - uvc_exit(context_); + 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_; } auto UVCCamera::GetCameraConstant() const -> camera_constant_t { diff --git a/src/camera/uvc_camera.h b/src/camera/uvc_camera.h index 34dafe2a..cf2836b9 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" @@ -20,16 +21,16 @@ 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_; + 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_; - static constexpr cv::ImreadModes read_type = cv::IMREAD_GRAYSCALE; + int frame_index_ = 0; + int previous_frame_index_ = 0; + NvJPEGDecoder* decoder_ = nullptr; + 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 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)); diff --git a/src/unambiguous_first.cc b/src/unambiguous_first.cc index c1fad942..b09e9aeb 100644 --- a/src/unambiguous_first.cc +++ b/src/unambiguous_first.cc @@ -13,16 +13,18 @@ using camera::camera_constants_t; auto main() -> int { - utils::StartNetworktables(9971); + // utils::StartNetworktables(9971); + LOG(INFO) << "Init"; + write(STDERR_FILENO, "ENTER SelectCameraConfig\n", 26); - 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"; std::vector cameras = { - camera_constants.at("main_bot_left"), - camera_constants.at("main_bot_right"), + camera_constants.at("dev_orin"), }; LOG(INFO) << "Loaded constants"; 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";