QRCodeDetectionNode.cpp

Go to the documentation of this file.

#include "QRCodeDetectionNode.hpp"
 
#include <iostream>
#include <string>
#include <cmath>
#include <thread>
#include <cstdio>
#include <stdlib.h>
#include <unistd.h>
#include <experimental/filesystem>
#include <memory>
#include <chrono>
 
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/objdetect.hpp>
#include <opencv2/imgproc.hpp>
 
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/msg/header.hpp>
#include <sensor_msgs/image_encodings.hpp>
 
#include <galaxis/node_ros_pubsub_wrapper.hpp>
#include "utils/logger.hpp"
 
 
using namespace galaxis::utils;
 
//ros-eloquent-rosbag2-storage-default-plugins
namespace fs = std::experimental::filesystem;
 
QRCodeDetectionNode::QRCodeDetectionNode() : GalaxisNode("QRCodeDetection") {
  this->m_detector = cv::QRCodeDetector();
 
  galaxis::ros::create_publisher(this, m_qrCodeDetectionPubSPtr);
  
  auto qosReliable = galaxis::ros::get_qos_profile(galaxis::ros::QoS_Profile_t::QOS_RELIABLE);
  // rclcpp::QoS qosBestEffort = galaxis::ros::get_qos_profile(galaxis::ros::QoS_Profile_t::QOS_BEST_EFFORT);
  m_imgSubscriber = this->create_subscription<sensor_msgs::msg::Image>(
    galaxis::ros::TOPIC_CAMERA_RAW,
    qosReliable,
    std::bind(&QRCodeDetectionNode::onImgCallback, this, std::placeholders::_1));
  // m_debugImgPublisher = this->create_publisher<sensor_msgs::msg::Image>("/galaxis/qrcodedetection/debug/img", qosBestEffort);
 
  // std::string test_path = "./nodes/QRCodeDetectorNode/test_video.mp4";
  // std::thread(&QRCodeDetectionNode::test_detection_loop, this, test_path).detach(); 
}
 
void QRCodeDetectionNode::onImgCallback(const sensor_msgs::msg::Image::SharedPtr p_ImgMsg){
  cv::Mat boundingBox;
  cv::Mat image = cv_bridge::toCvShare(p_ImgMsg, "bgr8")->image;
 
  bool foundQR = this->m_detector.detect(image, boundingBox);
 
  // check if bounding box is a clean rectangle
  foundQR = foundQR && this->checkBoundingBox(boundingBox, image);
  if(foundQR){
    this->drawBox(image, boundingBox);
  }
 
  // Publish results
  // this->publishImage(image); // enable for debug
  if(foundQR && m_foundQRCount <= m_foundQRCountForSendingMessage * 2){
    m_foundQRCount++;
  }
  else if(!foundQR && m_foundQRCount >= 0) {
    m_foundQRCount--;
  }
  
  if(m_foundQRCount >= m_foundQRCountForSendingMessage) {
    log("Sending message: QR Code found.");
 
    m_qrCodeFoundSometime = true;
    this->publishQR(p_ImgMsg->header.stamp, true);
  }
  else {
    log("Sending message: QR Code NOT found.");
    this->publishQR(p_ImgMsg->header.stamp, false);
 
    if(m_qrCodeFoundSometime) {
      log("Node stopped.");
      std::this_thread::sleep_for(std::chrono::milliseconds(250));
      std::exit(0);
    }
  }
}
 
void QRCodeDetectionNode::test_detection_loop(std::string path){
  cv::VideoCapture cap = cv::VideoCapture(path);
  if(!cap.isOpened()){
    log_error("Error openning video stream or file");
  }
  cv::Mat frame, boundingBox;
  int frameCounter = 0;
 
  while(cap.isOpened()){
    bool ret = cap.read(frame);
    frameCounter++;
 
    // reset video to beginning if end has been reached
    if(frameCounter == cap.get(cv::CAP_PROP_FRAME_COUNT)){
      frameCounter = 0;
      cap.set(cv::CAP_PROP_POS_FRAMES, 0);
    }
    if(!ret){continue;}
 
    bool foundQR = m_detector.detect(frame, boundingBox);
    // check if bounding box is a clean rectangle
    foundQR = foundQR && this->checkBoundingBox(boundingBox, frame);
 
    if(foundQR){
      // draw bounding box on frame 
      drawBox(frame, boundingBox);
      cv::imshow("detection", frame);
      cv::waitKey(1);
      // this->publishImage(frame);
    }
    // publish detection
    this->publishQR(this->get_clock()->now(), foundQR);
  }
  cap.release();
}
 
void QRCodeDetectionNode::publishQR(rclcpp::Time stamp, bool detected){
  std::unique_ptr<QRCodeDetectType> qrMsgUPtr = std::make_unique<QRCodeDetectType>();
  qrMsgUPtr->header.stamp = stamp;
  qrMsgUPtr->detected = detected;
 
  m_qrCodeDetectionPubSPtr->publish(std::move(qrMsgUPtr));
}
 
// void QRCodeDetectionNode::publishImage(cv::Mat &image){
//   std::unique_ptr<sensor_msgs::msg::Image> msgImgUPtr = std::make_unique<sensor_msgs::msg::Image>();
//   std_msgs::msg::Header header;
//   header.frame_id = "qr";
//   header.stamp = this->get_clock()->now();
//   cv_bridge::CvImage imgBridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, image);
//   imgBridge.toImageMsg(*msgImgUPtr);
//   m_debugImgPublisher->publish(std::move(msgImgUPtr));
// }
 
bool QRCodeDetectionNode::checkBoundingBox(cv::Mat &bbox, cv::Mat &image){
  // allowed deviation
  float percentage = 0.3;
  cv::Point2i topLeft = cv::Point2i(bbox.at<float>(0,0),bbox.at<float>(0,1));
  cv::Point2i topRight = cv::Point2i(bbox.at<float>(1,0),bbox.at<float>(1,1));
  cv::Point2i botRight = cv::Point2i(bbox.at<float>(2,0),bbox.at<float>(2,1));
  cv::Point2i botLeft = cv::Point2i(bbox.at<float>(3,0),bbox.at<float>(3,1));
  // std::cout << "Difference: " << std::abs(topLeft.y - topRight.y) << " Target: " << percentage * image.rows << std::endl;
  // std::cout << "Difference: " << std::abs(topRight.x - botRight.x) << " Target: " << percentage * image.cols << std::endl;
  // std::cout << "Difference: " << std::abs(botRight.y - botLeft.y) << " Target: " << percentage * image.rows << std::endl;
  // std::cout << "Difference: " << std::abs(botLeft.x - topLeft.x) << " Target: " << percentage * image.cols << std::endl;
 
  return std::abs(topLeft.y - topRight.y) < percentage * image.rows&&
        std::abs(topRight.x - botRight.x) < percentage * image.cols&&
        std::abs(botRight.y - botLeft.y) < percentage * image.rows&&
        std::abs(botLeft.x - topLeft.x) < percentage * image.cols;
}
 
void QRCodeDetectionNode::drawBox(cv::Mat &image, cv::Mat &bbox){
  int n = bbox.rows;
  for(int i = 0 ; i < n ; i++){
    cv::line(
      image, 
      cv::Point2i(bbox.at<float>(i,0),bbox.at<float>(i,1)), 
      cv::Point2i(bbox.at<float>((i+1) % n,0), bbox.at<float>((i+1) % n,1)), cv::Scalar(255,0,0), 
      3
    );
  }
}