AutoPilot Class Reference

#include <AutoPilot.hpp>

Collaboration diagram for AutoPilot:

Public Member Functions
	AutoPilot ()=delete
	AutoPilot (GalaxisNode *p_gNodePtr)
	~AutoPilot ()
void	set_obstacle_lookup_distance (int value)
void	set_obstacle_lookup_distance_step (float value)
void	set_obstacle_to_lane_center_min_distance_for_dodge (float value)
void	set_polynom_degree (int value)
void	set_polynom_min_points_amount (int value)
void	set_traj_points_to_append_amount (int value)
void	set_traj_points_start_appending_offset (int value)
void	set_line_cluster_eps (int value)
void	set_line_cluster_min_pts (int value)
void	set_line_polynom_evaluation_step (int value)
void	set_line_polynom_evaluation_step_size (int value)
void	set_min_number_of_clustered_line_points (int value)
std::shared_ptr< DriveInformation_t >	get_drive_information ()
double	eval_poly (DynamicPolynom p_polynom, double p_x)

Public Attributes
MapDataLayer	m_mapDataLayer
Localizer	m_localizer
Decision	m_decision
std::shared_ptr< LinesObject< int16_t > >	m_linesObject
std::unique_ptr< std::pair< std::vector< galaxis::lina::Vector2f >, std::vector< galaxis::lina::Vector2f > > >	m_laneCentersPairVecUPtr
std::pair< DynamicPolynom, DynamicPolynom >	m_lanePolynom
std::shared_ptr< DriveInformation_t >	m_lastDriveInformationSPtr
std::vector< double >	m_polynomPointsXVector
std::vector< double >	m_polynomPointsYVector
int	m_obstacleLookupDistanceCM = 200
int	m_obstacleLookupDistanceStepCM = 5
int	m_obstacleToLaneCenterMinimumDistanceForDodge = 10
int	m_polynomDegree = 3
int	m_polynomMinimumPointsAmount = 30
int	m_trajPointsToAppendAmount = 5
int	m_trajPointsStartAppendingOffset = 20
int	m_lineClusterEps = 2
int	m_lineClusterMinPts = 2
int	m_linePolynomEvaluationStepCM = 1
int	m_linePolynomEvaluationStepSizeCM = 8
int	m_minNumberOfClusteredLinePoints = 20
std::shared_ptr< std::vector< Vector2f > >	m_leftLinePolynomPointsVecSPtr
std::shared_ptr< std::vector< Vector2f > >	m_rightLinePolynomPointsVecSPtr

Private Member Functions
void	cluster_line_object (std::map< int16_t, std::vector< int16_t >> &linePointsMapInputRef, std::shared_ptr< std::map< int16_t, int16_t >> linePointsOutVecSPtr)
std::unique_ptr< std::pair< std::vector< galaxis::lina::Vector2f >, std::vector< galaxis::lina::Vector2f > > >	get_lane_centers (std::shared_ptr< PositionTimepoint > positionTimepoint)
std::unique_ptr< std::pair< std::vector< galaxis::lina::Vector2f >, std::vector< galaxis::lina::Vector2f > > >	get_lane_center ()
void	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)
void	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)
void	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)
std::tuple< float, float, float, float >	get_lane_blocked (std::pair< DynamicPolynom, DynamicPolynom > &lanePolynom)

