Galaxis Codebase: LaneDetectionGT Class Reference

#include <LaneDetectionGT.hpp>

Inheritance diagram for LaneDetectionGT:

Collaboration diagram for LaneDetectionGT:

Public Member Functions
	LaneDetectionGT ()

	~LaneDetectionGT ()

Private Member Functions
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 ()

Private Attributes
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

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

image_transport::Publisher	m_imgtransPub

rclcpp::Publisher< ObstacleListType >::SharedPtr	m_publisherObstacleSPtr

Detailed Description

Definition at line 32 of file LaneDetectionGT.hpp.

Constructor & Destructor Documentation

◆ LaneDetectionGT()

LaneDetectionGT::LaneDetectionGT ( )

Definition at line 26 of file LaneDetectionGT.cpp.

                                  :
     rclcpp::Node("LaneDetectionGTNode"), m_mapDataLayer(1) {
   log("Initializing LaneDetectionGT-Node...");
  
   // Read configuration
   std::string configPath = "settings/onstartup_simulation_lanedetection_config.conf";
   auto configMap                          = read_configuration(configPath);
   m_simulatedLDFPSWaitingTimeMicroseconds = static_cast<int>(1000000 / std::stof(configMap->at("simulatedFPS")));
   int configGTKeepLast = std::stoi(configMap->at("groundtruthKeepLast"));
   m_lineImageWindowSize = Vector2i(std::stoi(configMap->at("imageWindowWidth")), std::stoi(configMap->at("imageWindowHeight")));
   m_obstacleDetectionDistanceCM = std::stof(configMap->at("obstacleDetectionDistanceCM"));
   log("Configuration (", configPath, "):", "\n", 
       " - SimulatedFPS: ", configMap->at("simulatedFPS"), " (WaitTimeMicroS: ", m_simulatedLDFPSWaitingTimeMicroseconds, ")\n",
       " - GroundtruthKeepLast: ", configGTKeepLast, "\n",
       " - Image Size (Window Size for Lookup): ", m_lineImageWindowSize[0], "cm x ", m_lineImageWindowSize[1], "cm");
  
   // Subsciber
   rclcpp::QoS qos_params     = rclcpp::QoS(rclcpp::KeepLast(configGTKeepLast), qos_profile_default);
   m_subscriberLeftLineSPtr   = this->create_subscription<visualization_msgs::msg::Marker>("/simulation/visualization/groundtruth/left_line", qos_params, std::bind(&LaneDetectionGT::groundtruthLeftLineCallback, this, std::placeholders::_1));
   m_subscriberMiddleLineSPtr = this->create_subscription<visualization_msgs::msg::Marker>("/simulation/visualization/groundtruth/middle_line", qos_params, std::bind(&LaneDetectionGT::groundtruthCenterLineCallback, this, std::placeholders::_1));
   m_subscriberRightLineSPtr  = this->create_subscription<visualization_msgs::msg::Marker>("/simulation/visualization/groundtruth/right_line", qos_params, std::bind(&LaneDetectionGT::groundtruthRightLineCallback, this, std::placeholders::_1));
   
   m_subscriberObstacleSPtr  = this->create_subscription<visualization_msgs::msg::Marker>("/simulation/visualization/groundtruth/obstacle", qos_params, std::bind(&LaneDetectionGT::groundtruthObstacleCallback, this, std::placeholders::_1));
   
   qos_params = get_qos_profile(QOS_BEST_EFFORT);
   m_subscriberCarPoseSPtr  = this->create_subscription<geometry_msgs::msg::Pose>("/simulation/gazebo/model/dr_drift/pose", qos_params, std::bind(&LaneDetectionGT::carPoseCallback, this, std::placeholders::_1));
  
   m_imgtransPub = image_transport::create_publisher(this, TOPIC_VCI_LANE_IMAGE_SIMULATION);
   
   // obstacle Publisher
   galaxis::ros::create_publisher(this, m_publisherObstacleSPtr);
   log("Waiting for messages...");
  
   // starting background thread for line detection image publishing
   std::thread(&LaneDetectionGT::loop, this).detach();
 }

References carPoseCallback(), groundtruthCenterLineCallback(), groundtruthLeftLineCallback(), groundtruthObstacleCallback(), groundtruthRightLineCallback(), galaxis::utils::log(), loop(), m_imgtransPub, m_lineImageWindowSize, m_obstacleDetectionDistanceCM, m_publisherObstacleSPtr, m_simulatedLDFPSWaitingTimeMicroseconds, m_subscriberCarPoseSPtr, m_subscriberLeftLineSPtr, m_subscriberMiddleLineSPtr, m_subscriberObstacleSPtr, m_subscriberRightLineSPtr, and galaxis::utils::read_configuration().

Here is the call graph for this function:

