AutoPilot.cpp

Go to the documentation of this file.

#include "Decision/AutoPilot.hpp"
 
#include <polyfit/galaxis_polyfit.h>
#include <utils/poly.hpp>
 
AutoPilot::AutoPilot(GalaxisNode* p_gNodePtr) :
    m_mapDataLayer(3), m_threadPool(3), m_decision(dynamic_cast<rclcpp::Node*>(p_gNodePtr)) {
 
  // init
  m_leftLineXYTmpVecSPtr = std::make_shared<std::map<int16_t, int16_t>>();
  m_centerLineXYTmpVecSPtr = std::make_shared<std::map<int16_t, int16_t>>();
  m_rightLineXYTmpVecSPtr = std::make_shared<std::map<int16_t, int16_t>>();
  m_leftLineYXTmpVecSPtr = std::make_shared<std::map<int16_t, int16_t>>();
  m_centerLineYXTmpVecSPtr = std::make_shared<std::map<int16_t, int16_t>>();
  m_rightLineYXTmpVecSPtr = std::make_shared<std::map<int16_t, int16_t>>();
 
  m_leftLinePolynomXVectorSPtr   = std::make_shared<std::vector<double>>();
  m_leftLinePolynomYVectorSPtr   = std::make_shared<std::vector<double>>();
  m_centerLinePolynomXVectorSPtr = std::make_shared<std::vector<double>>();
  m_centerLinePolynomYVectorSPtr = std::make_shared<std::vector<double>>();
  m_rightLinePolynomXVectorSPtr  = std::make_shared<std::vector<double>>();
  m_rightLinePolynomYVectorSPtr  = std::make_shared<std::vector<double>>();
 
  m_leftLinePolynomPointsVecSPtr  = std::make_shared<std::vector<Vector2f>>();
  m_rightLinePolynomPointsVecSPtr = std::make_shared<std::vector<Vector2f>>();
 
  m_leftLinePolynomXVectorSPtr->reserve(500); 
  m_leftLinePolynomYVectorSPtr->reserve(500); 
  m_centerLinePolynomXVectorSPtr->reserve(500);
  m_centerLinePolynomYVectorSPtr->reserve(500);
  m_rightLinePolynomXVectorSPtr->reserve(500);
  m_rightLinePolynomYVectorSPtr->reserve(500);
 
  m_lastDriveInformationSPtr = nullptr;
 
  m_polynomPointsXVector.reserve(1000);
  m_polynomPointsYVector.reserve(1000);
 
  m_gNodePtr = p_gNodePtr;
 
  m_laneBlocked = std::make_tuple(-1.f, -1.f, -1.f, -1.f);
}
 
AutoPilot::~AutoPilot() {
  m_threadPool.join();
}
 
