AsyncCamera.cpp

Go to the documentation of this file.

#include "AsyncCamera.hpp"
 
#include "utils/logger.hpp"
 
#include <chrono>
 
#include "utils/clock.hpp"
 
#include <cv_bridge/cv_bridge.h>
 
// OPENCV includes
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
 
using namespace std;
using namespace galaxis::utils;
 
using namespace AVT::VmbAPI;
using namespace cv;
 
CameraFrameObserver::CameraFrameObserver( AVT::VmbAPI::CameraPtr camera ) : AVT::VmbAPI::IFrameObserver( camera ) {
  m_currentImageSPtr = nullptr;
  
  m_cameraPtr = camera;
  m_cameraPtr->FlushQueue();
}
 
CameraFrameObserver::~CameraFrameObserver() {
  m_cameraPtr->FlushQueue();
}
 
 
void CameraFrameObserver::FrameReceived( const AVT::VmbAPI::FramePtr imgFrame ) {
  auto timestampCameraImage = m_rosClock.now();
  log("[CAMERA] [ALVIUM] Frame received!");
 
  VmbUint32_t nImageSize = 0; 
  if(VmbErrorSuccess != imgFrame->GetImageSize( nImageSize )) {
    log_warning("[CAMERA] [ALVIUM] GetImageSize");
    m_cameraPtr->QueueFrame(imgFrame);
    return;
  }
  VmbUint32_t nWidth = 0;
  if(VmbErrorSuccess != imgFrame->GetWidth( nWidth )) {
    log_warning("[CAMERA] [ALVIUM] GetImageSize");
    m_cameraPtr->QueueFrame(imgFrame);
    return;
  }
  VmbUint32_t nHeight = 0;
  if(VmbErrorSuccess != imgFrame->GetHeight( nHeight )) {
    log_warning("[CAMERA] [ALVIUM] GetImageSize");
    m_cameraPtr->QueueFrame(imgFrame);
    return;
  }
 
  // Get Image
  unsigned char* imagePtr;
  if(VmbErrorSuccess != imgFrame->GetImage( imagePtr )) {
    log_warning("[CAMERA] [ALVIUM] GetImage");
    m_cameraPtr->QueueFrame(imgFrame);
    return;
  }
 
  // COPY TO OPENCV (for resizing)
  // Resize Parameters cv::Size(Width, Height)
  auto outImgSize = cv::Size(512, 256);
 
  if(static_cast<float>(nHeight)/static_cast<float>(nWidth) != static_cast<float>(outImgSize.height)/static_cast<float>(outImgSize.width)) {
    log_warning("[CAMERA] [ALVIUM] Camera Image Ratio NOT equal to desired Image Ration");
  }
 
  cv::Mat img(nHeight, nWidth, CV_8UC3, imagePtr);
  cv::Mat imgResize(outImgSize, CV_8UC3);
  cv::resize(img, imgResize, outImgSize);
 
 
  // release camera img frame => not used afterwards
  m_cameraPtr->QueueFrame(imgFrame);
 
 
  // create image msg and bridge to sensor image msg
  std::shared_ptr<sensor_msgs::msg::Image> msgImgSPtr = std::make_shared<sensor_msgs::msg::Image>();
  std_msgs::msg::Header header;
  header.frame_id = "cam";
  header.stamp = timestampCameraImage;
 
  m_bridgeImg = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, imgResize);
  m_bridgeImg.toImageMsg(*msgImgSPtr); // cv_bridge to sensor_msgs::Image
 
 
  m_currentImageSPtrMutex.lock();
  m_currentImageSPtr = msgImgSPtr;
  m_newImageConditionVar.notify_one();
  m_currentImageSPtrMutex.unlock();
 
}
 
std::shared_ptr<sensor_msgs::msg::Image> CameraFrameObserver::PopLastImage(bool blocking) {
  std::unique_lock<std::mutex> lock(m_currentImageSPtrMutex);
  std::shared_ptr<sensor_msgs::msg::Image> ret = m_currentImageSPtr;
  m_currentImageSPtr = nullptr;
 
  // if return image is nullptr and blocking is enabled then wait for new image
  if(ret == nullptr && blocking) {
    m_newImageConditionVar.wait(lock); // release lock and wait until notify_one called
    ret = m_currentImageSPtr;
    m_currentImageSPtr = nullptr;
  }
 
  return ret;
}
 
 
 
