stoplinedetection.cpp

Go to the documentation of this file.

#include "stoplinedetection.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 <opencv2/imgproc/imgproc.hpp>
#include <vector>
 
#include <iostream>
#include <ostream>
#include <string>
 
#include <cmath>
 
#include "utils/clock.hpp"
 
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/core/types.hpp>
 
using namespace ::std::literals;
 
using namespace galaxis::ros;
using namespace galaxis::utils;
 
using namespace std;
 
using std::placeholders::_1;
 
StopLineDetection::StopLineDetection() : GalaxisNode("StopLineDetection") {
  log("StopLineDetectorNode: Initializing...");
 
  std::string configPath = "settings/onstartup_startstopline.json";
  auto settingsJsonSPtr = read_json_file(configPath);
  json& settingsJson = (*settingsJsonSPtr)["settings"];
  m_debugMode = settingsJson["debug_mode"];
  m_startlineDetectionMode = settingsJson["startline_detection"];
  m_stopLineDetectionMode = settingsJson["stopline_detection"];
 
  // Publisher
  if(m_debugMode == true) {
    m_stopLineImagePublisher = image_transport::create_publisher(this, galaxis::ros::TOPIC_STOP_LINE_IMAGE);
    m_startLineImagePublisher = image_transport::create_publisher(this, galaxis::ros::TOPIC_START_LINE_IMAGE);
  }
  
  galaxis::ros::create_publisher(this, m_stopLinePublisherSPtr);
 
 
  // get gloabl settings
  m_simulationMode = this->get_global_settings()["simulation_mode"];
 
  log("Configuration:\n - SimulationMode: ", m_simulationMode ? "ENABLED" : "DISABLED");
 
  if(m_simulationMode) { // Initializing Simulation
    rclcpp::QoS qos_params = get_qos_profile(QOS_BEST_EFFORT);
    m_StopLineImageSubscriber = this->create_subscription<sensor_msgs::msg::Image>(
      "/camera/image_raw",
      qos_params,
      std::bind(&StopLineDetection::on_stop_line_detection_callback, this, _1));
  } else { // Initializing Normal Mode
    rclcpp::QoS qos_params = get_qos_profile(QOS_BEST_EFFORT);
    m_StopLineImageSubscriber = this->create_subscription<sensor_msgs::msg::Image>(
      TOPIC_CAMERA_RAW,
      qos_params,
      std::bind(&StopLineDetection::on_stop_line_detection_callback, this, _1));
  }
 
 
  // get camera matrix and top view transform
  newCameraMatrix = galaxis::camera::getOptimalCameraMatrix();
  perspectiveTransformMatrix = galaxis::camera::getPerspectiveTransform();
 
  this->enable_parameters(std::bind(&StopLineDetection::new_parameters_callback, this, _1));
}
 
StopLineDetection::~StopLineDetection() {}
 
void StopLineDetection::on_stop_line_detection_callback(sensor_msgs::msg::Image::SharedPtr msg) {
  galaxis::utils::MicroStopwatch stopwatch;
 
  // CvImage
  // class CvImage
  // {
  // public:
  //   std_msgs::Header header;
  //   std::string encoding;
  //   cv::Mat image;
  // };
  std::shared_ptr<cv_bridge::CvImage const> cvImgSPtr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8); //BGR8 //RGB8
 
  assert(cvImgSPtr->image.cols == 512 && cvImgSPtr->image.rows == 256);
 
  cv::Mat undistortedImage;
  undistortedImage.create(256, 512, cvImgSPtr->image.type());
  cv::undistort(cvImgSPtr->image, undistortedImage, galaxis::camera::matrixD_mtx, galaxis::camera::matrixK_dist, newCameraMatrix);
 
  cv::Mat undistortedTopViewImage;
  undistortedImage.create(256, 512, cvImgSPtr->image.type());
  cv::warpPerspective(undistortedImage, undistortedTopViewImage, perspectiveTransformMatrix, cv::Size(512, 256));
 
  // StopLineDetection
  bool stopLineDetected = false;
  int height = 0;
  int yOffset = 0;
  if(m_stopLineDetectionMode) {
    height = undistortedTopViewImage.rows * 0.3f;
    yOffset = undistortedTopViewImage.rows * 0.55f;
    cv::Rect roiForCropStopLine = Rect(undistortedTopViewImage.cols * 0.4f, yOffset, undistortedTopViewImage.cols * 0.25f, height);
    cv::Mat croppedImageForStartStopLineDetection = undistortedTopViewImage(roiForCropStopLine);
    cv::Mat copiedImageForStopLineDetec = croppedImageForStartStopLineDetection.clone(); // copy
    stopLineDetected = this->detect_stopline(copiedImageForStopLineDetec, msg->header);
  }
 
  // StartLineDetection
  bool startLineDetected = false;
  if(m_startlineDetectionMode) {
    height = undistortedTopViewImage.rows * 0.2f;
    yOffset = undistortedTopViewImage.rows * 0.6f;
    cv::Rect roiForCrop = Rect(undistortedTopViewImage.cols * 0.2f, yOffset, undistortedTopViewImage.cols * 0.4f, height);
    cv::Mat croppedImageForStartLineDetection = undistortedTopViewImage(roiForCrop);
    startLineDetected = this->detect_startline(croppedImageForStartLineDetection, msg->header);
  }
 
  if(startLineDetected || stopLineDetected) {
    log("Start or Stopline detected: Sending message...");
    std::unique_ptr<StopLineType> lineMessageUPtr = std::make_unique<StopLineType>();
 
    if(stopLineDetected && startLineDetected) {
      stopLineDetected = false;
    }
 
    lineMessageUPtr->header.stamp = msg->header.stamp;
    lineMessageUPtr->stopline_flag = stopLineDetected;
    lineMessageUPtr->stopline_distance = stopLineDetected ? 0.5f : -1.f; // TODO
    
    lineMessageUPtr->startline_flag = startLineDetected;
    lineMessageUPtr->startline_distance = startLineDetected ? 0.5f : -1.f; // TODO
    
    m_stopLinePublisherSPtr->publish(std::move(lineMessageUPtr));
    log("Total Calculation and Publish Time [ms]: ", stopwatch.get_milliseconds());
  }
}
 