Private Attributes
GalaxisNode *	m_gNodePtr
boost::asio::thread_pool	m_threadPool
std::shared_mutex	m_sharedThreadMutex
std::shared_ptr< std::map< int16_t, int16_t > >	m_leftLineXYTmpVecSPtr
std::shared_ptr< std::map< int16_t, int16_t > >	m_centerLineXYTmpVecSPtr
std::shared_ptr< std::map< int16_t, int16_t > >	m_rightLineXYTmpVecSPtr
std::shared_ptr< std::map< int16_t, int16_t > >	m_leftLineYXTmpVecSPtr
std::shared_ptr< std::map< int16_t, int16_t > >	m_centerLineYXTmpVecSPtr
std::shared_ptr< std::map< int16_t, int16_t > >	m_rightLineYXTmpVecSPtr
std::shared_ptr< std::vector< double > >	m_leftLinePolynomXVectorSPtr
std::shared_ptr< std::vector< double > >	m_leftLinePolynomYVectorSPtr
std::shared_ptr< std::vector< double > >	m_centerLinePolynomXVectorSPtr
std::shared_ptr< std::vector< double > >	m_centerLinePolynomYVectorSPtr
std::shared_ptr< std::vector< double > >	m_rightLinePolynomXVectorSPtr
std::shared_ptr< std::vector< double > >	m_rightLinePolynomYVectorSPtr
std::tuple< float, float, float, float >	m_laneBlocked
double	m_leftPolyMax
double	m_leftPolyMin
std::shared_ptr< std::vector< double > >	m_leftLinePoly
double	m_centerPolyMax
double	m_centerPolyMin
std::shared_ptr< std::vector< double > >	m_centerLinePoly
double	m_rightPolyMax
double	m_rightPolyMin
std::shared_ptr< std::vector< double > >	m_rightLinePoly
rclcpp::Time	m_lastTimeLaneVisible

Detailed Description

Definition at line 46 of file AutoPilot.hpp.

Constructor & Destructor Documentation

AutoPilot() [1/2]

AutoPilot::AutoPilot ( )

delete

AutoPilot() [2/2]

AutoPilot::AutoPilot

(

GalaxisNode *

p_gNodePtr

)

Definition at line 6 of file AutoPilot.cpp.

                                            :
    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);
}

References m_centerLinePolynomXVectorSPtr, m_centerLinePolynomYVectorSPtr, m_centerLineXYTmpVecSPtr, m_centerLineYXTmpVecSPtr, m_gNodePtr, m_laneBlocked, m_lastDriveInformationSPtr, m_leftLinePolynomPointsVecSPtr, m_leftLinePolynomXVectorSPtr, m_leftLinePolynomYVectorSPtr, m_leftLineXYTmpVecSPtr, m_leftLineYXTmpVecSPtr, m_polynomPointsXVector, m_polynomPointsYVector, m_rightLinePolynomPointsVecSPtr, m_rightLinePolynomXVectorSPtr, m_rightLinePolynomYVectorSPtr, m_rightLineXYTmpVecSPtr, and m_rightLineYXTmpVecSPtr.

~AutoPilot()

AutoPilot::~AutoPilot

(

)

Definition at line 44 of file AutoPilot.cpp.

                      {
  m_threadPool.join();
}

References m_threadPool.

Member Function Documentation

cluster_line_object()

void AutoPilot::cluster_line_object ( std::map< int16_t, std::vector< int16_t >> &  linePointsMapInputRef, std::shared_ptr< std::map< int16_t, int16_t >>  linePointsOutVecSPtr  )

private

Definition at line 289 of file AutoPilot.cpp.

                                                                                                                                                            {
  // 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();
}

References m_lineClusterEps, m_lineClusterMinPts, and m_sharedThreadMutex.

Referenced by get_lane_centers().

Here is the caller graph for this function:

eval_poly()

double AutoPilot::eval_poly	(	DynamicPolynom	p_polynom,
		double	p_x
	)

Definition at line 640 of file AutoPilot.cpp.

                                                                {
  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;
}

Referenced by get_drive_information(), get_lane_blocked(), and AutoPilotNode::send_map_debug_image().

Here is the caller graph for this function:

get_drive_information()

std::shared_ptr< DriveInformation_t > AutoPilot::get_drive_information

(

)

Definition at line 87 of file AutoPilot.cpp.

                                                                   { // 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;
}