// ############## Settings ##############
void AutoPilot::set_obstacle_lookup_distance(int value) {
  m_obstacleLookupDistanceCM = value;
}
void AutoPilot::set_obstacle_lookup_distance_step(float value) {
  m_obstacleLookupDistanceStepCM = value;
}
void AutoPilot::set_obstacle_to_lane_center_min_distance_for_dodge(float value) {
  m_obstacleToLaneCenterMinimumDistanceForDodge = value;
}
void AutoPilot::set_polynom_degree(int value) {
  m_polynomDegree = value;
}
void AutoPilot::set_polynom_min_points_amount(int value) {
  m_polynomMinimumPointsAmount = value;
}
void AutoPilot::set_traj_points_to_append_amount(int value) {
  m_trajPointsToAppendAmount = value;
}
void AutoPilot::set_traj_points_start_appending_offset(int value) {
  m_trajPointsStartAppendingOffset = value;
}
void AutoPilot::set_line_cluster_eps(int value){
  m_lineClusterEps = value;
}
void AutoPilot::set_line_cluster_min_pts(int value){
  m_lineClusterMinPts = value;
}
void AutoPilot::set_line_polynom_evaluation_step(int value) {
  m_linePolynomEvaluationStepCM = value;
}
void AutoPilot::set_line_polynom_evaluation_step_size(int value){
  m_linePolynomEvaluationStepSizeCM = value;
}
void AutoPilot::set_min_number_of_clustered_line_points(int value){
  m_minNumberOfClusteredLinePoints = value;
}
 
 
std::shared_ptr<DriveInformation_t> AutoPilot::get_drive_information() { // mTODO: add timestamp to get location at
  std::shared_ptr<DriveInformation_t> driveInformationSPtr = std::make_shared<DriveInformation_t>();
 
  std::shared_ptr<PositionTimepoint> positionTimepoint = m_localizer.get_latest_position_timepoint(); // TODO: check if we use simulation -> other clock
  driveInformationSPtr->positionTimepoint = *positionTimepoint;
 
  m_laneCentersPairVecUPtr = get_lane_centers(positionTimepoint);
 
  // left Lane
  std::pair<bool,bool> laneVisiblity = std::make_pair<bool, bool>(false, false); // <left, right>
  if (m_laneCentersPairVecUPtr->first.size() >= m_polynomMinimumPointsAmount) {
    if(m_decision.get_chosen_lane() == false) { // left lane
       // append line behind car for better/smoother fitting
      for(int i = m_trajPointsStartAppendingOffset; i < m_trajPointsStartAppendingOffset + m_trajPointsToAppendAmount; ++i) {
        m_laneCentersPairVecUPtr->first.push_back(galaxis::lina::Vector2f(0.f, - i * 1.f));
      }
    }
 
    m_polynomPointsXVector.clear();
    m_polynomPointsYVector.clear();
 
    for(auto const& point : m_laneCentersPairVecUPtr->first) {
      m_polynomPointsXVector.push_back(point[1] / 100); // from cm to m, rotate -90 cc to look into x direction (not y)
      m_polynomPointsYVector.push_back(-point[0] / 100);
    }
 
    std::get<0>(laneVisiblity) = true;
    std::get<0>(m_lanePolynom) = galaxis_polyfit(m_polynomPointsXVector, m_polynomPointsYVector, m_polynomPointsXVector.size() * 0.6f, 10, 7);
  }
  // right Lane
  if (m_laneCentersPairVecUPtr->second.size() >= m_polynomMinimumPointsAmount) {
    if(m_decision.get_chosen_lane() == true) { // left lane
       // append line behind car for better/smoother fitting
      for(int i = m_trajPointsStartAppendingOffset; i < m_trajPointsStartAppendingOffset + m_trajPointsToAppendAmount; ++i) {
        m_laneCentersPairVecUPtr->second.push_back(galaxis::lina::Vector2f(0.f, - i * 1.f));
      }
    }
 
    m_polynomPointsXVector.clear();
    m_polynomPointsYVector.clear();
 
    for(auto const& point : m_laneCentersPairVecUPtr->second) {
      m_polynomPointsXVector.push_back(point[1] / 100); // from cm to m
      m_polynomPointsYVector.push_back(-point[0] / 100);
    }
 
    std::get<1>(laneVisiblity) = true;
    std::get<1>(m_lanePolynom) = galaxis_polyfit(m_polynomPointsXVector, m_polynomPointsYVector, m_polynomPointsXVector.size() * 0.6f, 10, 7);
  }
 
  driveInformationSPtr->right = m_rightLinePoly;
  driveInformationSPtr->center = m_centerLinePoly;
  driveInformationSPtr->left = m_leftLinePoly;
 
 
  if (std::get<0>(laneVisiblity) == false && std::get<1>(laneVisiblity) == false) { // Blindfish -> Fall back to last polynom
    m_decision.update(positionTimepoint, m_laneBlocked);
 
    rclcpp::Duration maxDriveOnOldPoly(0, 750000000); // 750ms
    if (m_lastDriveInformationSPtr == nullptr || (positionTimepoint->timestamp - m_lastDriveInformationSPtr->timeLastPolynomCalculated) > maxDriveOnOldPoly) {
      // drive straight
      driveInformationSPtr->polynom = std::make_shared<std::vector<double>>();
      driveInformationSPtr->polynom->push_back(0.f);  // x^0
      driveInformationSPtr->polynom->push_back(0.f);  // x^1
      driveInformationSPtr->polynom->push_back(0.f); // x^2
      driveInformationSPtr->polynom->push_back(0.f);  // x^3
      // Set Velocity Ref
      driveInformationSPtr->velocityReference = m_decision.get_velocity_reference() * 0.5f;
      driveInformationSPtr->indicatorLeft = m_decision.get_indicator_left();
      driveInformationSPtr->indicatorRight = m_decision.get_indicator_right();
      driveInformationSPtr->startParking = (m_decision.get_current_situationstate() == SituationState::ParkingZone || m_decision.get_current_situationstate() == SituationState::Parking);
      log_warning("AutoPilot: Blindfish mode! Drive straight.");
    }
    else { // last polynom exists => calculate new shifted polynom
      // calculate x,y shift
      // Rotation2Df posDiffRotationMat(driveInformationSPtr->positionTimepoint.orientation);
      // Vector2f positionDiffToPolynomPosition = positionTimepoint->position - driveInformationSPtr->positionTimepoint.position;
      // positionDiffToPolynomPosition = posDiffRotationMat * positionDiffToPolynomPosition;
      // positionDiffToPolynomPosition = Vector2f(positionDiffToPolynomPosition[1],-positionDiffToPolynomPosition[0])
      // positionDiffToPolynomPosition /= 1000; // from mm to m
      // Rotation2Df rotationMat(positionTimepoint->orientation - driveInformationSPtr->positionTimepoint.orientation); // rotation matrix for point rotation
 
 
      m_polynomPointsXVector.clear();
      m_polynomPointsYVector.clear();
 
      // eval poly at estimated position // get time diff and multiply with velocity
      auto timestamp = positionTimepoint->timestamp - m_lastDriveInformationSPtr->positionTimepoint.timestamp;
      float drivenDistanceM = (m_lastDriveInformationSPtr->velocityReference * 1e9) / timestamp.nanoseconds(); // velocityReference is m/s (m/s * 1e9 = nm/s)
 
      // use drivenDistance as approximation of x-shift in polynom
      Vector2f newPosition = Vector2f(drivenDistanceM, eval_poly(m_lastDriveInformationSPtr->polynom, drivenDistanceM)); // nm / 1e9 = m
      
      float gradient = static_cast<float>(galaxis::utils::calculate_gradient(m_lastDriveInformationSPtr->polynom, newPosition[0]));
      float rotationClockwise = std::atan(gradient);
      Eigen::Rotation2Df rotationMat(-rotationClockwise); // because Eigen is CC
 
      // get points of last polynom
      // polynom is in meter => we want to get cm precision for new poly
      for(float x = 0.01; x < m_mapDataLayer.m_lookupWindowSizeCMHeight / 100.f; x += 0.01) { 
        // eval poly and direct shift
        // calulate pos on polynom + shift to new position
        Vector2f newPoint = Vector2f(x, eval_poly(m_lastDriveInformationSPtr->polynom, x)) - newPosition;
        newPoint = rotationMat * newPoint;
 
        m_polynomPointsXVector.push_back(newPoint[0]);
        m_polynomPointsYVector.push_back(newPoint[1]);
      }
 
      driveInformationSPtr->positionTimepoint = *positionTimepoint;
      // TODO: reenable when fixed
      // driveInformationSPtr->polynom = galaxis_polyfit(m_polynomPointsXVector, m_polynomPointsYVector, m_polynomPointsXVector.size() * 0.9, 20, 3);
      driveInformationSPtr->polynom = m_lastDriveInformationSPtr->polynom;
      driveInformationSPtr->velocityReference = m_decision.get_velocity_reference() * 0.7f; // decrease velocity
      driveInformationSPtr->timeLastPolynomCalculated = m_lastDriveInformationSPtr->timeLastPolynomCalculated;
      driveInformationSPtr->startParking = (m_decision.get_current_situationstate() == SituationState::ParkingZone || m_decision.get_current_situationstate() == SituationState::Parking);
 
      log_warning("AutoPilot: Blindfish mode! Fall back to last polynom.");
    }
  }
  else {
    m_laneBlocked = get_lane_blocked(m_lanePolynom);
    m_decision.update(positionTimepoint, m_laneBlocked);
 
    // TODO: move to decision state machine
    if(m_decision.get_chosen_lane() == false) { // leftLane
      driveInformationSPtr->polynom = m_lanePolynom.first;
    }
    else { // rightLane
      driveInformationSPtr->polynom = m_lanePolynom.second;
    }
 
    driveInformationSPtr->velocityReference = m_decision.get_velocity_reference();
    driveInformationSPtr->indicatorLeft = m_decision.get_indicator_left();
    driveInformationSPtr->indicatorRight = m_decision.get_indicator_right();
    driveInformationSPtr->startParking = (m_decision.get_current_situationstate() == SituationState::ParkingZone || m_decision.get_current_situationstate() == SituationState::Parking);
    driveInformationSPtr->timeLastPolynomCalculated = positionTimepoint->timestamp;
 
    m_lastDriveInformationSPtr = driveInformationSPtr; // set last correctly calculated driveInformation
  }
 
  return driveInformationSPtr;
}
 
std::unique_ptr<std::pair<std::vector<galaxis::lina::Vector2f>, std::vector<galaxis::lina::Vector2f>>> AutoPilot::get_lane_centers(std::shared_ptr<PositionTimepoint> positionTimepoint) { // TODO: add timestamp to get location at
  std::unique_ptr<std::pair<std::vector<galaxis::lina::Vector2f>, std::vector<galaxis::lina::Vector2f>>> lanesPairVecUPtr = std::make_unique<std::pair<std::vector<galaxis::lina::Vector2f>, std::vector<galaxis::lina::Vector2f>>>();
 
  m_linesObject   = m_mapDataLayer.getLineObject(positionTimepoint->position / 10, galaxis::autopilot::lookupLookBehindCarCM, positionTimepoint->orientation, Vector2i(m_mapDataLayer.m_lookupWindowSizeCMWidth, m_mapDataLayer.m_lookupWindowSizeCMHeight)); // m_localizer.get_position_timepoint_by_time()
 
  // DBSCAN cluster lines
  m_leftLineXYTmpVecSPtr->clear();
  m_centerLineXYTmpVecSPtr->clear();
  m_rightLineXYTmpVecSPtr->clear();
  m_leftLineYXTmpVecSPtr->clear();
  m_centerLineYXTmpVecSPtr->clear();
  m_rightLineYXTmpVecSPtr->clear();
 
  // XY
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::cluster_line_object, this, m_linesObject->leftLine.getPointsXY(), m_leftLineXYTmpVecSPtr));
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::cluster_line_object, this, m_linesObject->centerLine.getPointsXY(), m_centerLineXYTmpVecSPtr));
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::cluster_line_object, this, m_linesObject->rightLine.getPointsXY(), m_rightLineXYTmpVecSPtr));
  m_sharedThreadMutex.lock(); // wait for threads to complete
  m_sharedThreadMutex.unlock(); // wait for threads to complete
 
  // YX (switched)
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::cluster_line_object, this, m_linesObject->leftLine.getPointsYX(), m_leftLineYXTmpVecSPtr));
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::cluster_line_object, this, m_linesObject->centerLine.getPointsYX(), m_centerLineYXTmpVecSPtr));
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::cluster_line_object, this, m_linesObject->rightLine.getPointsYX(), m_rightLineYXTmpVecSPtr));
  m_sharedThreadMutex.lock(); // wait for threads to complete
  m_sharedThreadMutex.unlock(); // wait for threads to complete
 
  // Get center points of right/left line
  auto leftLaneMutVecPointer  = std::make_shared<MutexedVectorPointer<galaxis::lina::Vector2f>>(&lanesPairVecUPtr->first);
  auto rightLaneMutVecPointer = std::make_shared<MutexedVectorPointer<galaxis::lina::Vector2f>>(&lanesPairVecUPtr->second);
 
 
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::get_lane_centers_thread_helper, this, m_linesObject->reverseRotationMatrix, m_leftLineXYTmpVecSPtr, m_centerLineXYTmpVecSPtr, m_rightLineXYTmpVecSPtr, leftLaneMutVecPointer, rightLaneMutVecPointer, false));
  
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::get_lane_centers_thread_helper, this, m_linesObject->reverseRotationMatrix, m_leftLineYXTmpVecSPtr, m_centerLineYXTmpVecSPtr, m_rightLineYXTmpVecSPtr, leftLaneMutVecPointer, rightLaneMutVecPointer, true));
 
  // improve lanecenter calculation if center line has not enough points
  // if (m_centerLineYXTmpVecSPtr->size() < m_minNumberOfClusteredLinePoints) {
    // polynom + orthogonal calculation
  m_sharedThreadMutex.lock_shared();
  boost::asio::post(m_threadPool, std::bind(&AutoPilot::get_lane_centers_via_polynom_thread_helper, this, m_linesObject->reverseRotationMatrix, m_leftLineXYTmpVecSPtr, m_centerLineXYTmpVecSPtr, m_rightLineXYTmpVecSPtr, leftLaneMutVecPointer, rightLaneMutVecPointer));
  // }
 
  m_sharedThreadMutex.lock(); // wait for threads to complete
  m_sharedThreadMutex.unlock(); // wait for threads to complete
 
 
  return std::move(lanesPairVecUPtr);
}
 