◆ ~LaneDetectionGT()

LaneDetectionGT::~LaneDetectionGT ( )

Definition at line 63 of file LaneDetectionGT.cpp.

63 {}

Member Function Documentation

◆ carPoseCallback()

void LaneDetectionGT::carPoseCallback ( geometry_msgs::msg::Pose::SharedPtr p_message )

private

Definition at line 162 of file LaneDetectionGT.cpp.

                                                                              {
   if(m_carPoseMtx.try_lock()) {
     m_currentCarPoseTimestamp = this->get_clock()->now(); // WARNING Pose has no header!
  
     // Copied from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
     // yaw (z-axis rotation)
     double siny_cosp = 2 * (p_message->orientation.w * p_message->orientation.z + p_message->orientation.x * p_message->orientation.y);
     double cosy_cosp = 1 - 2 * (p_message->orientation.y * p_message->orientation.y + p_message->orientation.z * p_message->orientation.z);
     double yaw = std::atan2(siny_cosp, cosy_cosp);
  
     m_currentAbsoluteOrientation = 2*MATH_PI_DOUBLE - yaw;
  
     // fix orientation from simulation to internal galaxis representation
     m_currentAbsoluteOrientation += MATH_PI_DOUBLE / 2;
     m_currentAbsoluteOrientation = std::fmod(m_currentAbsoluteOrientation, 2*MATH_PI_DOUBLE);
  
     // POSITION
     m_currentCarPositionCM[0] = p_message->position.x * 100;
     m_currentCarPositionCM[1] = p_message->position.y * 100;
  
     // Shift Position to Front Axis
     Vector2f carPosOffToFrontAx = galaxis::autopilot::simluationCarPositionOffsetToFrontAxisM;
     Eigen::Rotation2Df rotationMat(-m_currentAbsoluteOrientation);
     carPosOffToFrontAx = rotationMat * carPosOffToFrontAx;
     m_currentCarPositionCM += carPosOffToFrontAx * 100;
  
     m_carPoseMtx.unlock();
     // log("CarPose updated. [(X: ", m_currentCarPositionCM[0], ", Y: ", m_currentCarPositionCM[1],")cm, Yaw: ", m_currentAbsoluteOrientation, "Rad]");
   }
 }

References m_carPoseMtx, m_currentAbsoluteOrientation, m_currentCarPoseTimestamp, m_currentCarPositionCM, MATH_PI_DOUBLE, and galaxis::autopilot::simluationCarPositionOffsetToFrontAxisM.

Referenced by LaneDetectionGT().

Here is the caller graph for this function:

◆ checkReady()

void LaneDetectionGT::checkReady ( )

private

Definition at line 195 of file LaneDetectionGT.cpp.

                                  {
   if(m_groundtruthTileIteratorLeftLine == -1 && m_groundtruthTileIteratorCenterLine == -1 && m_groundtruthTileIteratorRightLine == -1 && m_currentCarPositionCM[0] != -100000 && m_currentAbsoluteOrientation != -100000) {
     m_ready = true;
     log("Ready to start loop.");
   }
   else {
     m_ready = false;
   }
 }

References galaxis::utils::log(), m_currentAbsoluteOrientation, m_currentCarPositionCM, m_groundtruthTileIteratorCenterLine, m_groundtruthTileIteratorLeftLine, m_groundtruthTileIteratorRightLine, and m_ready.

Referenced by groundtruthCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ groundtruthCallback()

void LaneDetectionGT::groundtruthCallback	(	visualization_msgs::msg::Marker::SharedPtr	p_message,
		LINE_TYPE	type
	)

private

Definition at line 79 of file LaneDetectionGT.cpp.

                                                                                                         {
   if (m_ready) return;
  
   int* groundtruthTileIterator;
   char laneType;
   if (type == LINE_TYPE::LEFT_LINE){
     groundtruthTileIterator = &m_groundtruthTileIteratorLeftLine;
     laneType = 'l';
   }
   else if (type == LINE_TYPE::CENTER_LINE) {
     groundtruthTileIterator = &m_groundtruthTileIteratorCenterLine;
     laneType = 'c';
   }
   else if (type == LINE_TYPE::RIGHT_LINE) {
     groundtruthTileIterator = &m_groundtruthTileIteratorRightLine;
     laneType = 'r';
   }
   else {
     log_warning("Failure (Lane Type)");
     return;
   }
  
   // check if already all tiles received and inserted in map
   if (p_message->id != *groundtruthTileIterator) {
     if(p_message->id == 1 && *groundtruthTileIterator != -1) {
       *groundtruthTileIterator = -1;
     }
     else {
       return;
     }
   }
  
   if (type == LINE_TYPE::LEFT_LINE){
     log("Left Line Groundtruth Message Received (", p_message->id, ")");
   }
   else if (type == LINE_TYPE::CENTER_LINE) {
     log("Center Line Groundtruth Message Received (", p_message->id, ")");
   }
   else if (type == LINE_TYPE::RIGHT_LINE) {
     log("Right Line Groundtruth Message Received (", p_message->id, ")");
   }
   else {
     log_warning("Failure (Lane Type)");
     return;
   }
  
   // insert tile into map
   m_mapDataLayer.updateMap(p_message, laneType);
  
   checkReady();
  
   // wait for next tile to insert into map
   if (*groundtruthTileIterator != -1) {
     *groundtruthTileIterator += 1;
   }
 }

