main_test.cpp

Go to the documentation of this file.

#include <unistd.h>
 
#include <Localization/Localizer.hpp>
 
#include <Decision/AutoPilot.hpp>
#include <polyfit/galaxis_polyfit.h>
 
#include <utils/poly.hpp>
 
// Test Localizer
// int main(int argc, char* argv[]) {
//   rclcpp::Clock clock;
 
//   Localizer loc;
//   loc.init();
 
//   // first
//   // add_state(rclcpp::Time& timestamp, double speedMillimeterPerSec, double orientationRad)
//   loc.add_state(clock.now(), 1000.f, 0.f);
 
//   sleep(1);
 
//   loc.add_state(clock.now(), 1000.f, 0.f);
 
//   auto tp = loc.get_latest_position_timepoint();
 
//   std::cout << tp.to_string() << std::endl;
 
//   return 0;
// }
 
double eval_poly(DynamicPolynom p_polynom, double p_x) {
  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;
}
 
int main() {
  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;
}