void AutoPilot::cluster_line_object(std::map<int16_t, std::vector<int16_t>>& linePointsMapInputRef, std::shared_ptr<std::map<int16_t, int16_t>> linePointsOutVecSPtr) {
  // CLUSTERING PARAMETERS // TODO: evaluate // MOVED TO SYSTEM.JSON
  // int clusterEpsCM  = 2; // must be positive
  // int clusterMinPts = 2;
 
  // TODO: maybe use only median (higher performance)
  for(auto& [xKey, xVec]: linePointsMapInputRef) { // iterate through all points of specific line
    if(xVec.size() < 1)
      continue;
 
    // sort ascending
    std::sort(xVec.begin(), xVec.end()); // TODO: check performance
 
    int amountOfPoints     = 0;
    int16_t yMedianOfxVec = xVec.at(static_cast<int>((xVec.size() + 1) / 2) - 1);
 
    for(int16_t i = xKey - m_lineClusterEps; i <= xKey + m_lineClusterEps; ++i) { // iterate over possible points in Eps (X)
      auto iPointsVecIterator = linePointsMapInputRef.find(i);
 
      if(iPointsVecIterator != linePointsMapInputRef.end()) { // if there exist Y points for the X
        for(auto yPoint: iPointsVecIterator->second) { // iterate through all Y points and check if they are in Eps (Y)
 
          if(yPoint >= yMedianOfxVec - m_lineClusterEps && yPoint <= yMedianOfxVec + m_lineClusterEps) {
            ++amountOfPoints;
          }
        }
      }
 
      if(amountOfPoints >= m_lineClusterMinPts)
        break; // boost
    }
 
    if(amountOfPoints >= m_lineClusterMinPts) {
      linePointsOutVecSPtr->insert(std::pair<int16_t,int16_t>(xKey, yMedianOfxVec));
    }
  }
 
  m_sharedThreadMutex.unlock_shared();
}
 
