Localizer.hpp

Go to the documentation of this file.

#ifndef LOCALIZER_HPP
#define LOCALIZER_HPP
 
#include <cmath>
#include <galaxis/linatypes.hpp>
#include <utils/logger.hpp>
#include <chrono>
#include <iostream>
#include <mutex>
#include <rclcpp/rclcpp.hpp>
 
#include <boost/circular_buffer.hpp>
 
#include <vector>
 
using namespace galaxis::utils;
 
// Attention: units in millimeter
struct PositionTimepoint {
  rclcpp::Time timestamp;
  galaxis::lina::Vector2f position = galaxis::lina::Vector2f(0.f, 0.f); // position relative to start in mm
  double speed                     = 0; // speed in mm/s
  double orientation               = 0; // yaw angle in radians
};
 
// TODO: binary search for get_last_by_time
class PositionTimepointBuffer {
private:
  boost::circular_buffer<PositionTimepoint> data;
  std::shared_mutex m_mutex;
  unsigned int dataSize = 0;
 
public:
  PositionTimepointBuffer(unsigned int p_size = 200) :
      data(p_size), dataSize(p_size) {}
 
  void add(PositionTimepoint const& element) {
    m_mutex.lock();
 
    if(data.full()) {
      data.pop_front();
    }
    data.push_back(element);
    
    m_mutex.unlock();
  }
 
  PositionTimepoint get_last_by_time(rclcpp::Time& timestamp) {
    m_mutex.lock_shared();
 
    if(data.empty()) {
      log_warning("[Localizer] Position History Lookup failed. ");
      throw std::runtime_error("Localizer.hpp/PositionTimepointBuffer: Lookup failed!");
    }
 
    PositionTimepoint& element = data.back();
    for(int i = data.size() - 2; i >= 0; --i) {
      if(data[i].timestamp < timestamp) {
        if(std::abs((timestamp - data[i].timestamp).nanoseconds()) < std::abs((timestamp - element.timestamp).nanoseconds())) {
          element = data[i];
        }
        else {
          // pass (element remains element)
        }
        m_mutex.unlock_shared();
        return element;
      }
      element = data[i];
    }
 
    m_mutex.unlock_shared();
    log_warning("[Localizer] Position History Lookup fall back: ", std::to_string(data.at(data.size() - 1).timestamp.nanoseconds()), "  ", std::to_string(timestamp.nanoseconds()), " - Use oldest \n");
    return element;
  }
 
  PositionTimepoint get_last() {
    m_mutex.lock_shared();
    auto returnPoint = data.back();
    m_mutex.unlock_shared();
 
    return returnPoint;
  }
 
  bool is_empty() {
    m_mutex.lock_shared();
    bool isEmpty = data.empty();
    m_mutex.unlock_shared();
 
    return isEmpty;
  }
};
 
class Localizer {
public:
  Localizer();
 
  void init(rclcpp::Time& timestamp, double orientationRad);
  bool is_empty();
  void add_position_timepoint(rclcpp::Time& timestamp, galaxis::lina::Vector2f positionMillimeter, double speedMillimeterPerSec, double orientationRad);
  void add_state(rclcpp::Time& timestamp, double speedMillimeterPerSec, double orientationRad);
 
  std::shared_ptr<PositionTimepoint> get_latest_position_timepoint();
  std::shared_ptr<PositionTimepoint> get_position_timepoint_by_time(rclcpp::Time& timestamp);
 
private:
  PositionTimepointBuffer m_positionHistory;
};
 
#endif // LOCALIZER_HPP