References galaxis::utils::calculate_gradient(), eval_poly(), Decision::get_chosen_lane(), Decision::get_current_situationstate(), Decision::get_indicator_left(), Decision::get_indicator_right(), get_lane_blocked(), get_lane_centers(), Localizer::get_latest_position_timepoint(), Decision::get_velocity_reference(), galaxis::utils::log_warning(), m_centerLinePoly, m_decision, m_laneBlocked, m_laneCentersPairVecUPtr, m_lanePolynom, m_lastDriveInformationSPtr, m_leftLinePoly, m_localizer, MapDataLayer::m_lookupWindowSizeCMHeight, m_mapDataLayer, m_polynomMinimumPointsAmount, m_polynomPointsXVector, m_polynomPointsYVector, m_rightLinePoly, m_trajPointsStartAppendingOffset, m_trajPointsToAppendAmount, Parking, ParkingZone, and Decision::update().

Referenced by AutoPilotNode::send_controller_references().

Here is the call graph for this function:

Here is the caller graph for this function:

get_lane_blocked()

std::tuple< float, float, float, float > AutoPilot::get_lane_blocked ( std::pair< DynamicPolynom, DynamicPolynom > &  lanePolynom )

private

Definition at line 581 of file AutoPilot.cpp.

                                                                                                                     {
  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;
}

References eval_poly(), Localizer::get_latest_position_timepoint(), MapDataLayer::getObstacles(), m_localizer, m_mapDataLayer, m_obstacleLookupDistanceCM, m_obstacleLookupDistanceStepCM, and m_obstacleToLaneCenterMinimumDistanceForDodge.

Referenced by get_drive_information().

Here is the call graph for this function:

Here is the caller graph for this function:

get_lane_center()

std::unique_ptr<std::pair<std::vector<galaxis::lina::Vector2f>, std::vector<galaxis::lina::Vector2f> > > AutoPilot::get_lane_center ( )

private

get_lane_centers()

std::unique_ptr< std::pair< std::vector< galaxis::lina::Vector2f >, std::vector< galaxis::lina::Vector2f > > > AutoPilot::get_lane_centers ( std::shared_ptr< PositionTimepoint >  positionTimepoint )

private

Definition at line 231 of file AutoPilot.cpp.

                                                                                                                                                                     { // 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);
}

References cluster_line_object(), get_lane_centers_thread_helper(), get_lane_centers_via_polynom_thread_helper(), MapDataLayer::getLineObject(), galaxis::autopilot::lookupLookBehindCarCM, m_centerLineXYTmpVecSPtr, m_centerLineYXTmpVecSPtr, m_leftLineXYTmpVecSPtr, m_leftLineYXTmpVecSPtr, m_linesObject, MapDataLayer::m_lookupWindowSizeCMHeight, MapDataLayer::m_lookupWindowSizeCMWidth, m_mapDataLayer, m_rightLineXYTmpVecSPtr, m_rightLineYXTmpVecSPtr, m_sharedThreadMutex, and m_threadPool.

Referenced by get_drive_information().

Here is the call graph for this function:

Here is the caller graph for this function:

get_lane_centers_thread_helper()

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  )

private

Definition at line 329 of file AutoPilot.cpp.

                                                                 {
  
 
  // 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();
}

References insert_lane_point(), and m_sharedThreadMutex.

Referenced by get_lane_centers().

Here is the call graph for this function:

Here is the caller graph for this function:

get_lane_centers_via_polynom_thread_helper()

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  )

private

Definition at line 371 of file AutoPilot.cpp.

                                                                                                                                   {
  
  
  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();
}

References galaxis::utils::evaluate_polynom(), m_centerLinePoly, m_centerLinePolynomXVectorSPtr, m_centerLinePolynomYVectorSPtr, m_centerPolyMax, m_centerPolyMin, m_leftLinePoly, m_leftLinePolynomPointsVecSPtr, m_leftLinePolynomXVectorSPtr, m_leftLinePolynomYVectorSPtr, m_leftPolyMax, m_leftPolyMin, m_linePolynomEvaluationStepCM, m_linePolynomEvaluationStepSizeCM, MapDataLayer::m_lookupWindowSizeCMHeight, m_mapDataLayer, m_rightLinePoly, m_rightLinePolynomPointsVecSPtr, m_rightLinePolynomXVectorSPtr, m_rightLinePolynomYVectorSPtr, m_rightPolyMax, m_rightPolyMin, m_sharedThreadMutex, and MATH_PI_DOUBLE.