References CENTER_LINE, checkReady(), LEFT_LINE, galaxis::utils::log(), galaxis::utils::log_warning(), m_groundtruthTileIteratorCenterLine, m_groundtruthTileIteratorLeftLine, m_groundtruthTileIteratorRightLine, m_mapDataLayer, m_ready, RIGHT_LINE, and MapDataLayer::updateMap().

Referenced by groundtruthCenterLineCallback(), groundtruthLeftLineCallback(), and groundtruthRightLineCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ groundtruthCenterLineCallback()

void LaneDetectionGT::groundtruthCenterLineCallback ( visualization_msgs::msg::Marker::SharedPtr p_message )

private

Definition at line 71 of file LaneDetectionGT.cpp.

                                                                                                   {
   groundtruthCallback(p_message, LINE_TYPE::CENTER_LINE);
 }

References CENTER_LINE, and groundtruthCallback().

Referenced by LaneDetectionGT().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ groundtruthLeftLineCallback()

void LaneDetectionGT::groundtruthLeftLineCallback ( visualization_msgs::msg::Marker::SharedPtr p_message )

private

Definition at line 67 of file LaneDetectionGT.cpp.

                                                                                                 {
   groundtruthCallback(p_message, LINE_TYPE::LEFT_LINE);
 }

References groundtruthCallback(), and LEFT_LINE.

Referenced by LaneDetectionGT().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ groundtruthObstacleCallback()

void LaneDetectionGT::groundtruthObstacleCallback ( visualization_msgs::msg::Marker::SharedPtr p_message )

private

Definition at line 136 of file LaneDetectionGT.cpp.

                                                                                                 {
   
   if(m_obstacleMap.find(p_message->id) == m_obstacleMap.end()) { // check if obstacle already inserted
  
     // calc center point of obstacle
     Vector2f centerPoint = Vector2f(p_message->points[0].x - p_message->points[2].x, p_message->points[0].y - p_message->points[2].y) / 2;
     centerPoint[0] += p_message->points[2].x;
     centerPoint[1] += p_message->points[2].y;
  
     float width = std::fabs(p_message->points[0].x - p_message->points[1].x);
     if (width <= 0.0001) {
       width = std::fabs(p_message->points[0].y - p_message->points[1].y);
     }
  
     Obstacle obstacle;
     obstacle.positionCM = centerPoint * 100;
     obstacle.widthCM = width * 100; 
  
     m_obstacleMap.insert(std::pair<int, Obstacle>(p_message->id, obstacle));
  
     log("Obstacle Message Received (", p_message->id, ") with Center Point [m] (x  y): ", centerPoint[0], "  ", centerPoint[1], " and Width [m]: ", width);
   }
  
 }

References galaxis::utils::log(), m_obstacleMap, Obstacle::positionCM, and Obstacle::widthCM.

Referenced by LaneDetectionGT().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ groundtruthRightLineCallback()

void LaneDetectionGT::groundtruthRightLineCallback ( visualization_msgs::msg::Marker::SharedPtr p_message )

private

Definition at line 75 of file LaneDetectionGT.cpp.

                                                                                                  {
   groundtruthCallback(p_message, LINE_TYPE::RIGHT_LINE);
 }

References groundtruthCallback(), and RIGHT_LINE.

Referenced by LaneDetectionGT().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ loop()

void LaneDetectionGT::loop ( )

private

