Galaxis Codebase: galaxis::control::TwoAndAHalfCarrots Class Reference

#include <control.hpp>

Collaboration diagram for galaxis::control::TwoAndAHalfCarrots:

Public Member Functions
	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)

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())

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

Private Attributes
int	m_horizon

int	m_errorAccuracyStepsPerHorizon

const std::shared_ptr< std::vector< float > >	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

MicroStopwatch	m_debugStopwatch

std::shared_ptr< CarPoseSeries >	m_lastFirstPoseSeries

std::shared_ptr< CarPoseSeries >	m_lastSecondPoseSeries

Detailed Description

Definition at line 28 of file control.hpp.

Constructor & Destructor Documentation

◆ TwoAndAHalfCarrots()

TwoAndAHalfCarrots::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`
	)

Definition at line 19 of file control.cpp.

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

References m_carWheelbase, m_errorAccuracyStepsPerHorizon, m_errorIncraseFactor, m_horizonStepTimeSec, and set_horizon().

Here is the call graph for this function:

Member Function Documentation

◆ calculate_best_yaw_rate()

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 = `CarPose()`
	)

Definition at line 76 of file control.cpp.

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

References galaxis::calculate_error_between_polyspline_poseseries(), get_pose_series(), galaxis::utils::MicroStopwatch::get_time(), galaxis::utils::log(), galaxis::utils::log_warning(), m_crossAngleErrorWeight, m_crossLineMarkingErrorWeight, m_currentVelocity, m_debugStopwatch, m_errorAccuracyStepsPerHorizon, m_errorIncraseFactor, m_horizonStepTimeSec, m_lastFirstPoseSeries, m_lastSecondPoseSeries, m_lastYawRate, m_yawStepsSPtr, and galaxis::utils::MicroStopwatch::start().

Here is the call graph for this function:

◆ get_last_best_yaw_rate()

float TwoAndAHalfCarrots::get_last_best_yaw_rate ( ) const

Definition at line 154 of file control.cpp.

                                                        {
   return m_lastYawRate;
 }

References m_lastYawRate.

◆ get_last_first_poseseries()

std::shared_ptr< CarPoseSeries const > TwoAndAHalfCarrots::get_last_first_poseseries ( ) const

Definition at line 140 of file control.cpp.

                                                                                        {
   if(m_lastFirstPoseSeries == nullptr) {
     log_warning("LastFirstPoseSeries is nullptr");
   }
   return m_lastFirstPoseSeries;
 }

References galaxis::utils::log_warning(), and m_lastFirstPoseSeries.

Here is the call graph for this function:

◆ get_last_second_poseseries()

std::shared_ptr< CarPoseSeries const > TwoAndAHalfCarrots::get_last_second_poseseries ( ) const

Definition at line 147 of file control.cpp.

                                                                                         {
   if(m_lastSecondPoseSeries == nullptr) {
     log_warning("LastSecondPoseSeries is nullptr");
   }
   return m_lastSecondPoseSeries;
 }

References galaxis::utils::log_warning(), and m_lastSecondPoseSeries.

Here is the call graph for this function:

◆ get_pose_series()

std::shared_ptr< CarPoseSeries > TwoAndAHalfCarrots::get_pose_series	(	std::shared_ptr< CarPose > const	p_currentPoseSPtr,
		float	p_distance,
		float	p_yawRate,
		int	p_steps
	)

Definition at line 123 of file control.cpp.

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

References galaxis::control::get_new_pose(), galaxis::utils::log_error(), and m_carWheelbase.

Referenced by calculate_best_yaw_rate().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_cross_angle_error_weight()

void TwoAndAHalfCarrots::set_cross_angle_error_weight ( float val )

Definition at line 158 of file control.cpp.

                                                               {
   m_crossAngleErrorWeight = val;
 }

References m_crossAngleErrorWeight.

◆ set_cross_line_marking_error_weight()

void TwoAndAHalfCarrots::set_cross_line_marking_error_weight ( float val )

Definition at line 161 of file control.cpp.

                                                                      {
   m_crossLineMarkingErrorWeight = val;
 }

References m_crossLineMarkingErrorWeight.

◆ set_error_accuracy_steps_per_horizon()

bool TwoAndAHalfCarrots::set_error_accuracy_steps_per_horizon ( int value )

Definition at line 61 of file control.cpp.

                                                                        {
   if (value > 0) {
     m_errorAccuracyStepsPerHorizon = value;
     return true;
   }
   else {
     return false;
   }
 }

References m_errorAccuracyStepsPerHorizon.

◆ set_error_factor_first_horizon()

bool TwoAndAHalfCarrots::set_error_factor_first_horizon ( float factor )

Definition at line 57 of file control.cpp.

                                                                     {
   m_errorIncraseFactor = factor;
 }

References m_errorIncraseFactor.

◆ set_horizon()

void TwoAndAHalfCarrots::set_horizon ( int p_horizon )

Definition at line 36 of file control.cpp.

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

References galaxis::utils::log_error(), galaxis::utils::log_warning(), and m_horizon.

Referenced by TwoAndAHalfCarrots().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_horizon_step_time()

bool TwoAndAHalfCarrots::set_horizon_step_time ( float valueSec )

Definition at line 47 of file control.cpp.

                                                              {
   if (valueSec > 0) {
     m_horizonStepTimeSec = valueSec;
     return true;
   }
   else {
     return false;
   }
 }

References m_horizonStepTimeSec.

Member Data Documentation

◆ m_carWheelbase

float galaxis::control::TwoAndAHalfCarrots::m_carWheelbase = 0.205f

private

Definition at line 37 of file control.hpp.

Referenced by get_pose_series(), and TwoAndAHalfCarrots().

◆ m_crossAngleErrorWeight

float galaxis::control::TwoAndAHalfCarrots::m_crossAngleErrorWeight = 0

private

Definition at line 34 of file control.hpp.

Referenced by calculate_best_yaw_rate(), and set_cross_angle_error_weight().

◆ m_crossLineMarkingErrorWeight

float galaxis::control::TwoAndAHalfCarrots::m_crossLineMarkingErrorWeight = 0

private

Definition at line 35 of file control.hpp.

Referenced by calculate_best_yaw_rate(), and set_cross_line_marking_error_weight().

◆ m_currentVelocity

float galaxis::control::TwoAndAHalfCarrots::m_currentVelocity = 0

private

Definition at line 41 of file control.hpp.

Referenced by calculate_best_yaw_rate().

◆ m_debugStopwatch

MicroStopwatch galaxis::control::TwoAndAHalfCarrots::m_debugStopwatch

private

Definition at line 45 of file control.hpp.

Referenced by calculate_best_yaw_rate().

◆ m_errorAccuracyStepsPerHorizon

int galaxis::control::TwoAndAHalfCarrots::m_errorAccuracyStepsPerHorizon

private

Definition at line 31 of file control.hpp.

Referenced by calculate_best_yaw_rate(), set_error_accuracy_steps_per_horizon(), and TwoAndAHalfCarrots().

◆ m_errorIncraseFactor

float galaxis::control::TwoAndAHalfCarrots::m_errorIncraseFactor = 5

private

Definition at line 38 of file control.hpp.

Referenced by calculate_best_yaw_rate(), set_error_factor_first_horizon(), and TwoAndAHalfCarrots().

◆ m_horizon

int galaxis::control::TwoAndAHalfCarrots::m_horizon

private

Definition at line 30 of file control.hpp.

Referenced by set_horizon().

◆ m_horizonStepTimeSec

float galaxis::control::TwoAndAHalfCarrots::m_horizonStepTimeSec = 0.5f

private

Definition at line 42 of file control.hpp.

Referenced by calculate_best_yaw_rate(), set_horizon_step_time(), and TwoAndAHalfCarrots().

◆ m_lastFirstPoseSeries

std::shared_ptr<CarPoseSeries> galaxis::control::TwoAndAHalfCarrots::m_lastFirstPoseSeries

private

Definition at line 46 of file control.hpp.

Referenced by calculate_best_yaw_rate(), and get_last_first_poseseries().

◆ m_lastSecondPoseSeries

std::shared_ptr<CarPoseSeries> galaxis::control::TwoAndAHalfCarrots::m_lastSecondPoseSeries

private

Definition at line 47 of file control.hpp.

Referenced by calculate_best_yaw_rate(), and get_last_second_poseseries().

◆ m_lastYawRate

float galaxis::control::TwoAndAHalfCarrots::m_lastYawRate = 0

private

Definition at line 40 of file control.hpp.

Referenced by calculate_best_yaw_rate(), and get_last_best_yaw_rate().

◆ m_yawStepsSPtr

const std::shared_ptr<std::vector<float> > galaxis::control::TwoAndAHalfCarrots::m_yawStepsSPtr

private

Definition at line 32 of file control.hpp.

Referenced by calculate_best_yaw_rate().

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

utilities/include/control/control.hpp
utilities/src/control/control.cpp

Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ TwoAndAHalfCarrots()

Member Function Documentation

◆ calculate_best_yaw_rate()

◆ get_last_best_yaw_rate()

◆ get_last_first_poseseries()

◆ get_last_second_poseseries()

◆ get_pose_series()

◆ set_cross_angle_error_weight()

◆ set_cross_line_marking_error_weight()

◆ set_error_accuracy_steps_per_horizon()

◆ set_error_factor_first_horizon()

◆ set_horizon()

◆ set_horizon_step_time()

Member Data Documentation

◆ m_carWheelbase

◆ m_crossAngleErrorWeight

◆ m_crossLineMarkingErrorWeight

◆ m_currentVelocity

◆ m_debugStopwatch

◆ m_errorAccuracyStepsPerHorizon

◆ m_errorIncraseFactor

◆ m_horizon

◆ m_horizonStepTimeSec

◆ m_lastFirstPoseSeries

◆ m_lastSecondPoseSeries

◆ m_lastYawRate

◆ m_yawStepsSPtr