MapClasses.hpp

Go to the documentation of this file.

#ifndef MAP_CLASSES_HPP
#define MAP_CLASSES_HPP
 
#include "galaxis/linatypes.hpp"
#include "utils/clock.hpp"
 
#include "rclcpp/rclcpp.hpp"
 
#include <vector>
#include <map>
#include <mutex>
 
using namespace galaxis::lina;
 
namespace map {
  namespace classes {
 
    template <typename T>
    class LinePoints {
    private:
      // Both Maps contain same information but in different representations therefore must be consistent
      std::map<T, std::vector<T>> m_pointsXY;
      std::map<T, std::vector<T>> m_pointsYX;
 
      std::mutex m_mtx;
 
    public:
      LinePoints(){};
 
      void clear() {
        m_mtx.lock();
 
        m_pointsXY.clear();
        m_pointsYX.clear();
 
        m_mtx.unlock();
      }
 
      void insert(T x, T y) {
        m_mtx.lock();
 
        auto itX = m_pointsXY.find(x);
        if(itX == m_pointsXY.end()) {
          // insert new vector
          m_pointsXY.insert(std::pair<T, std::vector<T>>(x, std::vector<T>{ y }));
        } else {
          itX->second.push_back(y);
        }
 
        auto itY = m_pointsYX.find(y);
        if(itY == m_pointsYX.end()) {
          // insert new vector
          m_pointsYX.insert(std::pair<T, std::vector<T>>(y, std::vector<T>{ x }));
        }
        else {
          itY->second.push_back(x);
        }
 
        m_mtx.unlock();
      }
 
      std::map<T, std::vector<T>>& getPointsXY() {
        return m_pointsXY;
      }
      std::map<T, std::vector<T>>& getPointsYX() {
        return m_pointsYX;
      }
    };
 
    template <typename T>
    class LinesObject {
    public:
      LinePoints<T> leftLine;
      LinePoints<T> centerLine;
      LinePoints<T> rightLine;
 
      double rotateOrientation;
      Eigen::Rotation2Df rotationMatrix;
      Eigen::Rotation2Df reverseRotationMatrix;
 
      Vector2i windowSize;
 
      LinesObject() {}
 
      void clear() {
        leftLine.clear();
        centerLine.clear();
        rightLine.clear();
      }
    };
 
    // Obstacles
    class Obstacle {
    protected:
      rclcpp::Time _m_lastUpdateTime;
    public:
      rclcpp::Time const& get_last_update() const {
        return _m_lastUpdateTime;
      }
      void update(rclcpp::Time const& lastUpdate) {
        _m_lastUpdateTime = lastUpdate;
      }
      virtual Vector2f get_position_now() const = 0;
      virtual Vector3f get_dimensions() const = 0;
    };
 
    class StaticObstacle : public Obstacle {
    private:
      Vector2f m_positionCM;
      float m_sizeCM;
    public:
      StaticObstacle() = delete;
      StaticObstacle(rclcpp::Time const& timestamp, Vector2f posCM, float sizeCM) : m_positionCM(posCM), m_sizeCM(sizeCM) {
        update(timestamp);
      }
 
      Vector2f get_position_now() const {
        return m_positionCM;
      }
 
      Vector3f get_dimensions() const {
        return Vector3f(m_sizeCM, m_sizeCM, m_sizeCM);
      }
 
      float get_width() const {
        return m_sizeCM;
      }
 
      void update_position(rclcpp::Time const& timestamp, Vector2f const& posCM) {
        m_positionCM = posCM;
        update(timestamp);
      }
    };
 
  } // namespace classes
} // namespace map
 
#endif