bool StopLineDetection::detect_stopline(cv::Mat &image, std_msgs::msg::Header const& header) {
  galaxis::utils::MicroStopwatch stopwatch;
  bool stopLineDetected = false;
  
  cv::Mat greyImage;
  cv::Mat temp;
  assert(image.channels() > 1);
  cv::cvtColor(image, temp, cv::COLOR_BGR2GRAY);
  threshold(temp, greyImage, 200, 255, CV_THRESH_BINARY);
 
  cv::Mat inputImage;
  Canny(greyImage, inputImage, 60, 200, 3);
 
  cv::Mat debugImage;
  cv::cvtColor(inputImage, debugImage, CV_GRAY2BGR);
 
  // Do the work
  std::vector<cv::Vec4i> p_lines;
  cv::HoughLinesP( inputImage, p_lines, 
                  1, CV_PI/180, m_stopLineHoughThreshold, 
                  m_stopLineHoughMinLineLength, m_stopLineHoughMaxLineGap);
 
  // std::cout << "Raw detections from hough:  " << std::to_string(p_lines.size()) << std::endl;
  
  std::vector<cv::Vec4i> p_linesPostprocessed;
  p_linesPostprocessed.reserve(p_lines.size());
 
  int yTolerance = 18;
  int xMinLength = 50;
  for(auto const& line : p_lines) {
    if(std::fabs(line[1] - line[3]) < yTolerance && std::fabs(line[0] - line[2]) > xMinLength) {
      p_linesPostprocessed.push_back(line);
    }
  }
 
  if(p_linesPostprocessed.size() > 0) {
    stopLineDetected = true;
  }
 
  if(m_debugMode && m_stopLineImagePublisher.getNumSubscribers() > 0) {
    if(p_linesPostprocessed.size() > 0) log_debug("Detected StopLines: ", p_linesPostprocessed.size());
    // log_debug("StartStopLineDetection Time [us]: ", stopwatch.get());
 
    for (const cv::Vec4i& l : p_linesPostprocessed) {
      #ifndef CV_VERSION_EPOCH
          cv::line(debugImage, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255, 0, 0), 3, cv::LINE_AA);
      #else
          cv::line(debugImage, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255, 0, 0), 3, CV_AA);
      #endif
    }
 
    // send debug image
    std::shared_ptr<sensor_msgs::msg::Image> msgImgSPtr = std::make_shared<sensor_msgs::msg::Image>();
    cv_bridge::CvImage m_bridgeImg = cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, debugImage); // RGB8
    m_bridgeImg.toImageMsg(*msgImgSPtr); // cv_bridge to sensor_msgs::Image
    m_stopLineImagePublisher.publish(msgImgSPtr);
  }
 
  return stopLineDetected; 
}
 