Referenced by get_lane_centers().

Here is the call graph for this function:

Here is the caller graph for this function:

insert_lane_point()

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  )

inlineprivate

Definition at line 558 of file AutoPilot.cpp.

                                                {
  
  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);
}

Referenced by get_lane_centers_thread_helper().

Here is the caller graph for this function:

set_line_cluster_eps()

void AutoPilot::set_line_cluster_eps

(

int

value

)

Definition at line 70 of file AutoPilot.cpp.

                                             {
  m_lineClusterEps = value;
}

References m_lineClusterEps.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_line_cluster_min_pts()

void AutoPilot::set_line_cluster_min_pts

(

int

value

)

Definition at line 73 of file AutoPilot.cpp.

                                                 {
  m_lineClusterMinPts = value;
}

References m_lineClusterMinPts.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_line_polynom_evaluation_step()

void AutoPilot::set_line_polynom_evaluation_step

(

int

value

)

Definition at line 76 of file AutoPilot.cpp.

                                                          {
  m_linePolynomEvaluationStepCM = value;
}

References m_linePolynomEvaluationStepCM.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_line_polynom_evaluation_step_size()

void AutoPilot::set_line_polynom_evaluation_step_size

(

int

value

)

Definition at line 79 of file AutoPilot.cpp.

                                                              {
  m_linePolynomEvaluationStepSizeCM = value;
}

References m_linePolynomEvaluationStepSizeCM.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_min_number_of_clustered_line_points()

void AutoPilot::set_min_number_of_clustered_line_points

(

int

value

)

Definition at line 82 of file AutoPilot.cpp.

                                                                {
  m_minNumberOfClusteredLinePoints = value;
}

References m_minNumberOfClusteredLinePoints.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_obstacle_lookup_distance()

void AutoPilot::set_obstacle_lookup_distance

(

int

value

)

Definition at line 49 of file AutoPilot.cpp.

                                                      {
  m_obstacleLookupDistanceCM = value;
}

References m_obstacleLookupDistanceCM.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_obstacle_lookup_distance_step()

void AutoPilot::set_obstacle_lookup_distance_step

(

float

value

)

Definition at line 52 of file AutoPilot.cpp.

                                                             {
  m_obstacleLookupDistanceStepCM = value;
}

References m_obstacleLookupDistanceStepCM.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_obstacle_to_lane_center_min_distance_for_dodge()

void AutoPilot::set_obstacle_to_lane_center_min_distance_for_dodge

(

float

value

)

Definition at line 55 of file AutoPilot.cpp.

                                                                              {
  m_obstacleToLaneCenterMinimumDistanceForDodge = value;
}

References m_obstacleToLaneCenterMinimumDistanceForDodge.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_polynom_degree()

void AutoPilot::set_polynom_degree

(

int

value

)

Definition at line 58 of file AutoPilot.cpp.

                                            {
  m_polynomDegree = value;
}

References m_polynomDegree.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_polynom_min_points_amount()

void AutoPilot::set_polynom_min_points_amount

(

int

value

)

Definition at line 61 of file AutoPilot.cpp.

                                                       {
  m_polynomMinimumPointsAmount = value;
}

References m_polynomMinimumPointsAmount.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_traj_points_start_appending_offset()

void AutoPilot::set_traj_points_start_appending_offset

(

int

value

)

Definition at line 67 of file AutoPilot.cpp.

                                                                {
  m_trajPointsStartAppendingOffset = value;
}

References m_trajPointsStartAppendingOffset.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

set_traj_points_to_append_amount()

void AutoPilot::set_traj_points_to_append_amount

(

int

value

)

Definition at line 64 of file AutoPilot.cpp.

                                                          {
  m_trajPointsToAppendAmount = value;
}

References m_trajPointsToAppendAmount.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

Member Data Documentation

m_centerLinePoly

std::shared_ptr<std::vector<double> > AutoPilot::m_centerLinePoly

private

Definition at line 93 of file AutoPilot.hpp.

Referenced by get_drive_information(), and get_lane_centers_via_polynom_thread_helper().

