stoplinedetection.hpp

Go to the documentation of this file.

#ifndef STOPLINE_DETECTION_NODE_HPP
#define STOPLINE_DETECTION_NODE_HPP
 
 
#include "utils/clock.hpp"
 
#include "rclcpp/rclcpp.hpp"
#include <galaxis/GalaxisNode.hpp>
 
#include <sensor_msgs/msg/image.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <utils/settings.hpp>
 
#include "galaxis/CameraCalibration.hpp"
 
#include <opencv2/core/mat.hpp>
 
#include "galaxis/messages.hpp"
#include "std_msgs/msg/header.hpp"
 
using namespace cv;
 
class StopLineDetection : public GalaxisNode {
private:
  bool m_simulationMode = false;
  bool m_debugMode = false;
  bool m_startlineDetectionMode = true;
  bool m_stopLineDetectionMode = true;
 
  cv_bridge::CvImagePtr m_imgBridge_ptr;
  cv_bridge::CvImagePtr m_imgBridgeLeft_ptr;
  cv_bridge::CvImagePtr m_imgBridgeRight_ptr;
  cv_bridge::CvImagePtr m_imgBridgeLeftRef_ptr;
  cv_bridge::CvImagePtr m_imgBridgeRightRef_ptr;
  
  // ROS2
  //  Publisher
  image_transport::Publisher m_stopLineImagePublisher;
  image_transport::Publisher m_startLineImagePublisher;
  
  rclcpp::Publisher<StopLineType>::SharedPtr m_stopLinePublisherSPtr;
 
  //  Subscriber
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr m_StopLineImageSubscriber;
 
  bool m_stopLineDetectedInLastStep = false;
 
  int m_stopLineHoughMinLineLength = 25;
  int m_stopLineHoughMaxLineGap = 10;
  int m_stopLineHoughThreshold = 70;
 
  int m_startLineGreynessLowerThreshold = 80;
  double m_startLineQualityLevel = 0.35;
  double m_startLineMinDistance = 10;
  int m_startLineBlockSize = 3;
  double m_startLineK = 0.04;
  int m_startLineClusterEps = 10;
  int m_startLinePointsCountThreshold = 9;
 
  cv::Mat newCameraMatrix;
  cv::Mat perspectiveTransformMatrix;
 
public:
  StopLineDetection();
  ~StopLineDetection();
 
private:
  // standard mode callbacks
  void on_stop_line_detection_callback(sensor_msgs::msg::Image::SharedPtr msg);
  bool detect_startline(cv::Mat &image, std_msgs::msg::Header const& header);
  bool detect_stopline(cv::Mat &image, std_msgs::msg::Header const& header);
  rcl_interfaces::msg::SetParametersResult new_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
};
 
#endif // AUTOPILOT_NODE_HPP