Localizer.cpp

Go to the documentation of this file.

#include "Localization/Localizer.hpp"
 
#include <control/control.hpp>
#include <galaxis/controllerSettings.hpp>
 
using namespace galaxis::control;
 
Localizer::Localizer() :
    m_positionHistory(200) {}
 
void Localizer::init(rclcpp::Time& timestamp, double orientationRad) {
  m_positionHistory.add({ timestamp, galaxis::lina::Vector2f(0.f, 0.f), 0, orientationRad });
}
 
bool Localizer::is_empty() {
  return m_positionHistory.is_empty();
}
 
void Localizer::add_position_timepoint(rclcpp::Time& timestamp, galaxis::lina::Vector2f positionMillimeter, double speedMillimeterPerSec, double orientationRad) {
  PositionTimepoint newPositionPoint;
  newPositionPoint.timestamp = timestamp;
  newPositionPoint.speed = speedMillimeterPerSec;
  newPositionPoint.position = positionMillimeter;
  newPositionPoint.orientation = orientationRad;
 
  m_positionHistory.add(newPositionPoint);
}
 
// TODO: Test this function! (not used in SimulationMode)
void Localizer::add_state(rclcpp::Time& timestamp, double speedMillimeterPerSec, double orientationRad) {
  if(m_positionHistory.is_empty()) {
    log_warning("[LOCALIZER] Position History Empty!");
    this->add_position_timepoint(timestamp, galaxis::lina::Vector2f(0.f,0.f), speedMillimeterPerSec, orientationRad);
    return;
  }
 
  orientationRad = std::fmod(orientationRad, 2*MATH_PI_DOUBLE);
  // assert(orientationRad <= 2 * MATH_PI_DOUBLE + 0.1);
 
  PositionTimepoint newPositionPoint;
  newPositionPoint.timestamp = timestamp;
  newPositionPoint.orientation = orientationRad;
  newPositionPoint.speed = speedMillimeterPerSec;
 
  PositionTimepoint lastPositionPoint = m_positionHistory.get_last();
 
  // calc driven distance within time interval
  rclcpp::Duration timeIntervalNanoseconds = timestamp - lastPositionPoint.timestamp;
  double speed = (lastPositionPoint.speed + speedMillimeterPerSec) / 2.f;
  double drivenDistanceMillimeter = timeIntervalNanoseconds.nanoseconds() * speed / 1000000000.f; // based on speed at last point // TODO: CHECK
 
  // based on single-track model
  std::shared_ptr<CarPose> lastPose = std::make_shared<CarPose>();
  lastPose->orientation             = lastPositionPoint.orientation;
 
  double orientationDiff = orientationRad - lastPositionPoint.orientation;
 
  if(orientationDiff > MATH_PI_DOUBLE) { // if currentOrientation is lower than 0 deg (something around 2*PI) and lastOrientation greater than 0 deg
    orientationDiff -= 2 * MATH_PI_DOUBLE;
  } else if(orientationDiff < -MATH_PI_DOUBLE) { // analog the other way round
    orientationDiff += 2 * MATH_PI_DOUBLE;
  }
 
  // orientationDiff treated as turn angle
  // lastPose->position                = galaxis::lina::Vector2f(m_positionHistory.get_last().position[1], m_positionHistory.get_last().position[0]); // in mm
  // std::shared_ptr<CarPose> newPoseSPtr = galaxis::control::get_new_pose(lastPose, galaxis::controllerSettings::carWheelbase * 1000, drivenDistanceMillimeter, orientationDiff);
  // newPositionPoint.position    = galaxis::lina::Vector2f(newPoseSPtr->position[1], newPoseSPtr->position[0]);
  // newPositionPoint.orientation = orientationRad; // should be approx. equal to newPoseSPtr->orientation // TODO: check (look above) ***
 
  // new calculation
  Eigen::Rotation2Df lastOrientationRotMat(-lastPositionPoint.orientation); // counter-clock wise rotation
  galaxis::lina::Vector2f drivenDistanceVector = lastOrientationRotMat * galaxis::lina::Vector2f(0.f, drivenDistanceMillimeter);
  Eigen::Rotation2Df orientationDiffRotMat(-orientationDiff);
  drivenDistanceVector = orientationDiffRotMat * drivenDistanceVector;
 
  newPositionPoint.position = lastPositionPoint.position + drivenDistanceVector;
 
 
  log_debug("[LOCALIZER] Position: ", newPositionPoint.position[0], ",", newPositionPoint.position[1], "; Orientation: ", orientationRad);
 
  // append to buffer
  m_positionHistory.add(newPositionPoint);
}
 
std::shared_ptr<PositionTimepoint> Localizer::get_latest_position_timepoint() {
  return std::make_shared<PositionTimepoint>(m_positionHistory.get_last());
}
 
std::shared_ptr<PositionTimepoint> Localizer::get_position_timepoint_by_time(rclcpp::Time& timestamp) {
  return std::make_shared<PositionTimepoint>(m_positionHistory.get_last_by_time(timestamp));
}