Definition at line 206 of file LaneDetectionGT.cpp.

                            {
   while(1) {
     usleep(m_simulatedLDFPSWaitingTimeMicroseconds);
     if(!m_ready) continue;
  
     Vector2i effectiveWindowSize = Vector2i(-1,-1);
     std::shared_ptr<sensor_msgs::msg::Image> lineMessageUPtr = std::make_unique<sensor_msgs::msg::Image>();
  
     m_carPoseMtx.lock();
     lineMessageUPtr->header.stamp = m_currentCarPoseTimestamp;
     std::shared_ptr<LinesObject<int16_t>> lineObj = m_mapDataLayer.getLineObject(m_currentCarPositionCM, m_currentAbsoluteOrientation, m_lineImageWindowSize, effectiveWindowSize);
     m_carPoseMtx.unlock();
     
     // publish lane image
     if (effectiveWindowSize[0] < 0 || effectiveWindowSize[1] < 0) log_warning("Effective Window Size not correct!");
  
     lineMessageUPtr->width = m_lineImageWindowSize[0];
     lineMessageUPtr->height = m_lineImageWindowSize[1];
     lineMessageUPtr->encoding = sensor_msgs::image_encodings::RGB8;
     lineMessageUPtr->is_bigendian = 0;
     lineMessageUPtr->step = lineMessageUPtr->width * 3;
  
     lineMessageUPtr->data.clear();
     lineMessageUPtr->data.resize(lineMessageUPtr->step * lineMessageUPtr->height);
  
     int linePointsAmountOnImage = 0;
     
     // extract left line points
     LinePoints<int16_t>* lineMapObjPtr = &lineObj->leftLine;
     int pixelByteIndex = galaxis::autopilot::imageLLinePixelByteIndex;
  
     for (int i = 0; i < 3; ++i) { // for all 3 lines
       for(auto& [pointXRaw, pointsYVec] : lineMapObjPtr->getPointsXY()) {
         for (auto& pointYRaw : pointsYVec) {
           // rotate point
           Vector2f vec = Vector2f(pointXRaw, pointYRaw);
           vec = lineObj->reverseRotationMatrix * vec;
           
           // to integer
           int pointX = std::round(vec[0]) + lineMessageUPtr->width / 2;
           int pointY = lineMessageUPtr->height - std::round(vec[1]);
  
           //skip if behind car
           if (pointX < 0 || pointY < 0 || pointX >= lineMessageUPtr->width || pointY >= lineMessageUPtr->height) {
             continue;
           }
  
           try {
             int index = lineMessageUPtr->step * pointY + pointX * 3 + pixelByteIndex;
             if(index >= static_cast<int>(lineMessageUPtr->data.size())) continue;
  
             lineMessageUPtr->data[index] = 255;
  
             linePointsAmountOnImage++;
           }
           catch (const std::exception& ex) {
             log_warning("Exception thrown: ", ex.what());
           }
         }
       }
       // switch to next line (l->c->r)
       if (pixelByteIndex == galaxis::autopilot::imageLLinePixelByteIndex) {
         lineMapObjPtr = &lineObj->centerLine;
         pixelByteIndex = galaxis::autopilot::imageCLinePixelByteIndex;
       }
       else if (pixelByteIndex == galaxis::autopilot::imageCLinePixelByteIndex) {
         lineMapObjPtr = &lineObj->rightLine;
         pixelByteIndex = galaxis::autopilot::imageRLinePixelByteIndex;
       }
       else {
         // break;
       }
  
     }
  
     // publish message
     m_imgtransPub.publish(lineMessageUPtr);
     log("LineDetection Top View Image Message sent! LinePointsAmountOnImage: ", linePointsAmountOnImage);
  
     m_carPoseMtx.lock();
  
     // obstacle message
     std::unique_ptr<ObstacleListType> obstacleListUPtr = std::make_unique<ObstacleListType>();
     obstacleListUPtr->header.stamp = m_currentCarPoseTimestamp; // this->get_clock()->now();
  
     bool obstacleFound = false;
     for(auto& [id, obstacle] : m_obstacleMap) {
       Vector2f distanceToObstacleVec = obstacle.positionCM - m_currentCarPositionCM;
       if(distanceToObstacleVec.norm() <= m_obstacleDetectionDistanceCM) {
         
         Eigen::Rotation2Df rotationMat(m_currentAbsoluteOrientation);
         distanceToObstacleVec = rotationMat * distanceToObstacleVec;
  
         if(distanceToObstacleVec[1] >= 0) {
           // obstacle is in 180deg FOV -> add to object list
           ObstacleType obstacleListElt;
           obstacleListElt.pos_x = distanceToObstacleVec[0];
           obstacleListElt.pos_x /= 100; // to m
           obstacleListElt.pos_y = distanceToObstacleVec[1];
           obstacleListElt.pos_y /= 100; // to m
           obstacleListElt.width = obstacle.widthCM;
           obstacleListElt.width /= 100; // to m
  
           obstacleListUPtr->obstacles.push_back(obstacleListElt);
  
           log("[OBSTACLE] Found in FOV. Distance: ", distanceToObstacleVec.norm(), " - Position x,y: ", distanceToObstacleVec[0], ",", distanceToObstacleVec[1]);
           obstacleFound = true;
         }
       }
     }
  
     m_carPoseMtx.unlock();
  
     if(obstacleFound) {
       m_publisherObstacleSPtr->publish(std::move(obstacleListUPtr));
       log("Obstacle Detection Message sent!");
     }
  
   }
 }