LaneDetectionGT.hpp

Go to the documentation of this file.

#ifndef LANE_DETECTION_HPP
#define LANE_DETECTION_HPP
 
#include <memory>
#include <mutex>
#include <map>
 
#include <rclcpp/rclcpp.hpp>
#include <galaxis/messages.hpp>
#include <galaxis/linatypes.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <image_transport/image_transport.h>
 
#include "MapDataLayer.hpp"
 
#include <memory>
 
enum LINE_TYPE : int {
  LEFT_LINE   = 0,
  CENTER_LINE = 1,
  RIGHT_LINE  = 2
};
 
class Obstacle {
public:
  Vector2f positionCM;
  float widthCM;
};  
 
class LaneDetectionGT : public rclcpp::Node {
private:
  Vector2i m_lineImageWindowSize;
 
  MapDataLayer m_mapDataLayer;
 
  int m_simulatedLDFPSWaitingTimeMicroseconds = 1000;
  bool m_ready                                = false;
  float m_obstacleDetectionDistanceCM         = 200.f;
 
  int m_groundtruthTileIteratorLeftLine = 1;
  int m_groundtruthTileIteratorCenterLine = 1;
  int m_groundtruthTileIteratorRightLine = 1;
 
  std::map<int, Obstacle> m_obstacleMap;
 
  std::mutex m_carPoseMtx;
  Vector2f m_currentCarPositionCM = Vector2f(-100000,-100000);
  double m_currentAbsoluteOrientation = -100000;
  rclcpp::Time m_currentCarPoseTimestamp;
 
  // Subscription
  rclcpp::Subscription<visualization_msgs::msg::Marker>::SharedPtr m_subscriberLeftLineSPtr;
  rclcpp::Subscription<visualization_msgs::msg::Marker>::SharedPtr m_subscriberMiddleLineSPtr;
  rclcpp::Subscription<visualization_msgs::msg::Marker>::SharedPtr m_subscriberRightLineSPtr;
 
  rclcpp::Subscription<visualization_msgs::msg::Marker>::SharedPtr m_subscriberObstacleSPtr;
 
  rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr m_subscriberCarPoseSPtr;
 
  // Publisher
  image_transport::Publisher m_imgtransPub;
  rclcpp::Publisher<ObstacleListType>::SharedPtr m_publisherObstacleSPtr;
 
public:
  LaneDetectionGT();
  ~LaneDetectionGT();
 
private:
  void groundtruthLeftLineCallback(visualization_msgs::msg::Marker::SharedPtr p_message);
  void groundtruthCenterLineCallback(visualization_msgs::msg::Marker::SharedPtr p_message);
  void groundtruthRightLineCallback(visualization_msgs::msg::Marker::SharedPtr p_message);
  void groundtruthCallback(visualization_msgs::msg::Marker::SharedPtr p_message, LINE_TYPE type);
  
  void groundtruthObstacleCallback(visualization_msgs::msg::Marker::SharedPtr p_message);
 
  void carPoseCallback(geometry_msgs::msg::Pose::SharedPtr p_message);
 
  void checkReady();
  void loop();
};
 
#endif // LANE_DETECTION_HPP