m_centerLinePolynomXVectorSPtr

std::shared_ptr<std::vector<double> > AutoPilot::m_centerLinePolynomXVectorSPtr

private

Definition at line 80 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers_via_polynom_thread_helper().

m_centerLinePolynomYVectorSPtr

std::shared_ptr<std::vector<double> > AutoPilot::m_centerLinePolynomYVectorSPtr

private

Definition at line 81 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers_via_polynom_thread_helper().

m_centerLineXYTmpVecSPtr

std::shared_ptr<std::map<int16_t, int16_t> > AutoPilot::m_centerLineXYTmpVecSPtr

private

Definition at line 70 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers().

m_centerLineYXTmpVecSPtr

std::shared_ptr<std::map<int16_t, int16_t> > AutoPilot::m_centerLineYXTmpVecSPtr

private

Definition at line 73 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers().

m_centerPolyMax

double AutoPilot::m_centerPolyMax

private

Definition at line 91 of file AutoPilot.hpp.

Referenced by get_lane_centers_via_polynom_thread_helper().

m_centerPolyMin

double AutoPilot::m_centerPolyMin

private

Definition at line 92 of file AutoPilot.hpp.

Referenced by get_lane_centers_via_polynom_thread_helper().

m_decision

Decision AutoPilot::m_decision

Definition at line 51 of file AutoPilot.hpp.

Referenced by AutoPilotNode::AutoPilotNode(), AutoPilotNode::debug_publish_loop(), get_drive_information(), AutoPilotNode::new_parameters_callback(), AutoPilotNode::on_ecu_measurement_callback(), AutoPilotNode::on_parking_state_callback(), AutoPilotNode::on_qr_code_callback(), AutoPilotNode::on_simulation_pose_callback(), and AutoPilotNode::on_stop_line_callback().

m_gNodePtr

GalaxisNode* AutoPilot::m_gNodePtr

private

Definition at line 63 of file AutoPilot.hpp.

Referenced by AutoPilot().

m_laneBlocked

std::tuple<float, float, float, float> AutoPilot::m_laneBlocked

private

Definition at line 86 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_drive_information().

m_laneCentersPairVecUPtr

std::unique_ptr<std::pair<std::vector<galaxis::lina::Vector2f>, std::vector<galaxis::lina::Vector2f> > > AutoPilot::m_laneCentersPairVecUPtr

Definition at line 54 of file AutoPilot.hpp.

Referenced by get_drive_information(), and AutoPilotNode::send_map_debug_image().

m_lanePolynom

std::pair<DynamicPolynom, DynamicPolynom> AutoPilot::m_lanePolynom

Definition at line 55 of file AutoPilot.hpp.

Referenced by get_drive_information().

m_lastDriveInformationSPtr

std::shared_ptr<DriveInformation_t> AutoPilot::m_lastDriveInformationSPtr

Definition at line 57 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_drive_information().

m_lastTimeLaneVisible

rclcpp::Time AutoPilot::m_lastTimeLaneVisible

private

Definition at line 98 of file AutoPilot.hpp.

m_leftLinePoly

std::shared_ptr<std::vector<double> > AutoPilot::m_leftLinePoly

private

Definition at line 90 of file AutoPilot.hpp.

Referenced by get_drive_information(), and get_lane_centers_via_polynom_thread_helper().

m_leftLinePolynomPointsVecSPtr

std::shared_ptr<std::vector<Vector2f> > AutoPilot::m_leftLinePolynomPointsVecSPtr

Definition at line 122 of file AutoPilot.hpp.

Referenced by AutoPilot(), get_lane_centers_via_polynom_thread_helper(), and AutoPilotNode::send_map_debug_image().

m_leftLinePolynomXVectorSPtr

std::shared_ptr<std::vector<double> > AutoPilot::m_leftLinePolynomXVectorSPtr

private

Definition at line 77 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers_via_polynom_thread_helper().

m_leftLinePolynomYVectorSPtr

std::shared_ptr<std::vector<double> > AutoPilot::m_leftLinePolynomYVectorSPtr

private

