Draft: Fix/coredump on meteor lake by RoboDragons

src/driver/spinnakerdriver.cpp

@@ -16,9 +16,20 @@
 #ifdef SPINNAKER
 
 #include "spinnakerdriver.h"
+#include <cstdlib> 
+#include <algorithm>
+#include <iostream>
+#include <unistd.h>
 
 #define CATCH_SPINNAKER(f) try { f; } catch (Spinnaker::Exception &e) { std::cerr << "[Spinnaker] Could not set parameter: " << e.GetFullErrorMessage() << std::endl; }
 
+constexpr int    kMinBufferCount = 3;      // Minimum buffer count required for NewestOnly mode
+constexpr size_t kUsb3PacketSize = 1024;   // USB3 packet size alignment requirement
+// constexpr size_t kPageSize       = 4096;   // Page size alignment requirement for Meteor Lake
+
+static size_t alignUp(size_t size, size_t alignment) {
+	return ((size + alignment - 1) / alignment) * alignment;
+}
 
 class SpinnakerImage : public RawImage {
 public:
@@ -114,23 +125,40 @@ SpinnakerDriver::SpinnakerDriver(const CameraConfig& config) {
 	}
 
 	pCam->TLStream.StreamBufferHandlingMode.SetValue(Spinnaker::StreamBufferHandlingMode_NewestOnly);
-	pCam->TLStream.StreamBufferCountManual.SetValue(pCam->TLStream.StreamBufferCountManual.GetMin());
+
+	// NewestOnly mode requires at least kMinBufferCount buffers
+	int requiredBuffers = std::max(kMinBufferCount, (int)pCam->TLStream.StreamBufferCountManual.GetMin());
+	pCam->TLStream.StreamBufferCountManual.SetValue(requiredBuffers);
 
 	// Provide image buffers to achieve faster mapping with OpenCL
-	int width = pCam->WidthMax.GetValue();
-	int height = pCam->HeightMax.GetValue();
-	for(int i = 0; i < pCam->TLStream.StreamBufferCountManual.GetMin(); i++) {
-		std::shared_ptr<RawImage> buffer = std::make_shared<RawImage>(&PixelFormat::RGGB8, width/2, height/2, "spinnaker");
-		buffers[buffer] = std::make_unique<CLMap<uint8_t>>(buffer->write<uint8_t>());
-	}
+	int width  = (int)pCam->Width.GetValue();
+	int height = (int)pCam->Height.GetValue();
+	size_t rawSize = (size_t)width * height;
+
+	// Align to both USB3 packet and Page size for Meteor Lake stability/decoding
+	finalBufferSize_ = alignUp(rawSize, kUsb3PacketSize);
 
 	std::vector<void*> bufferPtrs;
-	for (auto& item: buffers)
-		bufferPtrs.push_back(**item.second);
+	for(int i = 0; i < requiredBuffers; i++) {
+		void* alignedPtr = nullptr;
+		if (posix_memalign(&alignedPtr, kUsb3PacketSize, finalBufferSize_) != 0) {
+			throw std::runtime_error("[Spinnaker] posix_memalign failed");
+		}
+
+		// Track pointer for explicit free() in destructor
+		m_allocatedPtrs.push_back(alignedPtr);
+
+		auto buffer = std::make_shared<RawImage>(&PixelFormat::RGGB8, width/2, height/2, (unsigned char*)alignedPtr);
+
+		// Create OpenCL mapping
+		buffers[buffer] = std::make_unique<CLMap<uint8_t>>(buffer->write<uint8_t>());
+
+		bufferPtrs.push_back(alignedPtr);
+	}
 
+	// Register user-owned buffers with the camera
 	pCam->SetBufferOwnership(Spinnaker::SPINNAKER_BUFFER_OWNERSHIP_USER);
-	pCam->SetUserBuffers(bufferPtrs.data(), buffers.size(), width*height);
-	//pCam->Timestamp.SetValue();
+	pCam->SetUserBuffers(bufferPtrs.data(), bufferPtrs.size(), finalBufferSize_);
 
 	if (IsWritable(pCam->GevSCPSPacketSize)) {
 		CATCH_SPINNAKER(pCam->GevSCPSPacketSize.SetValue(9000));
@@ -152,22 +180,27 @@ double SpinnakerDriver::expectedFrametime() {
 }
 
 SpinnakerDriver::~SpinnakerDriver() {
-	pCam->EndAcquisition();
-}
+	if (pCam) {
+		pCam->EndAcquisition();
+		pCam->DeInit();
+	}
 
+	// Explicitly free memory to prevent leaks
+	for (void* ptr : m_allocatedPtrs) {
+		if (ptr) free(ptr);
+	}
+	m_allocatedPtrs.clear();
+}
 std::shared_ptr<RawImage> SpinnakerDriver::borrow(const Spinnaker::ImagePtr& pImage) {
 	void* data = pImage->GetData();
-	for (auto& item : buffers) {
-		if(item.second != nullptr && **item.second == data) {
-			item.second = nullptr;
-			return item.first;
-		}
+
+	auto it = m_fastBufferPool.find(data);
+	if (it != m_fastBufferPool.end() && it->second.clMap != nullptr) {
+		it->second.clMap = nullptr; // mark as in use
+		return it->second.image;
 	}
 
-	std::cerr << "[Spinnaker] Did not get image with given buffer, creating new buffer; expect OpenCL performance degradation" << std::endl;
-	std::shared_ptr<RawImage> image = std::make_shared<RawImage>(&PixelFormat::RGGB8, (int)pImage->GetWidth() / 2, (int)pImage->GetHeight() / 2, (unsigned char*)pImage->GetData());
-	buffers[image] = nullptr;
-	return image;
+	return std::make_shared<RawImage>(&PixelFormat::RGGB8, (int)pImage->GetWidth() / 2, (int)pImage->GetHeight() / 2, (unsigned char*)data);
 }
 
 void SpinnakerDriver::restore(const RawImage& image) {

src/driver/spinnakerdriver.h

@@ -19,25 +19,33 @@
 #include "cameradriver.h"
 #include "Spinnaker.h"
 
+// Map raw pointer addresses to corresponding {RawImage, CLMap} context
+struct BufferContext {
+    std::shared_ptr<RawImage> image;
+    std::unique_ptr<CLMap<uint8_t>> clMap;
+};
 class SpinnakerDriver : public CameraDriver {
-public:
-	explicit SpinnakerDriver(const CameraConfig& config);
-	~SpinnakerDriver() override;
+  public:
+  explicit SpinnakerDriver(const CameraConfig& config);
+  ~SpinnakerDriver() override;
 
-	std::shared_ptr<RawImage> readImage() override;
+  std::shared_ptr<RawImage> readImage() override;
 
-	const PixelFormat format() override;
+  const PixelFormat format() override;
 
-	double expectedFrametime() override;
+  double expectedFrametime() override;
 
-	std::shared_ptr<RawImage> borrow(const Spinnaker::ImagePtr& pImage);
-	void restore(const RawImage& image);
+  std::shared_ptr<RawImage> borrow(const Spinnaker::ImagePtr& pImage);
+  void restore(const RawImage& image);
 
-private:
-	Spinnaker::SystemPtr pSystem;
-	Spinnaker::CameraPtr pCam;
+  private:
+  Spinnaker::SystemPtr pSystem;
+  Spinnaker::CameraPtr pCam;
 
-	std::map<std::shared_ptr<RawImage>, std::unique_ptr<CLMap<uint8_t>>> buffers; // Use own image buffers for page size alignment (OpenCL pinned memory and zero copy)
+  std::map<std::shared_ptr<RawImage>, std::unique_ptr<CLMap<uint8_t>>> buffers; // Use own image buffers for page size alignment (OpenCL pinned memory and zero copy)
+  size_t finalBufferSize_ = 0; // Allocated buffer size per frame, used for buffer matching in borrow()
+  std::vector<void*> m_allocatedPtrs;
+  std::map<void*, BufferContext> m_fastBufferPool;
 };
 
-#endif
+#endif

src/main.cpp

@@ -421,6 +421,7 @@ int main(int argc, char* argv[]) {
 #else
 			detection->set_t_sent(r.camera->getTime());
 #endif
+			std::cout << wrapper.DebugString() << std::endl;
 			r.socket->send(wrapper);
 			r.socket->updateTime();
 			r.openCl->clearEvents();