void AutoPilot::get_lane_centers_thread_helper(Eigen::Rotation2Df p_reverseRotationMat,
                                               std::shared_ptr<std::map<int16_t, int16_t>> p_leftLinePointsMapSPtr,
                                               std::shared_ptr<std::map<int16_t, int16_t>> p_centerLinePointsMapSPtr,
                                               std::shared_ptr<std::map<int16_t, int16_t>> p_rightLinePointsMapSPtr,
                                               std::shared_ptr<MutexedVectorPointer<galaxis::lina::Vector2f>> p_leftLaneMutexedVecSPtr,
                                               std::shared_ptr<MutexedVectorPointer<galaxis::lina::Vector2f>> p_rightLaneMutexedVecSPtr,
                                               bool p_yxMapping) {
  
 
  // Iterate Left (left-center, left-right)
  for(auto const& leftPointPairRef: *p_leftLinePointsMapSPtr) {
    auto centerPointPairIt = p_centerLinePointsMapSPtr->find(leftPointPairRef.first);
    if(centerPointPairIt != p_centerLinePointsMapSPtr->end()) { // found centerPoint
      float difference = centerPointPairIt->second - leftPointPairRef.second;
 
      this->insert_lane_point(p_leftLaneMutexedVecSPtr, leftPointPairRef.first, (leftPointPairRef.second + difference * 0.5f), p_reverseRotationMat, p_yxMapping);
      this->insert_lane_point(p_rightLaneMutexedVecSPtr, leftPointPairRef.first, (leftPointPairRef.second + difference * 1.5f), p_reverseRotationMat, p_yxMapping);
    }
 
    auto rightPointPairIt = p_rightLinePointsMapSPtr->find(leftPointPairRef.first);
    if(rightPointPairIt != p_rightLinePointsMapSPtr->end()) { // found rightPoint
      float difference = rightPointPairIt->second - leftPointPairRef.second;
 
      this->insert_lane_point(p_leftLaneMutexedVecSPtr, leftPointPairRef.first, (leftPointPairRef.second + difference * 0.25f), p_reverseRotationMat, p_yxMapping);
      this->insert_lane_point(p_rightLaneMutexedVecSPtr, leftPointPairRef.first, (leftPointPairRef.second + difference * 0.75f), p_reverseRotationMat, p_yxMapping);
    }
  }
 
  // Iterate Center (center-right)
  for(auto const& centerPointPairRef: *p_centerLinePointsMapSPtr) {
    auto rightPointPairIt = p_rightLinePointsMapSPtr->find(centerPointPairRef.first);
    if(rightPointPairIt != p_rightLinePointsMapSPtr->end()) { // found rightPoint
      float difference = rightPointPairIt->second - centerPointPairRef.second;
 
      this->insert_lane_point(p_leftLaneMutexedVecSPtr, centerPointPairRef.first, (centerPointPairRef.second - difference * 0.5f), p_reverseRotationMat, p_yxMapping);
      this->insert_lane_point(p_rightLaneMutexedVecSPtr, centerPointPairRef.first, (centerPointPairRef.second + difference * 0.5f), p_reverseRotationMat, p_yxMapping);
    }
  }
 
  m_sharedThreadMutex.unlock_shared();
}
 