Definition at line 78 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers_via_polynom_thread_helper().

m_leftLineXYTmpVecSPtr

std::shared_ptr<std::map<int16_t, int16_t> > AutoPilot::m_leftLineXYTmpVecSPtr

private

Definition at line 69 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers().

m_leftLineYXTmpVecSPtr

std::shared_ptr<std::map<int16_t, int16_t> > AutoPilot::m_leftLineYXTmpVecSPtr

private

Definition at line 72 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers().

m_leftPolyMax

double AutoPilot::m_leftPolyMax

private

Definition at line 88 of file AutoPilot.hpp.

Referenced by get_lane_centers_via_polynom_thread_helper().

m_leftPolyMin

double AutoPilot::m_leftPolyMin

private

Definition at line 89 of file AutoPilot.hpp.

Referenced by get_lane_centers_via_polynom_thread_helper().

m_lineClusterEps

int AutoPilot::m_lineClusterEps = 2

Definition at line 113 of file AutoPilot.hpp.

Referenced by cluster_line_object(), and set_line_cluster_eps().

m_lineClusterMinPts

int AutoPilot::m_lineClusterMinPts = 2

Definition at line 114 of file AutoPilot.hpp.

Referenced by cluster_line_object(), and set_line_cluster_min_pts().

m_linePolynomEvaluationStepCM

int AutoPilot::m_linePolynomEvaluationStepCM = 1

Definition at line 116 of file AutoPilot.hpp.

Referenced by get_lane_centers_via_polynom_thread_helper(), and set_line_polynom_evaluation_step().

m_linePolynomEvaluationStepSizeCM

int AutoPilot::m_linePolynomEvaluationStepSizeCM = 8

Definition at line 117 of file AutoPilot.hpp.

Referenced by get_lane_centers_via_polynom_thread_helper(), and set_line_polynom_evaluation_step_size().

m_linesObject

std::shared_ptr<LinesObject<int16_t> > AutoPilot::m_linesObject

Definition at line 53 of file AutoPilot.hpp.

Referenced by get_lane_centers(), and AutoPilotNode::send_map_debug_image().

m_localizer

Localizer AutoPilot::m_localizer

Definition at line 50 of file AutoPilot.hpp.

Referenced by AutoPilotNode::AutoPilotNode(), AutoPilotNode::car_pose_publish_loop(), get_drive_information(), get_lane_blocked(), AutoPilotNode::on_ecu_measurement_callback(), AutoPilotNode::on_obstacle_callback(), AutoPilotNode::on_simulation_pose_callback(), AutoPilotNode::on_stop_line_callback(), AutoPilotNode::on_vci_lane_detection_callback(), and AutoPilotNode::send_map_debug_image().

m_mapDataLayer

MapDataLayer AutoPilot::m_mapDataLayer

Definition at line 49 of file AutoPilot.hpp.

Referenced by AutoPilotNode::AutoPilotNode(), get_drive_information(), get_lane_blocked(), get_lane_centers(), get_lane_centers_via_polynom_thread_helper(), AutoPilotNode::new_parameters_callback(), AutoPilotNode::on_obstacle_callback(), AutoPilotNode::on_vci_lane_detection_callback(), AutoPilotNode::send_full_map_debug_image(), and AutoPilotNode::send_map_debug_image().

m_minNumberOfClusteredLinePoints

int AutoPilot::m_minNumberOfClusteredLinePoints = 20

Definition at line 118 of file AutoPilot.hpp.

Referenced by set_min_number_of_clustered_line_points().

m_obstacleLookupDistanceCM

int AutoPilot::m_obstacleLookupDistanceCM = 200

Definition at line 103 of file AutoPilot.hpp.

Referenced by get_lane_blocked(), and set_obstacle_lookup_distance().

m_obstacleLookupDistanceStepCM

int AutoPilot::m_obstacleLookupDistanceStepCM = 5

Definition at line 104 of file AutoPilot.hpp.

Referenced by get_lane_blocked(), and set_obstacle_lookup_distance_step().

m_obstacleToLaneCenterMinimumDistanceForDodge

