VCINode.cpp

Go to the documentation of this file.

#include "VCINode.hpp"
 
#include <galaxis/node_ros_pubsub_wrapper.hpp>
 
#include <utils/logger.hpp>
 
#include <sensor_msgs/image_encodings.hpp>
#include <std_msgs/msg/header.hpp>
 
#include "utils/clock.hpp"
 
 
using namespace galaxis::ros;
using namespace galaxis::utils;
 
using namespace std;
 
using std::placeholders::_1;
using namespace std::chrono_literals;
 
VCINodeAsync::VCINodeAsync() :
    GalaxisNode("VCINodeAsync"), m_sharedMemory("/galaxis__camera") {
  m_laneImagePublisher = image_transport::create_publisher(this, galaxis::ros::TOPIC_CAMERA_RAW);
 
  // rclcpp::QoS qos_params = get_qos_profile(QOS_BEST_EFFORT);
  // m_imagePublisherNew = this->create_publisher<sensor_msgs::msg::Image>(galaxis::ros::TOPIC_CAMERA_RAW_BE, qos_params);
 
  int maxTries = 3;
  while(m_sharedMemory.allocate(m_imageSizeBytes) == false && maxTries > 0) {
    log_warning("Failed to allocate shared memory. Trying again (",maxTries,")");
    std::this_thread::sleep_for(1000ms);
    if(maxTries == 1) {
      std::exit(-1);
    }
    maxTries--;
  }
 
  maxTries = 60;
  while(m_camera.start_grabbing_detached() == false) {
    log_warning("Failed to initialize camera. Trying again (",maxTries,")");
    std::this_thread::sleep_for(1000ms);
    if(maxTries == 1) {
      std::exit(-1);
    }
    maxTries--;
  }
 
  std::thread(&VCINodeAsync::camera_publish_loop, this).detach();
}
 
void VCINodeAsync::camera_publish_loop() {
  com::SharedData data;
  
  while(1) {
    auto msg = m_camera.pop_image(true); // blocking
 
    if(msg == nullptr) {
      log_warning("[ASYNC-VCINODE] camera_publish_loop: Image is nullptr.");
      std::this_thread::sleep_for(1ms); // std::chrono_literals
      continue;
    }
 
    // write data via shared memory
    data.tv_sec   = msg->header.stamp.sec;
    data.tv_nsec  = msg->header.stamp.nanosec;
    data.data     = msg->data.data();
    data.dataSize = msg->data.size();
    m_sharedMemory.write_mutexed(data);
 
    // m_imagePublisherNew->publish(*msg);
    m_laneImagePublisher.publish(msg);
 
    log("[ASYNC-VCINODE] camera_publish_loop: Image published via Shared Memory and ROS2.");
    log_debug("[ASYNC-VCINODE] camera_publish_loop: Timestamp sec.nsec:", msg->header.stamp.sec, ".",msg->header.stamp.nanosec);
  }
}