control.hpp

Go to the documentation of this file.

#ifndef _CONTROL_HPP_
#define _CONTROL_HPP_
 
#include "utils/logger.hpp"
#include "utils/poly.hpp"
#include "utils/clock.hpp"
#include "pose.hpp"
 
#include <exception>
#include <memory>
#include <math.h>
#include <vector>
 
#include <Eigen/Dense>
 
constexpr float MATH_PI = 3.141592653f;
constexpr double MATH_PI_DOUBLE = 3.141592653589793238;
 
using namespace galaxis;
using namespace galaxis::utils;
using namespace galaxis::control;
 
namespace galaxis {
  namespace control {
 
    std::shared_ptr<CarPose> get_new_pose(std::shared_ptr<CarPose> const p_currentPoseSPtr, float p_carWheelbase, float p_distance, float p_yawDiff);
 
    class TwoAndAHalfCarrots {
    private:
      int m_horizon;
      int m_errorAccuracyStepsPerHorizon;
      std::shared_ptr<std::vector<float>> const m_yawStepsSPtr;
 
      float m_crossAngleErrorWeight = 0;
      float m_crossLineMarkingErrorWeight = 0;
 
      float m_carWheelbase = 0.205f;
      float m_errorIncraseFactor = 5;
 
      float m_lastYawRate        = 0;
      float m_currentVelocity    = 0;
      float m_horizonStepTimeSec = 0.5f; // 800 milliseconds
 
      // debug
      MicroStopwatch m_debugStopwatch;
      std::shared_ptr<CarPoseSeries> m_lastFirstPoseSeries;
      std::shared_ptr<CarPoseSeries> m_lastSecondPoseSeries;
 
    public:
      // TwoAndAHalfCarrots() = delete;
      TwoAndAHalfCarrots(std::shared_ptr<std::vector<float>> const p_yawStepsSPtr, float p_horizonStepTimeSec = 0.5, int p_horizon = 2, int p_errorAccuracyStepsPerHorizon = 4, float p_carWheelbase = 0.205f, float p_errorIncraseFactor = 3);
      void set_horizon(int p_horizon);
      bool set_horizon_step_time(float valueSec);
      bool set_error_factor_first_horizon(float factor);
      bool set_error_accuracy_steps_per_horizon(int value);
      void set_cross_angle_error_weight(float val);
      void set_cross_line_marking_error_weight(float val);
 
 
      // calulates the best drive trajectory compared with reference trajectory and returns yaw rate (bzw. lenkwinkel)
      //  -> all trajectories are relative to the car
      //  -> Velocity must be in m/s
      //  -> assuming yaw rate is steering angle
      // TODO: implement other horizons
      float calculate_best_yaw_rate(std::shared_ptr<PolySpline> p_referenceTrajectorySPtr, 
                                    std::shared_ptr<line_markings_reference_t> p_lineMarkings,
                                    float p_currentVelocity, CarPose p_pose = CarPose());
 
      // based on single-track model
      // distance in m, yawrate in radians
      std::shared_ptr<CarPoseSeries> get_pose_series(std::shared_ptr<CarPose> const p_currentPoseSPtr, float p_distance, float p_yawRate, int p_steps);
 
      std::shared_ptr<CarPoseSeries const> get_last_first_poseseries() const;
      std::shared_ptr<CarPoseSeries const> get_last_second_poseseries() const;
      float get_last_best_yaw_rate() const;
    };
  } // namespace control
} // namespace galaxis
 
#endif // _CONTROL_HPP_