LaneDetectionGT.cpp

Go to the documentation of this file.

#include "lanedetection-groundtruth/LaneDetectionGT.hpp"
#include <galaxis/node_ros_pubsub_wrapper.hpp>
 
#include <iostream>
#include <stdio.h>
 
#include <utils/logger.hpp>
#include <utils/file.hpp>
#include <galaxis/AutopilotSettings.hpp>
#include <unistd.h>
#include <thread>
 
#include "lanedetection-groundtruth/MapClasses.hpp"
 
#include <sensor_msgs/image_encodings.hpp>
 
using namespace galaxis::ros;
using namespace galaxis::utils;
 
using namespace galaxis::map::data;
 
 
constexpr double MATH_PI_DOUBLE = 3.141592653589793238;
 
 
LaneDetectionGT::LaneDetectionGT() :
    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();
}
 
LaneDetectionGT::~LaneDetectionGT() {}
 
 
// Callback functions for ROS Subscriber
void LaneDetectionGT::groundtruthLeftLineCallback(visualization_msgs::msg::Marker::SharedPtr p_message) {
  groundtruthCallback(p_message, LINE_TYPE::LEFT_LINE);
}
 
void LaneDetectionGT::groundtruthCenterLineCallback(visualization_msgs::msg::Marker::SharedPtr p_message) {
  groundtruthCallback(p_message, LINE_TYPE::CENTER_LINE);
}
 
void LaneDetectionGT::groundtruthRightLineCallback(visualization_msgs::msg::Marker::SharedPtr p_message) {
  groundtruthCallback(p_message, LINE_TYPE::RIGHT_LINE);
}
 
void LaneDetectionGT::groundtruthCallback(visualization_msgs::msg::Marker::SharedPtr p_message, LINE_TYPE type) {
  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;
  }
}
 
void LaneDetectionGT::groundtruthObstacleCallback(visualization_msgs::msg::Marker::SharedPtr p_message) {
  
  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);
  }
 
}
 
 
void LaneDetectionGT::carPoseCallback(geometry_msgs::msg::Pose::SharedPtr p_message) {
  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]");
  }
}
 
 
// checks if all tiles for all lanes received as well as carPose -> then LaneDetectionGT::loop() 
void LaneDetectionGT::checkReady() {
  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;
  }
}
 
// main loop: running
void LaneDetectionGT::loop() {
  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!");
    }
 
  }
}