AlviumAsyncCamera::AlviumAsyncCamera() : m_vimbaSystem(VimbaSystem::GetInstance()) {
  m_runGrabbing = false;
 
  if(VmbErrorSuccess != m_vimbaSystem.Startup()) {
    log_warning("[CAMERA] [ALVIUM] Vimba System failed to startup!");
  }
}
 
AlviumAsyncCamera::~AlviumAsyncCamera() {
  stop_grabbing_detached();
  m_vimbaSystem.Shutdown();
}
 
void AlviumAsyncCamera::stop_grabbing_detached() {
  if(this->m_runGrabbing) {
    m_camera->StopContinuousImageAcquisition();
    delete m_frameObserver;
    this->m_runGrabbing = false;
  }
}
 
std::shared_ptr<sensor_msgs::msg::Image> AlviumAsyncCamera::pop_image(bool blocking) {
  if(m_frameObserver != nullptr) {
    return m_frameObserver->PopLastImage(blocking);
  }
  else {
    log_warning("[CAMERA] [ALVIUM] Frame Observer not initialized!");
    return nullptr;
  }
}
 
bool AlviumAsyncCamera::start_grabbing_detached() {
 
  AVT::VmbAPI::CameraPtrVector cameraPtrVec;
  // Get all known cameras
  if ( VmbErrorSuccess != m_vimbaSystem.GetCameras( cameraPtrVec )){
    log_warning("[CAMERA] [ALVIUM] Failed to get cameras!");
    return false;
  }
 
  if(cameraPtrVec.size() < 1) {
    log_warning("[CAMERA] [ALVIUM] No camera found!");
    return false;
  }
 
  std::string cameraIdString;
  if(VmbErrorSuccess != cameraPtrVec[0]->GetID(cameraIdString)) {
    log_warning("[CAMERA] [ALVIUM] Failed to get the camera ID string.");
    return false;
  }
 
  // open camera
  if(VmbErrorSuccess != m_vimbaSystem.OpenCameraByID(cameraIdString.c_str(), VmbAccessModeFull, m_camera)) {
    log_warning("[CAMERA] [ALVIUM] Failed to open the camera.");
    return false;
  }
 
  // load settings
  if(VmbErrorSuccess != m_camera->LoadCameraSettings("AlviumCameraVimbaSettings.xml")) {
    log_warning("[CAMERA] [ALVIUM] Failed to load camera settings.");
    return false;
  }
 
  // // set to rgb8
  // AVT::VmbAPI::FeaturePtr formatFeature;
  // if(VmbErrorSuccess != m_camera->GetFeatureByName( "PixelFormat", formatFeature )) {
  //   log_warning("[CAMERA] [ALVIUM] Failed to get the pixel format feature.");
  // }
 
  // if(VmbErrorSuccess != formatFeature->SetValue(VmbPixelFormatRgb8)) {
  //   log_warning("[CAMERA] [ALVIUM] Failed to get the pixel format feature.");
  // }
 
  // // prepare for image transformation
  // if( VmbErrorSuccess != SetIntFeatureValueModulo2( m_camera, "Width" ) ){
  //   log_warning("[CAMERA] [ALVIUM] Failed to set int feature.");
  // }
  // if( VmbErrorSuccess != SetIntFeatureValueModulo2( m_camera, "Height" ) ){
  //   log_warning("[CAMERA] [ALVIUM] Failed to set int feature.");
  // }
 
  // create frame observer (This will be wrapped in a shared_ptr so we don't delete it)
  m_frameObserver = new CameraFrameObserver( m_camera );
 
  // start streaming
  if(VmbErrorSuccess != m_camera->StartContinuousImageAcquisition( 100/*number_of_frames*/, IFrameObserverPtr( m_frameObserver ))) {
    log_warning("[CAMERA] [ALVIUM] Failed to set start continuous image acquision.");
    return false;
  }
 
  this->m_runGrabbing = true;
  return true;
}