control.cpp

Go to the documentation of this file.

#include "control/control.hpp"
 
#include "utils/logger.hpp"
#include "utils/poly.hpp"
#include "utils/clock.hpp"
#include "control/pose.hpp"
 
#include <exception>
#include <memory>
#include <math.h>
#include <vector>
 
#include <Eigen/Dense>
 
using namespace galaxis;
using namespace galaxis::utils;
using namespace galaxis::control;
 
TwoAndAHalfCarrots::TwoAndAHalfCarrots(std::shared_ptr<std::vector<float>> const p_yawStepsSPtr, float p_horizonStepTimeSec, int p_horizon, int p_errorAccuracyStepsPerHorizon, float p_carWheelbase, float p_errorIncraseFactor) :
    m_yawStepsSPtr(p_yawStepsSPtr) {
  this->set_horizon(p_horizon);
 
  assert(m_horizonStepTimeSec > 0);
  m_horizonStepTimeSec = p_horizonStepTimeSec;
 
  assert(p_carWheelbase > 0);
  m_carWheelbase = p_carWheelbase;
  m_errorIncraseFactor = p_errorIncraseFactor;
 
  assert(p_errorAccuracyStepsPerHorizon > 0);
  m_errorAccuracyStepsPerHorizon = p_errorAccuracyStepsPerHorizon;
 
  //log("TwoAndAHalfCarrots \n - HorizonTimeStep: ", m_horizonStepTimeSec, "\n - Horizon: ", m_horizon, "\n - ErrorAccuracy (StepsPerHorizon): ", m_errorAccuracyStepsPerHorizon, "\n - Wheelbase: ", m_carWheelbase, "");
}
 
void TwoAndAHalfCarrots::set_horizon(int p_horizon) {
  if(p_horizon < 1) {
    log_error();
    throw std::invalid_argument("Invalid Horizon! Horizon must be greater than 0!");
  } else if(p_horizon != 2) {
    log_warning("TwoAndAHalfCarrots only supports horizon 2 at the moment!");
  }
 
  m_horizon = p_horizon;
}
 
bool TwoAndAHalfCarrots::set_horizon_step_time(float valueSec) {
  if (valueSec > 0) {
    m_horizonStepTimeSec = valueSec;
    return true;
  }
  else {
    return false;
  }
}
 
bool TwoAndAHalfCarrots::set_error_factor_first_horizon(float factor) {
  m_errorIncraseFactor = factor;
}
 
bool TwoAndAHalfCarrots::set_error_accuracy_steps_per_horizon(int value) {
  if (value > 0) {
    m_errorAccuracyStepsPerHorizon = value;
    return true;
  }
  else {
    return false;
  }
}
 
// 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 TwoAndAHalfCarrots::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) { // currentYawRate?
  if(p_currentVelocity < 0) {
    log_warning("Velocity below 0! Return last Yaw Rate and skip calculation.");
    return m_lastYawRate;
  }
 
  m_currentVelocity                   = p_currentVelocity;
  float coveredDistancePerHorizonStep = m_currentVelocity * m_horizonStepTimeSec;
 
  float bestYawRate = 0;
  float bestError   = 100000000; // TODO: maybe set maximum error here -> then yaw rate = 0
 
  std::shared_ptr<CarPose> currentPose = std::make_shared<CarPose>(p_pose);
 
  // debug track calculation time
  m_debugStopwatch.start();
 
  for(auto const& yawStepH1: *m_yawStepsSPtr) {
    std::shared_ptr<CarPoseSeries> firstPoseSeries = get_pose_series(currentPose, coveredDistancePerHorizonStep, yawStepH1, m_errorAccuracyStepsPerHorizon);
    for(auto const& yawStepH2: *m_yawStepsSPtr) {
      // calculate point series
      std::shared_ptr<CarPoseSeries> secondPoseSeries = get_pose_series(firstPoseSeries->at(firstPoseSeries->size() - 1), coveredDistancePerHorizonStep, yawStepH2, m_errorAccuracyStepsPerHorizon);
 
      // calculate error
      float error = calculate_error_between_polyspline_poseseries(p_referenceTrajectorySPtr, firstPoseSeries, p_lineMarkings, m_crossAngleErrorWeight, m_crossLineMarkingErrorWeight) * m_errorIncraseFactor;
      error += calculate_error_between_polyspline_poseseries(p_referenceTrajectorySPtr, secondPoseSeries, p_lineMarkings, m_crossAngleErrorWeight, m_crossLineMarkingErrorWeight);
 
      if(error < bestError) {
        bestYawRate = yawStepH1;
        bestError   = error;
 
        m_lastFirstPoseSeries  = firstPoseSeries;
        m_lastSecondPoseSeries = secondPoseSeries;
      }
    }
  }
 
  // debug print calculation time
  long long time = m_debugStopwatch.get_time();
  log("[CONTROL] Yaw Calculation Time [microsec]: ", time, "");
 
  m_lastYawRate = bestYawRate;
  return bestYawRate;
}
 