void AutoPilot::get_lane_centers_via_polynom_thread_helper(Eigen::Rotation2Df p_reverseRotationMat,
                                               std::shared_ptr<std::map<int16_t, int16_t>> p_leftLinePointsMapSPtr,
                                               std::shared_ptr<std::map<int16_t, int16_t>> p_centerLinePointsMapSPtr,
                                               std::shared_ptr<std::map<int16_t, int16_t>> p_rightLinePointsMapSPtr,
                                               std::shared_ptr<MutexedVectorPointer<galaxis::lina::Vector2f>> p_leftLaneMutexedVecSPtr,
                                               std::shared_ptr<MutexedVectorPointer<galaxis::lina::Vector2f>> p_rightLaneMutexedVecSPtr) {
  
  
  m_leftLinePolynomXVectorSPtr->clear();
  m_leftLinePolynomYVectorSPtr->clear();
  m_centerLinePolynomXVectorSPtr->clear();
  m_centerLinePolynomYVectorSPtr->clear();
  m_rightLinePolynomXVectorSPtr->clear();
  m_rightLinePolynomYVectorSPtr->clear();
 
  m_leftLinePolynomPointsVecSPtr->clear();
  m_rightLinePolynomPointsVecSPtr->clear();
 
  m_leftLinePoly = nullptr;
  m_rightLinePoly = nullptr;
  m_centerLinePoly = nullptr;
 
  Vector2f point1, point2, pointsDistanceVector, newPointToInsert, newPointToInsertLeft, newPointToInsertRight, toLaneCenterVector;
  Eigen::Rotation2Df rotationRightLine(MATH_PI_DOUBLE / 2);
  Eigen::Rotation2Df rotationLeftLine (-MATH_PI_DOUBLE / 2);
 
  if(p_leftLinePointsMapSPtr->size() > 25) {
    // Iterate Left
    int leftMin = 10000000;
    int leftMax = 0;
    for(auto const& pointRef: *p_leftLinePointsMapSPtr) {
      Vector2f point = Vector2f(pointRef.first, pointRef.second);
      point = p_reverseRotationMat * point;
      m_leftLinePolynomXVectorSPtr->push_back(point[1]); // looking in X direction not Y
      m_leftLinePolynomYVectorSPtr->push_back(-point[0]);
 
      if (point[1] < leftMin) { // point[1] because we want to look in X direction
        leftMin = point[1];
      }
      if (point[1] > leftMax) { // point[1] because we want to look in X direction
        leftMax = point[1];
      }
 
      m_leftPolyMax = leftMax;
      m_leftPolyMin = leftMin;
    }
 
    m_leftLinePoly = galaxis_polyfit(*m_leftLinePolynomXVectorSPtr, *m_leftLinePolynomYVectorSPtr, m_leftLinePolynomXVectorSPtr->size() * 0.7, 10, 7);
 
    for(int x = leftMin; x < m_mapDataLayer.m_lookupWindowSizeCMHeight - m_linePolynomEvaluationStepSizeCM && x < leftMax - m_linePolynomEvaluationStepSizeCM; x += m_linePolynomEvaluationStepCM) {
      // left
      point1[0] = x;
      point1[1] = galaxis::utils::evaluate_polynom(*m_leftLinePoly, point1[0]);
      point2[0] = x + m_linePolynomEvaluationStepSizeCM;
      point2[1] = galaxis::utils::evaluate_polynom(*m_leftLinePoly, point2[0]);
 
      pointsDistanceVector = point2 - point1;
      toLaneCenterVector = pointsDistanceVector.normalized(); 
      toLaneCenterVector = rotationLeftLine * toLaneCenterVector;
      
      newPointToInsert = point1 + pointsDistanceVector / 2; // ausgehend von Mitte zwischen beiden points
 
      newPointToInsertLeft  = newPointToInsert + toLaneCenterVector * 20; // 20cm to left lane center (80cm street width)
      newPointToInsertRight = newPointToInsert + toLaneCenterVector * 60; // 60cm to right lane center (80cm street width)
      p_leftLaneMutexedVecSPtr->push_back(Vector2f(-newPointToInsertLeft[1], newPointToInsertLeft[0]));
      p_rightLaneMutexedVecSPtr->push_back(Vector2f(-newPointToInsertRight[1], newPointToInsertRight[0]));
 
      // debug: show in debug image    
      // m_leftLinePolynomPointsVecSPtr->push_back(Vector2f(-newPointToInsertLeft[1], newPointToInsertLeft[0]));
      // m_leftLinePolynomPointsVecSPtr->push_back(Vector2f(-newPointToInsertRight[1], newPointToInsertRight[0]));
      
 
      // m_leftLinePolynomPointsVecSPtr->push_back(Vector2f(-point1[1],point1[0]));
    }
  }
  else {
    // log_debug("[AutoPilot] get_lane_centers_via_polynom*: Not enought points for left lane polynom");
  }
  
  if(p_centerLinePointsMapSPtr->size() > 25) {
    // Iterate Left
    int leftMin = 10000000;
    int leftMax = 0;
    for(auto const& pointRef: *p_centerLinePointsMapSPtr) {
      Vector2f point = Vector2f(pointRef.first, pointRef.second);
      point = p_reverseRotationMat * point;
      m_centerLinePolynomXVectorSPtr->push_back(point[1]); // looking in X direction not Y
      m_centerLinePolynomYVectorSPtr->push_back(-point[0]);
 
      if (point[1] < leftMin) { // point[1] because we want to look in X direction
        leftMin = point[1];
      }
      if (point[1] > leftMax) { // point[1] because we want to look in X direction
        leftMax = point[1];
      }
      m_centerPolyMax = leftMax;
      m_centerPolyMin = leftMin;
    }
 
    
    m_centerLinePoly = galaxis_polyfit(*m_centerLinePolynomXVectorSPtr, *m_centerLinePolynomYVectorSPtr, m_centerLinePolynomXVectorSPtr->size() * 0.7, 10, 7);
    
 
    for(int x = leftMin; x < m_mapDataLayer.m_lookupWindowSizeCMHeight - m_linePolynomEvaluationStepSizeCM && x < leftMax - m_linePolynomEvaluationStepSizeCM; x += m_linePolynomEvaluationStepCM) {
      // left
      point1[0] = x;
      point1[1] = galaxis::utils::evaluate_polynom(*m_centerLinePoly, point1[0]);
      point2[0] = x + m_linePolynomEvaluationStepSizeCM;
      point2[1] = galaxis::utils::evaluate_polynom(*m_centerLinePoly, point2[0]);
 
      pointsDistanceVector = point2 - point1;
      toLaneCenterVector = pointsDistanceVector.normalized(); 
      toLaneCenterVector = rotationLeftLine * toLaneCenterVector;
      
      newPointToInsert = point1 + pointsDistanceVector / 2; // ausgehend von Mitte zwischen beiden points
 
      newPointToInsertLeft  = newPointToInsert + toLaneCenterVector * -20; // 20cm to left lane center (80cm street width)
      newPointToInsertRight = newPointToInsert + toLaneCenterVector * 20; // 60cm to right lane center (80cm street width)
      p_leftLaneMutexedVecSPtr->push_back(Vector2f(-newPointToInsertLeft[1], newPointToInsertLeft[0]));
      p_rightLaneMutexedVecSPtr->push_back(Vector2f(-newPointToInsertRight[1], newPointToInsertRight[0]));
 
      // debug: show in debug image    
      // m_leftLinePolynomPointsVecSPtr->push_back(Vector2f(-newPointToInsertLeft[1], newPointToInsertLeft[0]));
      // m_leftLinePolynomPointsVecSPtr->push_back(Vector2f(-newPointToInsertRight[1], newPointToInsertRight[0]));
      
 
      // m_leftLinePolynomPointsVecSPtr->push_back(Vector2f(-point1[1],point1[0]));
    }
  }
  else {
    // log_debug("[AutoPilot] get_lane_centers_via_polynom*: Not enought points for left lane polynom");
  }
  
  if(p_rightLinePointsMapSPtr->size() > 25) {
    // Iterate Right
    int rightMin = 10000000;
    int rightMax = 0;
    for(auto const& pointRef: *p_rightLinePointsMapSPtr) {
      Vector2f point = Vector2f(pointRef.first, pointRef.second);
      point = p_reverseRotationMat * point;
      m_rightLinePolynomXVectorSPtr->push_back(point[1]);
      m_rightLinePolynomYVectorSPtr->push_back(-point[0]);
 
      if (point[1] < rightMin) { // point[1] because we want to look in X direction
        rightMin = point[1];
      }
      if (point[1] > rightMax) { // point[1] because we want to look in X direction
        rightMax = point[1];
      }
 
      m_rightPolyMax = rightMax;
      m_rightPolyMin = rightMin;
    }
    m_rightLinePoly = galaxis_polyfit(*m_rightLinePolynomXVectorSPtr, *m_rightLinePolynomYVectorSPtr, m_rightLinePolynomXVectorSPtr->size() * 0.7, 10, 7);
 
    for(int x = rightMin; x < m_mapDataLayer.m_lookupWindowSizeCMHeight - m_linePolynomEvaluationStepSizeCM && x < rightMax - m_linePolynomEvaluationStepSizeCM; x += m_linePolynomEvaluationStepCM) {
      // right
      point1[0] = x;
      point1[1] = galaxis::utils::evaluate_polynom(*m_rightLinePoly, point1[0]);
      point2[0] = x + m_linePolynomEvaluationStepSizeCM;
      point2[1] = galaxis::utils::evaluate_polynom(*m_rightLinePoly, point2[0]);
 
      pointsDistanceVector = point2 - point1;
      toLaneCenterVector = pointsDistanceVector.normalized();
      toLaneCenterVector = rotationRightLine * toLaneCenterVector;
      
      newPointToInsert = point1 + pointsDistanceVector / 2; // ausgehend von Mitte zwischen beiden points
 
      newPointToInsertLeft  = newPointToInsert + toLaneCenterVector * 60; // 60cm to left lane center (80cm street width)
      newPointToInsertRight = newPointToInsert + toLaneCenterVector * 20; // 20cm to right lane center (80cm street width)
 
      p_leftLaneMutexedVecSPtr->push_back(Vector2f(-newPointToInsertLeft[1], newPointToInsertLeft[0]));
      p_rightLaneMutexedVecSPtr->push_back(Vector2f(-newPointToInsertRight[1], newPointToInsertRight[0])); 
 
      // m_rightLinePolynomPointsVecSPtr->push_back(Vector2f(-point1[1],point1[0]));
    }
  }
  else {
    // log_debug("[AutoPilot] get_lane_centers_via_polynom*: Not enought points for right lane polynom");
  }
 
  // p_leftLaneMutexedVecSPtr
  // p_rightLaneMutexedVecSPtr
 
  m_sharedThreadMutex.unlock_shared();
}
 
