main_test.cpp File Reference

#include <unistd.h>
#include <Localization/Localizer.hpp>
#include <Decision/AutoPilot.hpp>
#include <polyfit/galaxis_polyfit.h>
#include <utils/poly.hpp>

Include dependency graph for main_test.cpp:

Go to the source code of this file.

Functions
double	eval_poly (DynamicPolynom p_polynom, double p_x)
int	main ()

Function Documentation

eval_poly()

double eval_poly	(	DynamicPolynom	p_polynom,
		double	p_x
	)

Definition at line 32 of file main_test.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 main().

Here is the caller graph for this function:

main()

int main

(

)

Definition at line 46 of file main_test.cpp.

           {
  std::vector<double> m_polynomPointsXVector;
  std::vector<double> m_polynomPointsYVector;
 
  std::shared_ptr<DriveInformation_t> m_lastDriveInformationSPtr = std::make_shared<DriveInformation_t>();
  rclcpp::Clock time;
  m_lastDriveInformationSPtr->polynom = std::make_shared<std::vector<double>>();
  m_lastDriveInformationSPtr->polynom->push_back(0);
  m_lastDriveInformationSPtr->polynom->push_back(0);
  m_lastDriveInformationSPtr->polynom->push_back(0);
  m_lastDriveInformationSPtr->polynom->push_back(0.1);
  m_lastDriveInformationSPtr->positionTimepoint.timestamp = time.now();
  m_lastDriveInformationSPtr->positionTimepoint.speed = 1e9;
  m_lastDriveInformationSPtr->positionTimepoint.orientation = 0;
 
  sleep(1);
  std::shared_ptr<PositionTimepoint> positionTimepoint = std::make_shared<PositionTimepoint>();
  positionTimepoint->timestamp = time.now();
  positionTimepoint->speed = 1e9;
  positionTimepoint->orientation = 0;
 
  // eval poly at estimated position // get time diff and multiply with velocity
  // 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 < 150 / 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]);
  }
 
  // TODO reenable
  std::cout << m_lastDriveInformationSPtr->polynom->at(0) << " + " << m_lastDriveInformationSPtr->polynom->at(1) << "x + " << m_lastDriveInformationSPtr->polynom->at(2) << "x^2 + " << m_lastDriveInformationSPtr->polynom->at(3) << "x^3" <<std::endl;
  
  m_lastDriveInformationSPtr->polynom = galaxis_polyfit(m_polynomPointsXVector, m_polynomPointsYVector, m_polynomPointsXVector.size() * 0.9, 20, 3);
  std::cout << m_lastDriveInformationSPtr->polynom->at(0) << " + " << m_lastDriveInformationSPtr->polynom->at(1) << "x + " << m_lastDriveInformationSPtr->polynom->at(2) << "x^2 + " << m_lastDriveInformationSPtr->polynom->at(3) << "x^3" <<std::endl;
 
  return 0;
}

References galaxis::utils::calculate_gradient(), and eval_poly().

Here is the call graph for this function: