Line data Source code
1 : #include "../../includes/inference/CameraStreamer.hpp" 2 : 3 : 4 : // Constructor: initializes camera capture, inference reference, and settings 5 0 : CameraStreamer::CameraStreamer(double scale) 6 0 : : scale_factor(scale), m_publisherFrameObject(nullptr), m_running(true) { 7 : 8 0 : segmentationInferencer = std::make_shared<TensorRTInferencer>("/home/jetson/models/lane-detection/model.engine"); 9 0 : yoloInferencer = std::make_shared<YOLOv5TRT>("/home/jetson/models/object-detection/yolov5m_updated.engine", "/home/jetson/models/object-detection/labels.txt"); 10 : 11 : // Define GStreamer pipeline for CSI camera 12 : std::string pipeline = "nvarguscamerasrc sensor-mode=4 ! " 13 : "video/x-raw(memory:NVMM), width=1280, height=720, " 14 : "format=(string)NV12, framerate=30/1 ! " 15 : "nvvidconv ! video/x-raw, format=(string)BGRx ! " 16 : "videoconvert ! video/x-raw, format=(string)BGR ! " 17 0 : "appsink drop=1 buffers=1"; 18 : 19 0 : std::cout << "[CameraStreamer] Using GStreamer pipeline: " << pipeline << std::endl; 20 : 21 0 : cap.open(pipeline, cv::CAP_GSTREAMER); // Open camera stream with GStreamer 22 : 23 0 : std::cout << "[CameraStreamer] Camera opened." << std::endl; 24 : 25 0 : if (!cap.isOpened()) { // Check if camera opened successfully 26 0 : std::cerr << "Error: Could not open CSI camera" << std::endl; 27 0 : exit(-1); // Terminate if failed 28 : } 29 0 : } 30 : 31 : // Destructor: clean up resources 32 0 : CameraStreamer::~CameraStreamer() { 33 0 : stop(); // Stop the camera stream 34 : 35 : // Join all threads safely 36 0 : if (captureThread.joinable()) captureThread.join(); 37 0 : if (segmentationThread.joinable()) segmentationThread.join(); 38 0 : if (detectionThread.joinable()) detectionThread.join(); 39 : 40 0 : if (cap.isOpened()) { 41 0 : cap.release(); // Release camera 42 : } 43 : 44 0 : cudaDeviceSynchronize(); // Ensure all CUDA operations are complete 45 : 46 0 : if (cuda_resource) { 47 0 : cudaGraphicsUnregisterResource(cuda_resource); // Unregister CUDA graphics resource 48 0 : cuda_resource = nullptr; 49 : } 50 : 51 0 : std::cout << "[~CameraStreamer] Destructor done." << std::endl; 52 0 : } 53 : 54 0 : void CameraStreamer::segmentationWorker() { 55 0 : while (m_running) { 56 0 : cv::Mat frame; 57 0 : if (segmentationBuffer.getFrame(frame)) { 58 : // auto start = std::chrono::high_resolution_clock::now(); 59 : 60 0 : segmentationInferencer->doInference(frame); 61 : 62 : // auto end = std::chrono::high_resolution_clock::now(); 63 : // auto duration_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count(); 64 : 65 : // std::cout << "[Segmentation] Inference time: " << duration_ms << " ms" << std::endl; 66 : } else { 67 0 : std::this_thread::sleep_for(std::chrono::milliseconds(1)); 68 : } 69 : } 70 0 : } 71 : 72 0 : void CameraStreamer::detectionWorker() { 73 0 : while (m_running) { 74 0 : cv::Mat frame; 75 0 : if (detectionBuffer.getFrame(frame)) { 76 0 : yoloInferencer->process_image(frame); 77 : } else { 78 0 : std::this_thread::sleep_for(std::chrono::milliseconds(1)); 79 : } 80 : } 81 0 : } 82 : 83 : // Main loop: capture, undistort, predict, visualize and render frames 84 0 : void CameraStreamer::start() { 85 0 : m_running = true; 86 : 87 0 : captureThread = std::thread(&CameraStreamer::captureLoop, this); 88 0 : segmentationThread = std::thread(&CameraStreamer::segmentationWorker, this); 89 0 : detectionThread = std::thread(&CameraStreamer::detectionWorker, this); 90 0 : } 91 : 92 0 : void CameraStreamer::captureLoop() { 93 0 : auto start_time = std::chrono::high_resolution_clock::now(); 94 0 : int frame_count = 0; 95 0 : const int framesToSkip = 1; // Skip frames to reduce processing load 96 0 : cv::Mat frame; 97 : 98 0 : while (m_running) { 99 0 : auto frame_start = std::chrono::high_resolution_clock::now(); 100 : 101 0 : for (int i = 0; i < framesToSkip; ++i) { 102 0 : cap.grab(); // Grab frames without decoding 103 : } 104 0 : cap >> frame; // Read one frame (decoded) 105 : 106 0 : if (frame.empty()) { 107 0 : std::cerr << "Empty frame, exiting" << std::endl; 108 0 : break; 109 : } 110 : 111 0 : segmentationBuffer.update(frame); 112 0 : detectionBuffer.update(frame); 113 : 114 0 : frame_count++; 115 0 : auto now = std::chrono::high_resolution_clock::now(); 116 0 : auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - start_time).count(); 117 : 118 0 : if (elapsed >= 1) { 119 0 : std::cout << "Average FPS: " << frame_count / static_cast<double>(elapsed) << std::endl; 120 0 : start_time = now; 121 0 : frame_count = 0; 122 : } 123 : } 124 0 : } 125 : 126 0 : void CameraStreamer::stop() { 127 0 : if (!m_running) return; 128 0 : m_running = false; 129 : 130 : // Wait for any CUDA operations to finish 131 : try { 132 0 : cudaDeviceSynchronize(); 133 0 : } catch (const std::exception& e) { 134 0 : std::cerr << "CUDA sync error in stop(): " << e.what() << std::endl; 135 : } 136 0 : std::this_thread::sleep_for(std::chrono::milliseconds(100)); 137 : 138 0 : std::cout << "[CameraStreamer] Shutdown complete." << std::endl; 139 : }