MapDataLayer.hpp

Go to the documentation of this file.

#ifndef MAP_DATA_LAYER_HPP_SIMULATION
#define MAP_DATA_LAYER_HPP_SIMULATION
 
#include <map>
#include <unordered_map>
#include <array>
#include <memory>
#include <mutex>
 
#include <galaxis/linatypes.hpp>
#include <galaxis/messages.hpp>
#include <galaxis/AutopilotSettings.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <geometry_msgs/msg/point.hpp>
 
#include "MapClasses.hpp"
 
// ThreadPool: https://www.boost.org/doc/libs/1_74_0/doc/html/boost_asio/reference/thread_pool.html
#include <boost/asio.hpp>
#include <boost/asio/thread_pool.hpp>
 
using namespace galaxis::lina;
using namespace galaxis::map::data;
 
struct MapCell {
  bool leftLine   = false;
  bool centerLine = false;
  bool rightLine  = false;
};
 
constexpr int MAP_SIZE       = 20000; // 20 x 20m with cell size of 1cm^2
 
class MapDataLayer {
  typedef std::array<MapCell, MAP_SIZE> ColumnMapArray;
  typedef std::array<ColumnMapArray, MAP_SIZE> MapArray;
 
private:
  int m_amountOfThreads;
  boost::asio::thread_pool m_threadPool;
 
  MapArray m_data; // [X][Y] // (0,0) - (left,bottom)
  Vector2f m_mapSize;
  std::mutex m_mapMutex;
 
  Vector2f m_absoluteCarPositionOffset;
 
  geometry_msgs::msg::Point m_lastPointLeftLane;
  bool m_lastPointLeftLaneUsable = false;
  geometry_msgs::msg::Point m_lastPointCenterLane;
  bool m_lastPointCenterLaneUsable = false;
  geometry_msgs::msg::Point m_lastPointRightLane;
  bool m_lastPointRightLaneUsable = false;
 
  std::shared_ptr<LinesObject<int16_t>> m_linesObjectSPtr;
 
public:
  MapDataLayer();
  MapDataLayer(int amountOfThreads);
  ~MapDataLayer();
  void init();
  void clear();
 
  void updateMap(visualization_msgs::msg::Marker::SharedPtr laneDetectionSPtr, char laneType);
  std::shared_ptr<LinesObject<int16_t>> getLineObject(Vector2f absolutePositionCM, float absoluteOrientation, Vector2i windowSize, Vector2i& effectiveWindowSize);
 
private:
  void updateCell(Vector2i position, char laneType);
  void iterateThroughMap(int arrayStartX, int arrayEndX, int arrayStartY, int arrayEndY, Vector2i const& absoluteCarPosition);
};
 
#endif // MAP_DATA_LAYER_HPP_SIMULATION