int AutoPilot::m_obstacleToLaneCenterMinimumDistanceForDodge = 10

Definition at line 105 of file AutoPilot.hpp.

Referenced by get_lane_blocked(), and set_obstacle_to_lane_center_min_distance_for_dodge().

m_polynomDegree

int AutoPilot::m_polynomDegree = 3

Definition at line 107 of file AutoPilot.hpp.

Referenced by set_polynom_degree().

m_polynomMinimumPointsAmount

int AutoPilot::m_polynomMinimumPointsAmount = 30

Definition at line 108 of file AutoPilot.hpp.

Referenced by get_drive_information(), and set_polynom_min_points_amount().

m_polynomPointsXVector

std::vector<double> AutoPilot::m_polynomPointsXVector

Definition at line 59 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_drive_information().

m_polynomPointsYVector

std::vector<double> AutoPilot::m_polynomPointsYVector

Definition at line 60 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_drive_information().

m_rightLinePoly

std::shared_ptr<std::vector<double> > AutoPilot::m_rightLinePoly

private

Definition at line 96 of file AutoPilot.hpp.

Referenced by get_drive_information(), and get_lane_centers_via_polynom_thread_helper().

m_rightLinePolynomPointsVecSPtr

std::shared_ptr<std::vector<Vector2f> > AutoPilot::m_rightLinePolynomPointsVecSPtr

Definition at line 123 of file AutoPilot.hpp.

Referenced by AutoPilot(), get_lane_centers_via_polynom_thread_helper(), and AutoPilotNode::send_map_debug_image().

m_rightLinePolynomXVectorSPtr

std::shared_ptr<std::vector<double> > AutoPilot::m_rightLinePolynomXVectorSPtr

private

Definition at line 83 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers_via_polynom_thread_helper().

m_rightLinePolynomYVectorSPtr

std::shared_ptr<std::vector<double> > AutoPilot::m_rightLinePolynomYVectorSPtr

private

Definition at line 84 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers_via_polynom_thread_helper().

m_rightLineXYTmpVecSPtr

std::shared_ptr<std::map<int16_t, int16_t> > AutoPilot::m_rightLineXYTmpVecSPtr

private

Definition at line 71 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers().

m_rightLineYXTmpVecSPtr

std::shared_ptr<std::map<int16_t, int16_t> > AutoPilot::m_rightLineYXTmpVecSPtr

private

Definition at line 74 of file AutoPilot.hpp.

Referenced by AutoPilot(), and get_lane_centers().

m_rightPolyMax

double AutoPilot::m_rightPolyMax

private

Definition at line 94 of file AutoPilot.hpp.

Referenced by get_lane_centers_via_polynom_thread_helper().

m_rightPolyMin

double AutoPilot::m_rightPolyMin

private

Definition at line 95 of file AutoPilot.hpp.

Referenced by get_lane_centers_via_polynom_thread_helper().

m_sharedThreadMutex

std::shared_mutex AutoPilot::m_sharedThreadMutex

private

Definition at line 66 of file AutoPilot.hpp.

Referenced by cluster_line_object(), get_lane_centers(), get_lane_centers_thread_helper(), and get_lane_centers_via_polynom_thread_helper().

m_threadPool

boost::asio::thread_pool AutoPilot::m_threadPool

private

Definition at line 65 of file AutoPilot.hpp.

Referenced by get_lane_centers(), and ~AutoPilot().

m_trajPointsStartAppendingOffset

int AutoPilot::m_trajPointsStartAppendingOffset = 20

Definition at line 111 of file AutoPilot.hpp.

Referenced by get_drive_information(), and set_traj_points_start_appending_offset().

m_trajPointsToAppendAmount

int AutoPilot::m_trajPointsToAppendAmount = 5

Definition at line 110 of file AutoPilot.hpp.

Referenced by get_drive_information(), and set_traj_points_to_append_amount().

---

The documentation for this class was generated from the following files:

• nodes/Autopilot/include/Decision/AutoPilot.hpp
• nodes/Autopilot/src/Decision/AutoPilot.cpp