MapDataLayer.hpp

Go to the documentation of this file.

#ifndef MAP_DATA_LAYER_HPP
#define MAP_DATA_LAYER_HPP
 
#include <map>
#include <unordered_map>
#include <array>
#include <memory>
#include <mutex>
#include <atomic>
 
#include <galaxis/linatypes.hpp>
#include <galaxis/messages.hpp>
#include <galaxis/AutopilotSettings.hpp>
#include <sensor_msgs/msg/image.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>
#include <shared_mutex>
#include <rclcpp/rclcpp.hpp>
 
using namespace galaxis::lina;
using namespace map::classes;
 
/*
MAPCONTENTS MESSAGE: # has to be refactored to ros_sensor_msgs
 
uint8[40000] center_lane
uint8[40000] right_lane
uint8[40000] left_lane
bool vertical_line False
 
 
uint8[40000] crossing_line
# uint8[40000] crosswalk
# uint8[40000] sign
 
uint64 timestamp
*/
 
constexpr uint8_t MAPCELL_MAXVALUE = 255; // uint8_t
 
struct MapCell {
  uint8_t leftLine   = 0;
  uint8_t centerLine = 0;
  uint8_t rightLine  = 0;
  bool imageIgnoreField = false;
  uint32_t lastUpdateStep = 0;
};
 
constexpr int MAP_SIZE       = 20000; // 20m 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:
 bool m_simulationMode = false;
 
  boost::asio::thread_pool m_threadPool;
  int m_amountOfThreads;
  std::shared_mutex m_sharedThreadMutex;
 
  // line data
  MapArray m_data; // [X][Y] // (0,0) - (left,bottom)
  Vector2f m_mapSize;
  std::mutex m_mapMutex;
  std::atomic<uint32_t> m_updateStep = 0;
 
  Vector2f m_absoluteCarPositionOffset;
 
  std::shared_ptr<LinesObject<int16_t>> m_linesObjectSPtr;
 
public:
  // SETTINGS
  int m_lookupWindowSizeCMWidth = 160;
  int m_lookupWindowSizeCMHeight = 150;
  // int m_lookupLookBehindCarCM = 10;
  uint8_t m_cellThresholdRGBValue = 80;
  uint8_t m_cellThresholdExtract = 99;
  uint8_t m_cellLineDecreasePerView = 100;
  uint8_t m_cellLineIncreasePerView = 100;
  float m_laneImagePixelToPositionConversionFactor = 1.5625f;
  float m_obstaclePositionDiffForRerecognitionCM = 40.0;
 
  // obstacle data
  std::shared_ptr<std::vector<StaticObstacle>> m_staticObstacleVecSPtr;
  rclcpp::Duration m_obstacleKeepDuration;
 
public:
  MapDataLayer();
  MapDataLayer(int amountOfThreads);
  ~MapDataLayer();
  void init();
  void clear();
 
  // ############## Settings ##############
  void enableSimulationMode();
  void setLookupWindowSizeCMWidth(int value);
  void setLookupWindowSizeCMHeight(int value);
  // void setLookupLookBehindCarCM(int value);
  void setCellThresholdRGBValue(uint8_t value);
  void setCellThresholdExtract(uint8_t value);
  void setCellLineDecreasePerView(uint8_t value);
  void setCellLineIncreasePerView(uint8_t value);
  void setLaneImageConversionFactor(float factor);
  void setObstaclePositionDiffForRerecognition(float diff);
  void setObstacleTrackTimeDuration(int sec, int nsec);
 
 
  bool isInImageIgnoreField(Vector2f p_position, unsigned int p_maxImageX);
 
  // line functions
  void updateMap(const sensor_msgs::msg::Image::ConstSharedPtr& laneDetectionSPtr, Vector2f currentAbsoluteCarPositionCM, double currentAbsoluteOrientation);
  std::shared_ptr<LinesObject<int16_t>> getLineObject(Vector2f absolutePositionCM, float lookBehindPosOffsetCM, double absoluteOrientation, Vector2i windowSize);
 
  // obstacle functions
  std::shared_ptr<std::vector<StaticObstacle>> getObstacles(rclcpp::Time const& timestamp, bool removeOld = true); 
  void addOrUpdateStaticObstacle(ObstacleType const& p_obstacle, Vector2f absolutePositionCM, double currentOrientation, rclcpp::Time const& timestamp);
 
private:
  void preprocessPoints(const sensor_msgs::msg::Image::ConstSharedPtr& laneDetectionSPtr, Vector2f const& currentPositionCM, int arrayOffsetStartIndex, int arrayOffsetEndIndex, double currentAbsoluteOrientation);
  inline void updateCell(MapCell const& cellUpdate, Vector2i position);
  inline void decreaseStepCell(MapCell* cellPtr);
  float checkPositionRelativeToEdge(int x1, int y1, int x2, int y2, int xp, int yp);
  void iterateThroughMap(int arrayStartX, int arrayEndX, int arrayStartY, int arrayEndY, Vector2i const& absoluteCarPosition, std::array<Vector2f, 4> const& windowEdges);
};
 
#endif // _MAP_DATA_LAYER_HPP_