bool StopLineDetection::detect_startline(cv::Mat &image, std_msgs::msg::Header const& header) {
  galaxis::utils::MicroStopwatch stopwatch;
 
  cv::Mat& frame = image;
  bool startLineDetected = false;
 
  
  cv::Mat greyImage;
  cv::Mat temp;
  cv::cvtColor(frame, temp, cv::COLOR_BGR2GRAY);
  threshold(temp, greyImage, m_startLineGreynessLowerThreshold, 255, CV_THRESH_BINARY);
 
  std::vector<cv::Point2f> corners;
  bool use_harris_detector = false;
 
 
  cv::RNG rng(12345);
 
  cv::goodFeaturesToTrack(greyImage, corners, 0, m_startLineQualityLevel, m_startLineMinDistance, cv::Mat(), m_startLineBlockSize,
                          use_harris_detector, m_startLineK);
 
 
  double yAverage = 0.0;
  for(auto const& corner : corners) {
    yAverage += corner.y;
  }
  yAverage /= corners.size();
 
  
  int pointsInAverage = 0;
  std::vector<cv::Point2f> cornersPostprocessed;
  cornersPostprocessed.reserve(corners.size());
  for(auto const& corner : corners) {
    if(std::fabs(corner.y - yAverage) < m_startLineClusterEps) {
      pointsInAverage++;
      cornersPostprocessed.push_back(corner);
    }
  }
 
  if(pointsInAverage > m_startLinePointsCountThreshold) {
    log_debug("StartLine with Detected Corners: ", cornersPostprocessed.size());
    log_debug("Corner Detection Time [ms]: ", stopwatch.get_milliseconds());
    startLineDetected = true;
  }
 
  if(m_debugMode && m_startLineImagePublisher.getNumSubscribers() > 0) {
    int r = 4;
    for (const cv::Point2f& corner : cornersPostprocessed)
    {
      cv::circle(frame, corner, r, cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)), -1, 8,
                  0);
    }
 
    // send debug image
    std::shared_ptr<sensor_msgs::msg::Image> msgImgSPtr = std::make_shared<sensor_msgs::msg::Image>();
    cv_bridge::CvImage m_bridgeImg = cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, frame); // RGB8
    m_bridgeImg.toImageMsg(*msgImgSPtr); // cv_bridge to sensor_msgs::Image
    m_startLineImagePublisher.publish(msgImgSPtr);
  }
 
  return startLineDetected; 
}
 
rcl_interfaces::msg::SetParametersResult StopLineDetection::new_parameters_callback(const std::vector<rclcpp::Parameter>& parameters) {
  log("[StopLineDetection] NEW PARAMETERS");
  
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  result.reason = "";
 
  for (const auto &param: parameters) {
    if (param.get_name() == "stopline.hough_lines_minLineLength") {
      if(check_parameter<int>(param, result, 1, 1000)) {
        m_stopLineHoughMinLineLength = param.as_int();
      }
    }
    else if (param.get_name() == "stopline.hough_lines_maxLineGap") {
      if(check_parameter<int>(param, result, 1, 1000)) {
        m_stopLineHoughMaxLineGap = param.as_int();
      }
    }
    else if (param.get_name() == "startline.whiteness_lower_threshold") {
      if(check_parameter<int>(param, result, 1, 1000)) {
        m_startLineGreynessLowerThreshold = param.as_int();
      }
    }
    else if (param.get_name() == "startline.feature_quality_level") {
      if(check_parameter<double>(param, result, 0, 1)) {
        m_startLineQualityLevel = param.as_double();
      }
    }
    else if (param.get_name() == "startline.feature_min_distance") {
      if(check_parameter<int>(param, result, 0, 1000)) {
        m_startLineMinDistance = param.as_int();
      }
    }
    else if (param.get_name() == "startline.feature_block_size") {
      if(check_parameter<int>(param, result, 0, 1000)) {
        m_startLineBlockSize = param.as_int();
      }
    }
    else if (param.get_name() == "startline.feature_k") {
      if(check_parameter<double>(param, result, 0, 5)) {
        m_startLineK = param.as_double();
      }
    }
    else if (param.get_name() == "startline.cluster_eps") {
      if(check_parameter<int>(param, result, 0, 50)) {
        m_startLineClusterEps = param.as_int();
      }
    }
    else if (param.get_name() == "startline.points_count_threshold") {
      if(check_parameter<int>(param, result, 0, 500)) {
        m_startLinePointsCountThreshold = param.as_int();
      }
    }
    else if (param.get_name() == "stopline.hough_lines_threshold") {
      if(check_parameter<int>(param, result, 1, 1000)) {
        m_stopLineHoughThreshold = param.as_int();
      }
    } else {
      result.successful = false;
      result.reason += "[" + param.get_name() + "] UNKNOWN param or NOT IMPLEMENTED; ";
    }
  }
 
  if(result.successful == false) {
    log_warning("[AutopilotNode] Failed to set some parameters. Reasons: ", result.reason);
  }
 
  return result;
}