void AutoPilot::insert_lane_point(std::shared_ptr<MutexedVectorPointer<galaxis::lina::Vector2f>> p_mutVecPointerSPtr,
                       float p_x,
                       float p_y,
                       Eigen::Rotation2Df const& p_rotationMat,
                       bool p_reverseInsertion) {
  
  Vector2f point;
  if(!p_reverseInsertion) { // for normal insertion
    point[0] = p_x;
    point[1] = p_y;
  }
  else { // for reverse insertion
    point[1] = p_x;
    point[0] = p_y;
  }
 
  // rotate Point
  point = p_rotationMat * point;
 
  // insert
  p_mutVecPointerSPtr->push_back(point);
}
 
std::tuple<float, float, float, float> AutoPilot::get_lane_blocked(std::pair<DynamicPolynom, DynamicPolynom>& lanePolynom) {
  std::tuple<float, float, float, float> laneBlocked = std::make_tuple<float, float, float>(-1, -1, -1, -1); // left, right, intersection-right lane, 
 
  std::shared_ptr<PositionTimepoint> positionTimepointNow = m_localizer.get_latest_position_timepoint(); // TODO: check if we use simulation -> other clock
  auto staticObstaclesVectorSPtr = m_mapDataLayer.getObstacles(positionTimepointNow->timestamp);
 
  for(auto& obstacle : *staticObstaclesVectorSPtr) {
    Vector2f obstaclePos = obstacle.get_position_now();
    Vector2f distanceToObstacle = obstaclePos - positionTimepointNow->position / 10; // mm to cm
    if(distanceToObstacle.norm() <= m_obstacleLookupDistanceCM) {
      
      Eigen::Rotation2Df rotationMat(positionTimepointNow->orientation);
      distanceToObstacle = rotationMat * distanceToObstacle;
 
      if(distanceToObstacle[1] >= 0) {
        // obstacle is in 180deg FOV -> check if line blocked
        double x;
        Vector2f laneToObstacleDistance;
        for(double y = 0; y < m_obstacleLookupDistanceCM; y+= m_obstacleLookupDistanceStepCM) {
          if(std::get<0>(laneBlocked) < 0) {
            x = -eval_poly(std::get<0>(lanePolynom), y / 100 /* to m */) * 100 /* to cm */; // x,y flipped because of polynom look into x direction => *(-1)
            laneToObstacleDistance[0] = x - distanceToObstacle[0];
            laneToObstacleDistance[1] = y - distanceToObstacle[1];
            if(laneToObstacleDistance.norm() - obstacle.get_width() / 2 < m_obstacleToLaneCenterMinimumDistanceForDodge) {
              // lane blocked
              std::get<0>(laneBlocked) = y;
            }
          }
 
          // same for right lane
          if(std::get<1>(laneBlocked) < 0) {
            x = -eval_poly(std::get<1>(lanePolynom), y / 100) * 100; // x,y flipped because of polynom look into x direction
            laneToObstacleDistance[0] = x - distanceToObstacle[0];
            laneToObstacleDistance[1] = y - distanceToObstacle[1];
            if(laneToObstacleDistance.norm() - obstacle.get_width() / 2 < m_obstacleToLaneCenterMinimumDistanceForDodge) {
              // lane blocked
              std::get<1>(laneBlocked) = y;
            }
          }
        }
 
        // check if obstacle at right lane of intersection
        if(distanceToObstacle[0] >= 10 && distanceToObstacle[0] <= 120) {
          if(std::get<2>(laneBlocked) < 0.f || std::get<2>(laneBlocked) > distanceToObstacle[1]) {
            std::get<2>(laneBlocked) = distanceToObstacle[1];
          }
        }
        if(distanceToObstacle[1] <= 120) {
          if(std::get<3>(laneBlocked) < 0.f || std::get<3>(laneBlocked) > distanceToObstacle[1]) {
            std::get<3>(laneBlocked) = distanceToObstacle[1];
          }
        }
      }
    }
  }
 
  return laneBlocked;
}
 
double AutoPilot::eval_poly(DynamicPolynom p_polynom, double p_x) {
  double value = 0.0;
  double iValue;
  for(size_t i = 0; i < p_polynom->size(); ++i) {
    iValue = 1;
    // x^i
    for(size_t j = 0; j < i; ++j) {
      iValue *= p_x;
    }
    value += p_polynom->at(i) * iValue;
  }
  return value;
}