AutoPilot.hpp

Go to the documentation of this file.

#pragma once
 
#ifndef AUTOPILOT_HPP
#define AUTOPILOT_HPP
 
#include "Map/MapDataLayer.hpp"
#include "Localization/Localizer.hpp"
#include "Decision/DecisionStateMachine.hpp"
#include <galaxis/GalaxisNode.hpp>
 
#include "polyfit/galaxis_polyfit.h"
#include "galaxis/linatypes.hpp"
#include "utils/mvector.hpp"
 
#include <boost/asio.hpp>
#include <boost/asio/thread_pool.hpp>
 
#include <memory>
 
using namespace galaxis;
 
constexpr double MATH_PI_DOUBLE = 3.141592653589793238;
 
struct DriveInformation_t {
  DynamicPolynom polynom;
 
  DynamicPolynom left;
  double leftPolyMax;
  double leftPolyMin;
  DynamicPolynom center;
  double centerPolyMax;
  double centerPolyMin;
  DynamicPolynom right;
  double rightPolyMax;
  double rightPolyMin;
 
  float velocityReference = 1; // m/s
  bool indicatorLeft  = false;
  bool indicatorRight = false;
  PositionTimepoint positionTimepoint;
  rclcpp::Time timeLastPolynomCalculated;
 
  bool startParking = false;
};
 
class AutoPilot {
  // Member Vars
public:
  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;
 
private:
  GalaxisNode* m_gNodePtr;
 
  boost::asio::thread_pool m_threadPool;
  std::shared_mutex m_sharedThreadMutex;
 
  // Temporary Line Vector
  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;
 
  // Temporary Line Vector 2
  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;
 
public:
  // Settings
  // > Obstacle
  int m_obstacleLookupDistanceCM = 200;
  int m_obstacleLookupDistanceStepCM = 5;
  int m_obstacleToLaneCenterMinimumDistanceForDodge = 10;
  // > Polynom
  int m_polynomDegree = 3;
  int m_polynomMinimumPointsAmount = 30;
  // > Trajectory: append behind car a straight line
  int m_trajPointsToAppendAmount = 5; 
  int m_trajPointsStartAppendingOffset = 20;
  // > Postprocessing: Cluster Line
  int m_lineClusterEps = 2;
  int m_lineClusterMinPts = 2;
  // > Postprocessing: Lane Center Improvement with Polynom Fitting
  int m_linePolynomEvaluationStepCM = 1;
  int m_linePolynomEvaluationStepSizeCM = 8;
  int m_minNumberOfClusteredLinePoints = 20;
 
 
  // Line Polynom Points for Plotting (DEBUG)
  std::shared_ptr<std::vector<Vector2f>> m_leftLinePolynomPointsVecSPtr;
  std::shared_ptr<std::vector<Vector2f>> m_rightLinePolynomPointsVecSPtr;
 
 
// Class Functions
public:
  AutoPilot() = delete;
  AutoPilot(GalaxisNode* p_gNodePtr);
  ~AutoPilot();
 
  // ############## Settings ##############
  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();
 
private:
  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);
 
  inline 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); 
 
public:
  double eval_poly(DynamicPolynom p_polynom, double p_x);
};
 
#endif