434 lines
12 KiB
C++
434 lines
12 KiB
C++
#include "Camera.h"
|
|
#include "JPEG_XL.h"
|
|
#include "timing.h"
|
|
#include <iostream>
|
|
#include <mutex>
|
|
#include <filesystem>
|
|
#include <opencv2/opencv.hpp>
|
|
|
|
|
|
class FrameObserver : public IFrameObserver
|
|
{
|
|
public:
|
|
typedef std::function<void(int)> Callback;
|
|
Callback call;
|
|
int cam_id = 0;
|
|
std::atomic<int> settle = { 3 };
|
|
|
|
void registerCallback(Callback f) {
|
|
call = f;
|
|
}
|
|
|
|
FrameObserver(CameraPtr camera) : IFrameObserver(camera) {};
|
|
|
|
void FrameReceived(const FramePtr pFrame)
|
|
{
|
|
int old_val = settle.fetch_sub(1);
|
|
std::cout << "[FrameObserver cam=" << cam_id << "] FrameReceived settle=" << old_val << " → new=" << old_val - 1 << std::endl;
|
|
|
|
if (old_val > 0) {
|
|
std::cout << "[FrameObserver cam=" << cam_id << "] DUMP (settling)" << std::endl;
|
|
m_pCamera->QueueFrame(pFrame);
|
|
return;
|
|
}
|
|
|
|
bool bQueueDirectly = true;
|
|
VmbFrameStatusType eReceiveStatus;
|
|
|
|
if (VmbErrorSuccess == pFrame->GetReceiveStatus(eReceiveStatus))
|
|
{
|
|
/* ignore any incompletely frame */
|
|
if (VmbFrameStatusComplete != eReceiveStatus)
|
|
{
|
|
VmbUint64_t id;
|
|
std::string name;
|
|
std::string serial;
|
|
pFrame->GetFrameID(id);
|
|
m_pCamera->GetModel(name);
|
|
m_pCamera->GetSerialNumber(serial);
|
|
std::cout << "!!!incomplete frame, rcvd error: " << eReceiveStatus << " id: " << id << " camera: " << name << " + " << serial << std::endl;
|
|
m_pCamera->QueueFrame(pFrame);
|
|
return;
|
|
}
|
|
VmbUint64_t id;
|
|
pFrame->GetFrameID(id);
|
|
std::cout << "[FrameObserver cam=" << cam_id << "] rcvd frame ID=" << id << " → ENQUEUE" << std::endl;
|
|
// Lock the frame queue
|
|
m_FramesMutex.lock();
|
|
// Add frame to queue
|
|
m_Frames.push(pFrame);
|
|
// Unlock frame queue
|
|
m_FramesMutex.unlock();
|
|
// Emit the frame received signal
|
|
call(cam_id);
|
|
// callback!
|
|
bQueueDirectly = false;
|
|
}
|
|
else {
|
|
std::cout << "[FrameObserver cam=" << cam_id << "] frame rcvd error" << std::endl;
|
|
}
|
|
|
|
// If any error occurred we queue the frame without notification
|
|
if (true == bQueueDirectly)
|
|
{
|
|
m_pCamera->QueueFrame(pFrame);
|
|
}
|
|
};
|
|
|
|
FramePtr GetFrame() {
|
|
FramePtr res;
|
|
// Lock the frame queue
|
|
m_FramesMutex.lock();
|
|
// Pop frame from queue
|
|
if (!m_Frames.empty())
|
|
{
|
|
res = m_Frames.front();
|
|
m_Frames.pop();
|
|
}
|
|
// Unlock frame queue
|
|
m_FramesMutex.unlock();
|
|
return res;
|
|
}
|
|
|
|
void QueueF(FramePtr& frame) {
|
|
m_pCamera->QueueFrame(frame);
|
|
}
|
|
|
|
void ClearFrameQueue() {
|
|
// Lock the frame queue
|
|
m_FramesMutex.lock();
|
|
std::queue<FramePtr> empty;
|
|
std::swap(m_Frames, empty);
|
|
m_FramesMutex.unlock();
|
|
}
|
|
private:
|
|
std::queue<FramePtr> m_Frames;
|
|
std::mutex m_FramesMutex;
|
|
};
|
|
|
|
VimbaHandler::VimbaHandler(std::vector<std::string> cameraIds, MQTTClient* mqtt_c, motor_info* motor_i, bool demo) :
|
|
m_vmbSystem(VmbSystem::GetInstance()), mqtt_client(mqtt_c), gimbal_data(motor_i), demo_flag(demo)
|
|
{
|
|
VmbErrorType err = m_vmbSystem.Startup();
|
|
|
|
if (err != VmbErrorSuccess)
|
|
{
|
|
throw std::runtime_error("Could not start API, err=" + std::to_string(err));
|
|
}
|
|
|
|
//CameraPtrVector cameras;
|
|
//err = m_vmbSystem.GetCameras(cameras);
|
|
//if (err != VmbErrorSuccess)
|
|
//{
|
|
// m_vmbSystem.Shutdown();
|
|
// throw std::runtime_error("Could not get cameras, err=" + std::to_string(err));
|
|
//}
|
|
//if (cameras.empty())
|
|
//{
|
|
// m_vmbSystem.Shutdown();
|
|
// throw std::runtime_error("No cameras found.");
|
|
//}
|
|
|
|
if (cameraIds.size() > 0)
|
|
{
|
|
int id = 0;
|
|
|
|
for (std::string cameraId : cameraIds)
|
|
{
|
|
CameraPtr cam;
|
|
err = m_vmbSystem.GetCameraByID(cameraId.c_str(), cam);
|
|
if (err != VmbErrorSuccess)
|
|
{
|
|
m_vmbSystem.Shutdown();
|
|
throw std::runtime_error("No camera found with ID=" + std::string(cameraId) + ", err = " + std::to_string(err));
|
|
}
|
|
else {
|
|
m_cameras.push_back(cam);
|
|
}
|
|
}
|
|
}
|
|
|
|
//else
|
|
//{
|
|
// m_camera = cameras[0];
|
|
//}
|
|
for (CameraPtr camptr : m_cameras)
|
|
{
|
|
err = camptr->Open(VmbAccessModeFull);
|
|
if (err != VmbErrorSuccess)
|
|
{
|
|
m_vmbSystem.Shutdown();
|
|
throw std::runtime_error("Could not open camera, err=" + std::to_string(err));
|
|
}
|
|
else {
|
|
std::string name;
|
|
if (camptr->GetName(name) == VmbErrorSuccess)
|
|
{
|
|
std::cout << "Opened Camera " << name << std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
VimbaHandler::~VimbaHandler()
|
|
{
|
|
try
|
|
{
|
|
Stop();
|
|
}
|
|
catch (std::runtime_error& e)
|
|
{
|
|
std::cout << e.what() << std::endl;
|
|
}
|
|
|
|
m_vmbSystem.Shutdown();
|
|
}
|
|
|
|
void VimbaHandler::Open()
|
|
{
|
|
}
|
|
|
|
void VimbaHandler::Close()
|
|
{
|
|
}
|
|
|
|
void VimbaHandler::Start()
|
|
{
|
|
save_thread_running = true;
|
|
std::cout << "starting Camera thread" << std::endl;
|
|
image_saver_thread = std::thread(&VimbaHandler::SaveImage, this);
|
|
int cam_id = 0;
|
|
for (CameraPtr cam : m_cameras) {
|
|
IFrameObserverPtr FO_ptr;
|
|
ImageQueue8 image_queue;
|
|
m_ImageQueue_vec.push_back(image_queue);
|
|
SP_SET(FO_ptr, new FrameObserver(cam));
|
|
SP_DYN_CAST<FrameObserver>(FO_ptr)->cam_id = cam_id;
|
|
cam_id++;
|
|
VmbErrorType err = cam->StartContinuousImageAcquisition(5, FO_ptr);
|
|
FO_ptr_vec.push_back(FO_ptr);
|
|
if (err != VmbErrorSuccess)
|
|
{
|
|
throw std::runtime_error("Could not start acquisition, err=" + std::to_string(err));
|
|
}
|
|
else {
|
|
|
|
SP_DYN_CAST<FrameObserver>(FO_ptr)->registerCallback(std::bind(&VimbaHandler::EnqueueToStoreStruct, this, std::placeholders::_1));
|
|
}
|
|
}
|
|
cam_started = true;
|
|
}
|
|
|
|
void VimbaHandler::Stop()
|
|
{
|
|
for (CameraPtr cam : m_cameras) {
|
|
VmbErrorType err = cam->StopContinuousImageAcquisition();
|
|
if (err != VmbErrorSuccess)
|
|
{
|
|
throw std::runtime_error("Could not stop acquisition, err=" + std::to_string(err));
|
|
}
|
|
}
|
|
cam_started = false;
|
|
save_thread_running = false;
|
|
if (image_saver_thread.joinable()) {
|
|
image_saver_thread.join();
|
|
}
|
|
}
|
|
|
|
void VimbaHandler::evaluateCommand(std::string cmd, double val)
|
|
{
|
|
if (cmd == "fps" && val > 0)
|
|
ChangeFramerate(val);
|
|
else if (cmd == "jxlq" && val > 0)
|
|
jxlq = val;
|
|
else if (cmd == "jxle" && val > 0)
|
|
jxle = val;
|
|
else if (cmd == "display" && val >= 0)
|
|
display_image = int(val);
|
|
}
|
|
|
|
void VimbaHandler::EnqueueToStoreStruct(int cam_id)
|
|
{
|
|
FramePtr frame = SP_DYN_CAST<FrameObserver>(FO_ptr_vec[cam_id])->GetFrame();
|
|
|
|
VmbUint32_t Width;
|
|
VmbUint32_t Height;
|
|
VmbUint32_t BufferSize;
|
|
VmbPixelFormatType PixelFormat;
|
|
const VmbUchar_t* pBuffer(NULL);
|
|
|
|
if (VmbErrorSuccess == frame->GetPixelFormat(PixelFormat)
|
|
&& VmbErrorSuccess == frame->GetWidth(Width)
|
|
&& VmbErrorSuccess == frame->GetHeight(Height)
|
|
&& VmbErrorSuccess == frame->GetBufferSize(BufferSize)
|
|
&& VmbErrorSuccess == frame->GetBuffer(pBuffer))
|
|
{
|
|
NanoUnixTimer time;
|
|
long long tstamp = time.Stamp_longlong();
|
|
std::lock_guard<std::mutex> lg(queue_mut);
|
|
ImageStore8Ptr pFrame;
|
|
// in case we reached the maximum number of queued frames
|
|
// take of the oldest and reuse it to store the newly arriving frame
|
|
|
|
std::vector<VmbUchar_t> data_in = std::vector<VmbUchar_t>(pBuffer, pBuffer + BufferSize);
|
|
if (m_ImageQueue_vec[cam_id].size() >= 100)
|
|
{
|
|
pFrame = m_ImageQueue_vec[cam_id].front();
|
|
m_ImageQueue_vec[cam_id].pop();
|
|
if (!pFrame->equal(Width, Height, PixelFormat))
|
|
{
|
|
pFrame.reset();
|
|
}
|
|
}
|
|
if (pFrame == NULL)
|
|
{
|
|
pFrame = ImageStore8Ptr(new image_store_8bit(data_in.data(), data_in.size(), Width, Height, PixelFormat, tstamp, cam_id));
|
|
}
|
|
else
|
|
{
|
|
pFrame->setData(data_in.data(), data_in.size(), tstamp);
|
|
}
|
|
m_ImageQueue_vec[cam_id].push(pFrame);
|
|
|
|
SP_DYN_CAST<FrameObserver>(FO_ptr_vec[cam_id])->QueueF(frame);
|
|
std::cout << "Enqueue finished Camera:" << cam_id << " -- Queue size: " << m_ImageQueue_vec[cam_id].size() << std::endl;
|
|
if (cam_id == 0)
|
|
cv_proc.notify_one();
|
|
}
|
|
}
|
|
|
|
void VimbaHandler::SaveImage()
|
|
{
|
|
while (save_thread_running) {
|
|
std::unique_lock<std::mutex> lock(proc_wait_mut);
|
|
cv_proc.wait(lock);
|
|
ImageStore8Ptr pFrame;
|
|
for (ImageQueue8& imageQueue : m_ImageQueue_vec) {
|
|
while (imageQueue.size() > 0 && save_jxl)
|
|
{
|
|
Timer time;
|
|
queue_mut.lock();
|
|
pFrame = imageQueue.front();
|
|
imageQueue.pop();
|
|
queue_count_rgb = imageQueue.size();
|
|
queue_mut.unlock();
|
|
|
|
|
|
int imagetype;
|
|
int imagechannels;
|
|
if (pFrame->dataSize() / (pFrame->height() * pFrame->width()) == 1) {
|
|
imagetype = CV_8UC1;
|
|
imagechannels = 1;
|
|
}
|
|
else if (pFrame->dataSize() / (pFrame->height() * pFrame->width()) == 3) {
|
|
imagetype = CV_8UC3;
|
|
imagechannels = 3;
|
|
}
|
|
|
|
cv::Mat img_to_rotate = cv::Mat(pFrame->height(), pFrame->width(), imagetype, pFrame->data());
|
|
cv::Mat img;
|
|
cv::rotate(img_to_rotate, img, cv::ROTATE_90_COUNTERCLOCKWISE);
|
|
if (display_image) {
|
|
cv::namedWindow("Display Image", cv::WINDOW_NORMAL);
|
|
cv::resizeWindow("Display Image", img.cols / 4, img.rows / 4);
|
|
cv::imshow("Display Image", img);
|
|
cv::waitKey(10);
|
|
}
|
|
std::string cameraname;
|
|
if (pFrame->getCamId() == 0)
|
|
cameraname = "RGB";
|
|
if (pFrame->getCamId() == 1)
|
|
cameraname = "ACR";
|
|
if (pFrame->getCamId() == 2)
|
|
cameraname = "NIR";
|
|
std::string filename = output_dir + "/" + cameraname + "/" + std::to_string(pFrame->getTimestamp()) + ".jxl";
|
|
std::filesystem::path path(filename);
|
|
// Extract the directory part of the path
|
|
std::filesystem::path dir = path.parent_path();
|
|
// Create directories if they don't exist
|
|
if (!dir.empty() && !std::filesystem::exists(dir)) {
|
|
std::filesystem::create_directories(dir);
|
|
std::cout << "Created directories: " << dir << std::endl;
|
|
}
|
|
if (!demo_flag) {
|
|
JPEGXL jxl_writer(img.cols, img.rows, img.data, imagechannels, (float)jxlq, (int)jxle);
|
|
jxl_writer.WriteFile(filename.c_str());
|
|
std::cout << "Camera: " + std::to_string(pFrame->getCamId()) + " -- Compress TIME:" << std::to_string(time.ElapsedMillis()) << " -- SaveQueue size:" << imageQueue.size() << std::endl;
|
|
}
|
|
else {
|
|
std::filesystem::copy("test_smoke.jxl", filename);
|
|
}
|
|
time.Reset();
|
|
// NOTE: optional network upload of saved images to the ground
|
|
// station was removed here; it had hardcoded NFS/SMB paths. If
|
|
// reintroduced it should read its destination from config.
|
|
std::string payload = "{ \"fwt\":\"" + fwt_name + "\" ,\"cam\":\"" + cameraname + "\", \"hdg\":" + std::to_string((int)(gimbal_data->hdg * 10)) + ", \"time\":" + std::to_string(pFrame->getTimestamp()) + " }";
|
|
mqtt_client->publish(mqtt_RGB, payload);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool VimbaHandler::ChangeFramerate(double fr)
|
|
{
|
|
VmbErrorType result;
|
|
for (CameraPtr cam : m_cameras) {
|
|
FeaturePtr pFeature;
|
|
result = SP_ACCESS(cam)->GetFeatureByName("AcquisitionFrameRate", pFeature);
|
|
if (result == VmbErrorSuccess)
|
|
result = pFeature->SetValue(fr);
|
|
if (result == VmbErrorSuccess) {
|
|
std::cout << "camera fps changed: " << fr << std::endl;
|
|
}
|
|
}
|
|
if (result == VmbErrorSuccess) {
|
|
return true;
|
|
}
|
|
else {
|
|
std::cout << "camera fps change failed with error:" << result << std::endl;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void VimbaHandler::TriggerSettle()
|
|
{
|
|
for (auto& fo : FO_ptr_vec) {
|
|
SP_DYN_CAST<FrameObserver>(fo)->settle.store(3);
|
|
}
|
|
}
|
|
|
|
bool VimbaHandler::TriggerCamera()
|
|
{
|
|
TriggerSettle();
|
|
std::cout << "[TriggerCamera] settle reset to 3, firing 4 triggers (3 settle + 1 real)..." << std::endl;
|
|
|
|
FeaturePtr pFeature;
|
|
VmbErrorType result;
|
|
|
|
for (int i = 0; i < 4; i++) {
|
|
result = SP_ACCESS(m_cameras[0])->GetFeatureByName("TriggerSoftware", pFeature);
|
|
if (result == VmbErrorSuccess)
|
|
result = pFeature->RunCommand();
|
|
int sl = SP_DYN_CAST<FrameObserver>(FO_ptr_vec[0])->settle.load();
|
|
std::cout << "[TriggerCamera] trigger #" << (i+1) << " settle_before=" << sl << " result=" << result << std::endl;
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(400));
|
|
}
|
|
|
|
std::cout << "[TriggerCamera] done, 4th frame should be real image" << std::endl;
|
|
return (result == VmbErrorSuccess);
|
|
}
|
|
|
|
void VimbaHandler::SetTowerName(std::string name)
|
|
{
|
|
fwt_name = name;
|
|
mqtt_RGB = "GGS/FWT/" + fwt_name + "/CamEvent";
|
|
}
|
|
|
|
void VimbaHandler::SetOutputDir(std::string dir)
|
|
{
|
|
output_dir = std::move(dir);
|
|
}
|