std::shared_ptr<CarPoseSeries> TwoAndAHalfCarrots::get_pose_series(std::shared_ptr<CarPose> const p_currentPoseSPtr, float p_distance, float p_yawRate, int p_steps) {
  if(p_steps < 1) {
    log_error("[CONTROL] Step Amount below 1.");
    throw std::runtime_error("TwoAndAHalfCarrots::get_pose_series: Step amount below 1.");
  }
 
  std::shared_ptr<CarPoseSeries> series = std::make_shared<CarPoseSeries>();
  series->push_back(p_currentPoseSPtr); // start pose
 
  float step_distance = p_distance / static_cast<float>(p_steps);
  for(int i = 0; i < p_steps; ++i) {
    series->push_back(galaxis::control::get_new_pose(series->at(i), m_carWheelbase, step_distance, p_yawRate));
  }
 
  return series;
}
 
std::shared_ptr<CarPoseSeries const> TwoAndAHalfCarrots::get_last_first_poseseries() const {
  if(m_lastFirstPoseSeries == nullptr) {
    log_warning("LastFirstPoseSeries is nullptr");
  }
  return m_lastFirstPoseSeries;
}
 
std::shared_ptr<CarPoseSeries const> TwoAndAHalfCarrots::get_last_second_poseseries() const {
  if(m_lastSecondPoseSeries == nullptr) {
    log_warning("LastSecondPoseSeries is nullptr");
  }
  return m_lastSecondPoseSeries;
}
 
float TwoAndAHalfCarrots::get_last_best_yaw_rate() const {
  return m_lastYawRate;
}
 
void TwoAndAHalfCarrots::set_cross_angle_error_weight(float val){
  m_crossAngleErrorWeight = val;
}
void TwoAndAHalfCarrots::set_cross_line_marking_error_weight(float val){
  m_crossLineMarkingErrorWeight = val;
}
 
// based on single-track model
// distance in m, yawrate in radians
// TODO: switch to millimeter
std::shared_ptr<CarPose> galaxis::control::get_new_pose(std::shared_ptr<CarPose> const p_currentPoseSPtr, float p_carWheelbase, float p_distance, float p_yawDiff) {
  std::shared_ptr<CarPose> newPoseSPtr = std::make_shared<CarPose>();
  newPoseSPtr->position                = p_currentPoseSPtr->position;
  newPoseSPtr->orientation             = p_currentPoseSPtr->orientation;
 
  Eigen::Rotation2Df rotation(p_currentPoseSPtr->orientation);
 
  // filter edge case
  if(std::sin(p_yawDiff) == 0) {
    Eigen::Vector2f distance(p_distance, 0);
    newPoseSPtr->position += rotation * distance;
    return newPoseSPtr;
  }
 
  // r = L/sin(steering_angle)
  float radiusOfSteeringCircle = p_carWheelbase / std::sin(p_yawDiff);
  // U = 2PI * r
  float steeringCircumference = 2 * MATH_PI * radiusOfSteeringCircle;
  // angle = distance / circumference * 2PI
  float circleDrivenAngle = p_distance / steeringCircumference * 2 * MATH_PI;
 
  float directDistanceBetweenStartAndNewPose = 2 * std::sin(circleDrivenAngle / 2) * radiusOfSteeringCircle;
 
  Eigen::Vector2f position(0, 0);
  position[0] = directDistanceBetweenStartAndNewPose * std::cos(circleDrivenAngle / 2);
  position[1] = directDistanceBetweenStartAndNewPose * std::sin(circleDrivenAngle / 2);
 
  // Rotate new position with old orientation
  position = rotation * position;
 
  newPoseSPtr->position += position;
  newPoseSPtr->orientation += circleDrivenAngle;
 
  return newPoseSPtr;
}