MapDataLayer Class Reference

#include <MapDataLayer.hpp>

Public Member Functions
	MapDataLayer ()
	MapDataLayer (int amountOfThreads)
	~MapDataLayer ()
void	init ()
void	clear ()
void	enableSimulationMode ()
void	setLookupWindowSizeCMWidth (int value)
void	setLookupWindowSizeCMHeight (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)
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)
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)
	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)

Public Attributes
int	m_lookupWindowSizeCMWidth = 160
int	m_lookupWindowSizeCMHeight = 150
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
std::shared_ptr< std::vector< StaticObstacle > >	m_staticObstacleVecSPtr
rclcpp::Duration	m_obstacleKeepDuration

Private Types
typedef std::array< MapCell, MAP_SIZE >	ColumnMapArray
typedef std::array< ColumnMapArray, MAP_SIZE >	MapArray
typedef std::array< MapCell, MAP_SIZE >	ColumnMapArray
typedef std::array< ColumnMapArray, MAP_SIZE >	MapArray

Private Member Functions
void	preprocessPoints (const sensor_msgs::msg::Image::ConstSharedPtr &laneDetectionSPtr, Vector2f const &currentPositionCM, int arrayOffsetStartIndex, int arrayOffsetEndIndex, double currentAbsoluteOrientation)
void	updateCell (MapCell const &cellUpdate, Vector2i position)
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)
void	updateCell (Vector2i position, char laneType)
void	iterateThroughMap (int arrayStartX, int arrayEndX, int arrayStartY, int arrayEndY, Vector2i const &absoluteCarPosition)

Private Attributes
bool	m_simulationMode = false
boost::asio::thread_pool	m_threadPool
int	m_amountOfThreads
std::shared_mutex	m_sharedThreadMutex
MapArray	m_data
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
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

Detailed Description

Definition at line 55 of file MapDataLayer.hpp.

Member Typedef Documentation

ColumnMapArray [1/2]

typedef std::array<MapCell, MAP_SIZE> MapDataLayer::ColumnMapArray

private

Definition at line 34 of file MapDataLayer.hpp.

ColumnMapArray [2/2]

typedef std::array<MapCell, MAP_SIZE> MapDataLayer::ColumnMapArray

private

Definition at line 56 of file MapDataLayer.hpp.

MapArray [1/2]

typedef std::array<ColumnMapArray, MAP_SIZE> MapDataLayer::MapArray

private

Definition at line 35 of file MapDataLayer.hpp.

MapArray [2/2]

typedef std::array<ColumnMapArray, MAP_SIZE> MapDataLayer::MapArray

private

Definition at line 57 of file MapDataLayer.hpp.

Constructor & Destructor Documentation

MapDataLayer() [1/4]

MapDataLayer::MapDataLayer

(

)

Definition at line 18 of file MapDataLayer.cpp.

                           :
    m_threadPool(4), m_obstacleKeepDuration(1, 0) {
  m_amountOfThreads = 4;
  init();
}

References init(), and m_amountOfThreads.

Here is the call graph for this function:

MapDataLayer() [2/4]

MapDataLayer::MapDataLayer

(

int

amountOfThreads = 4

)

Definition at line 12 of file MapDataLayer.cpp.

                                                  :
    m_threadPool(amountOfThreads), m_amountOfThreads(amountOfThreads), m_obstacleKeepDuration(1, 0) {
  assert(m_amountOfThreads > 0);
  init();
}

References init(), and m_amountOfThreads.

Here is the call graph for this function:

~MapDataLayer() [1/2]

MapDataLayer::~MapDataLayer

(

)

Definition at line 24 of file MapDataLayer.cpp.

                            {
  m_threadPool.join();
}

References m_threadPool.

MapDataLayer() [3/4]

MapDataLayer::MapDataLayer

(

)

MapDataLayer() [4/4]

MapDataLayer::MapDataLayer

(

int

amountOfThreads

)

~MapDataLayer() [2/2]

MapDataLayer::~MapDataLayer

(

)

Member Function Documentation

addOrUpdateStaticObstacle()

void MapDataLayer::addOrUpdateStaticObstacle	(	ObstacleType const &	p_obstacle,
		Vector2f	absolutePositionCM,
		double	currentOrientation,
		rclcpp::Time const &	timestamp
	)

Definition at line 391 of file MapDataLayer.cpp.

                                                                                                                                                                {
  Vector2f obsPosition = Vector2f(p_obstacle.pos_x * 100, p_obstacle.pos_y * 100);
  Eigen::Rotation2Df rotationMat(-currentOrientation);
  obsPosition = rotationMat * obsPosition;
  obsPosition += absolutePositionCM;
 
  bool obstacleUpdated = false;
  for(StaticObstacle& obstacleStatic : *m_staticObstacleVecSPtr) {
    Vector2f distanceDiff = obsPosition - obstacleStatic.get_position_now();
    // TODO: also check dimensions
    if(distanceDiff.norm() < m_obstaclePositionDiffForRerecognitionCM) { // obstacle already inserted -> update
      obstacleStatic.update_position(timestamp, obsPosition);
      obstacleUpdated = true;
      break;
    }
  }
 
  if(obstacleUpdated == false) { // add new static obstacle
    StaticObstacle newObstacle(timestamp, obsPosition, p_obstacle.width * 100);
    m_staticObstacleVecSPtr->push_back(newObstacle);
  }
}

References m_obstaclePositionDiffForRerecognitionCM, and m_staticObstacleVecSPtr.

Referenced by AutoPilotNode::on_obstacle_callback().

Here is the caller graph for this function:

checkPositionRelativeToEdge()

float MapDataLayer::checkPositionRelativeToEdge ( int  x1, int  y1, int  x2, int  y2, int  xp, int  yp  )

private

Definition at line 328 of file MapDataLayer.cpp.

                                                                                              {
  // Edge: (x1,y1) and (x2,y2) [counterclockwise]
  // https://stackoverflow.com/questions/2752725/finding-whether-a-point-lies-inside-a-rectangle-or-not
  return (x2 - x1) * (yp - y1) - (xp - x1) * (y2 - y1);
}

Referenced by iterateThroughMap().

Here is the caller graph for this function:

clear() [1/2]

void MapDataLayer::clear

(

)

clear() [2/2]

void MapDataLayer::clear

(

)

Definition at line 41 of file MapDataLayer.cpp.

                         {
  for(ColumnMapArray& columnArray: m_data) {
    columnArray.fill(MapCell());
  }
}

References m_data.

Referenced by init().

Here is the caller graph for this function:

decreaseStepCell()

void MapDataLayer::decreaseStepCell ( MapCell *  cellPtr )

inlineprivate

Definition at line 226 of file MapDataLayer.cpp.

                                                           {
  // return; // TODO: remove
  uint32_t updateDiff = m_updateStep - cellPtr->lastUpdateStep;
  int decreaseValue   = updateDiff * m_cellLineDecreasePerView;
 
  // decrease
  if(decreaseValue < cellPtr->leftLine)
    cellPtr->leftLine = cellPtr->leftLine - decreaseValue;
  else 
    cellPtr->leftLine = 0;
 
  if(decreaseValue < cellPtr->centerLine)
    cellPtr->centerLine = cellPtr->centerLine - decreaseValue;
  else 
    cellPtr->centerLine = 0;
  
  if(decreaseValue < cellPtr->rightLine)
    cellPtr->rightLine = cellPtr->rightLine - decreaseValue;
  else 
    cellPtr->rightLine = 0;
      
  cellPtr->lastUpdateStep = m_updateStep;
}

References MapCell::centerLine, MapCell::lastUpdateStep, MapCell::leftLine, m_cellLineDecreasePerView, m_updateStep, and MapCell::rightLine.

enableSimulationMode()

void MapDataLayer::enableSimulationMode

(

)

Definition at line 48 of file MapDataLayer.cpp.

                                        {
  m_simulationMode = true;
}

References m_simulationMode.

Referenced by AutoPilotNode::AutoPilotNode().

Here is the caller graph for this function:

getLineObject() [1/2]

std::shared_ptr< LinesObject< int16_t > > MapDataLayer::getLineObject	(	Vector2f	absolutePositionCM,
		float	absoluteOrientation,
		Vector2i	windowSize,
		Vector2i &	effectiveWindowSize
	)

Definition at line 133 of file MapDataLayer.cpp.

                                                                                                                                                                          {
  m_mapMutex.lock();
 
  // Eigen rotates COUNTER-clock wise!
  Eigen::Rotation2Df rotationMatrix(-absoluteOrientation);
  Eigen::Rotation2Df reverseRotationMatrix(absoluteOrientation);
 
  m_linesObjectSPtr->clear();
  m_linesObjectSPtr->rotationMatrix        = rotationMatrix;
  m_linesObjectSPtr->reverseRotationMatrix = reverseRotationMatrix;
 
  absolutePositionCM += m_absoluteCarPositionOffset;
  Vector2i absolutePositionInteger = Vector2i(std::round(absolutePositionCM[0]), std::round(absolutePositionCM[1]));
 
  // put larger rectangle above window for map iteration
  Vector2f marginX = Vector2f(windowSize[0] / 2, 0);
  Vector2f marginY = Vector2f(0, windowSize[1]);
  marginX          = rotationMatrix * marginX;
  marginY          = rotationMatrix * marginY;
 
  std::array<Vector2f, 4> windowEdges;
  windowEdges[0] = absolutePositionCM - marginX;
  windowEdges[1] = absolutePositionCM + marginX;
  windowEdges[2] = windowEdges[0] + marginY;
  windowEdges[3] = windowEdges[1] + marginY;
 
  Vector2i largerWindowBottomLeftCorner = Vector2i(std::round(map::utils::getMin(windowEdges, 0)), std::round(map::utils::getMin(windowEdges, 1)));
  Vector2i largerWindowTopRightCorner   = Vector2i(std::round(map::utils::getMax(windowEdges, 0)), std::round(map::utils::getMax(windowEdges, 1)));
 
  // check if lookup (axis aligned) window (larger window) inside of map
  if(largerWindowBottomLeftCorner[0] < 0) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (X smaller than 0)");
    largerWindowBottomLeftCorner[1] = 0;
  }
  if(largerWindowTopRightCorner[0] >= m_mapSize[0]) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (X larger than map)");
    largerWindowTopRightCorner[0] = m_mapSize[0] - 1;
  }
  if(largerWindowBottomLeftCorner[1] < 0) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (Y smaller than 0)");
    largerWindowBottomLeftCorner[1] = 0;
  }
  if(largerWindowTopRightCorner[1] >= m_mapSize[1]) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (Y larger than map)");
    largerWindowTopRightCorner[1] = m_mapSize[1] - 1;
  }
 
  // int largerWindowStepsPerThread = (largerWindowTopRightCorner[0] - largerWindowBottomLeftCorner[0]) / m_amountOfThreads;
 
  // assert(largerWindowStepsPerThread > 0);
 
  // int arrayStartX = largerWindowBottomLeftCorner[0];
  // int arrayEndX   = arrayStartX + largerWindowStepsPerThread;
  // int arrayStartY = largerWindowBottomLeftCorner[1];
  // int arrayEndY   = largerWindowTopRightCorner[1];
 
  // for(int i = 0; i < m_amountOfThreads; ++i) {
  //   if(i == m_amountOfThreads - 1) {
  //     arrayEndX = largerWindowTopRightCorner[0] + 1;
  //   } else {
  //     arrayEndX = arrayStartX + largerWindowStepsPerThread;
  //   }
 
  //   boost::asio::post(m_threadPool, std::bind(&MapDataLayer::iterateThroughMap, this, arrayStartX, arrayEndX, arrayStartY, arrayEndY, absolutePositionInteger));
  //   arrayStartX = arrayEndX;
  // }
 
  iterateThroughMap(largerWindowBottomLeftCorner[0], largerWindowTopRightCorner[0] + 1, largerWindowBottomLeftCorner[1], largerWindowTopRightCorner[1], absolutePositionInteger);
 
  // m_threadPool.wait();
  m_mapMutex.unlock();
 
  effectiveWindowSize[0] = largerWindowTopRightCorner[0] - largerWindowBottomLeftCorner[0];
  assert(effectiveWindowSize[0] >= 0);
  effectiveWindowSize[1] = largerWindowTopRightCorner[1] - largerWindowBottomLeftCorner[1];
  assert(effectiveWindowSize[1] >= 0);
 
  return m_linesObjectSPtr;
}

References map::utils::getMax(), map::utils::getMin(), iterateThroughMap(), galaxis::utils::log_warning(), m_absoluteCarPositionOffset, m_linesObjectSPtr, m_mapMutex, and m_mapSize.

Here is the call graph for this function:

getLineObject() [2/2]

std::shared_ptr< LinesObject< int16_t > > MapDataLayer::getLineObject	(	Vector2f	absolutePositionCM,
		float	lookBehindPosOffsetCM,
		double	absoluteOrientation,
		Vector2i	windowSize
	)

Definition at line 250 of file MapDataLayer.cpp.

                                                                                                                                                                         {
  m_mapMutex.lock();
 
  Eigen::Rotation2Df rotationMatrix(-absoluteOrientation);
  Eigen::Rotation2Df reverseRotationMatrix(absoluteOrientation);
 
  Vector2f lookBehindVector = rotationMatrix * Vector2f(0.f, galaxis::autopilot::lookupLookBehindCarCM);
 
  m_linesObjectSPtr->clear();
  m_linesObjectSPtr->rotationMatrix        = rotationMatrix;
  m_linesObjectSPtr->reverseRotationMatrix = reverseRotationMatrix;
  m_linesObjectSPtr->rotateOrientation = -absoluteOrientation;
 
  absolutePositionCM += m_absoluteCarPositionOffset;
  Vector2i absolutePositionInteger = Vector2i(std::round(absolutePositionCM[0]), std::round(absolutePositionCM[1]));
 
  // put larger rectangle above window for map iteration
  Vector2f marginX = Vector2f(windowSize[0] / 2, 0);
  Vector2f marginY = Vector2f(0, windowSize[1]);
  marginX          = rotationMatrix * marginX;
  marginY          = rotationMatrix * marginY;
 
  std::array<Vector2f, 4> windowEdges;
  windowEdges[0] = absolutePositionCM - lookBehindVector - marginX; // left lower 
  windowEdges[1] = absolutePositionCM - lookBehindVector + marginX; // right lower
  windowEdges[2] = windowEdges[0] + marginY; // left upper
  windowEdges[3] = windowEdges[1] + marginY; // right upper
 
  Vector2i largerWindowBottomLeftCorner = Vector2i(std::round(map::utils::getMin(windowEdges, 0)), std::round(map::utils::getMin(windowEdges, 1)));
  Vector2i largerWindowTopRightCorner   = Vector2i(std::round(map::utils::getMax(windowEdges, 0)), std::round(map::utils::getMax(windowEdges, 1)));
 
  // check if lookup (axis aligned) window (larger window) inside of map
  if(largerWindowBottomLeftCorner[0] < 0) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (X smaller than 0)");
    largerWindowBottomLeftCorner[1] = 0;
  }
  if(largerWindowTopRightCorner[0] >= m_mapSize[0]) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (X larger than map)");
    largerWindowTopRightCorner[0] = m_mapSize[0] - 1;
  }
  if(largerWindowBottomLeftCorner[1] < 0) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (Y smaller than 0)");
    largerWindowBottomLeftCorner[1] = 0;
  }
  if(largerWindowTopRightCorner[1] >= m_mapSize[1]) {
    log_warning("MapDataLayer::getLineObject: lookup (axis aligned) window size out of map size (Y larger than map)");
    largerWindowTopRightCorner[1] = m_mapSize[1] - 1;
  }
 
  int largerWindowStepsPerThread = (largerWindowTopRightCorner[0] - largerWindowBottomLeftCorner[0]) / m_amountOfThreads;
 
  int arrayStartX = largerWindowBottomLeftCorner[0];
  int arrayEndX   = arrayStartX + largerWindowStepsPerThread;
  int arrayStartY = largerWindowBottomLeftCorner[1];
  int arrayEndY   = largerWindowTopRightCorner[1];
 
  for(int i = 0; i < m_amountOfThreads; ++i) {
    if(i == m_amountOfThreads - 1) {
      arrayEndX = largerWindowTopRightCorner[0] + 1;
    } else {
      arrayEndX = arrayStartX + largerWindowStepsPerThread;
    }
    
    m_sharedThreadMutex.lock_shared();
    boost::asio::post(m_threadPool, std::bind(&MapDataLayer::iterateThroughMap, this, arrayStartX, arrayEndX, arrayStartY, arrayEndY, absolutePositionInteger, windowEdges));
    arrayStartX = arrayEndX;
  }
 
  // wait for threads to complete
  m_sharedThreadMutex.lock();
  m_sharedThreadMutex.unlock();
 
  m_linesObjectSPtr->windowSize = Vector2i(largerWindowTopRightCorner[0] - largerWindowBottomLeftCorner[0], largerWindowTopRightCorner[1] - largerWindowBottomLeftCorner[1]);
 
  m_mapMutex.unlock();
  return m_linesObjectSPtr;
}

References map::utils::getMax(), map::utils::getMin(), iterateThroughMap(), galaxis::utils::log_warning(), galaxis::autopilot::lookupLookBehindCarCM, m_absoluteCarPositionOffset, m_amountOfThreads, m_linesObjectSPtr, m_mapMutex, m_mapSize, m_sharedThreadMutex, and m_threadPool.

Referenced by AutoPilot::get_lane_centers(), LaneDetectionGT::loop(), and AutoPilotNode::send_full_map_debug_image().

Here is the call graph for this function:

Here is the caller graph for this function:

getObstacles()

std::shared_ptr< std::vector< StaticObstacle > > MapDataLayer::getObstacles	(	rclcpp::Time const &	timestamp,
		bool	removeOld = true
	)

Definition at line 375 of file MapDataLayer.cpp.

                                                                                                               {
  if(removeOld) {
    // rm old obstacles
    for(std::vector<StaticObstacle>::iterator it = m_staticObstacleVecSPtr->begin(); it != m_staticObstacleVecSPtr->end(); /* noop */) {
      if (timestamp - it->get_last_update() > m_obstacleKeepDuration) {
        it = m_staticObstacleVecSPtr->erase(it);
      }
      else {
        ++it;
      }
    }
  }
 
  return m_staticObstacleVecSPtr;
}

References m_obstacleKeepDuration, and m_staticObstacleVecSPtr.

Referenced by AutoPilot::get_lane_blocked(), and AutoPilotNode::send_map_debug_image().

Here is the caller graph for this function:

init() [1/2]

void MapDataLayer::init

(

)

init() [2/2]

void MapDataLayer::init

(

)

Definition at line 28 of file MapDataLayer.cpp.

                        {
  log("MapDataLayer:\n - Threads ", m_amountOfThreads, "\n - Map Size ", MAP_SIZE, "x", MAP_SIZE);
 
  m_mapSize                   = Vector2f(MAP_SIZE, MAP_SIZE);
  m_absoluteCarPositionOffset = m_mapSize * 0.5;
 
  m_linesObjectSPtr = std::make_shared<LinesObject<int16_t>>();
 
  m_staticObstacleVecSPtr = std::make_shared<std::vector<StaticObstacle>>();
 
  clear();
}

References clear(), galaxis::utils::log(), m_absoluteCarPositionOffset, m_amountOfThreads, m_linesObjectSPtr, m_mapSize, m_staticObstacleVecSPtr, and MAP_SIZE.

Referenced by MapDataLayer().

Here is the call graph for this function:

Here is the caller graph for this function:

isInImageIgnoreField()

bool MapDataLayer::isInImageIgnoreField	(	Vector2f	p_position,
		unsigned int	p_maxImageX
	)

Definition at line 115 of file MapDataLayer.cpp.

                                                                                     {
  // check if in left ignore triangle
  // y = m*x + n
  float mLeftTriangle = static_cast<float>((imageIgnoreFieldArray[2] - imageIgnoreFieldArray[0])) / static_cast<float>(imageIgnoreFieldArray[1]);
  float nLeftTriangle = imageIgnoreFieldArray[0];
 
  if(p_position[1] > mLeftTriangle * p_position[0] + nLeftTriangle) {
    return true;
  }
 
  // check if in ignore box
  if(p_position[1] > imageIgnoreFieldArray[2] || p_position[1] > imageIgnoreFieldArray[4]) {
    return true;
  }
 
  // check if in right ignore triangle
  // y = m*x + n
  float mRightTriangle = static_cast<float>((imageIgnoreFieldArray[5] - imageIgnoreFieldArray[4])) / static_cast<float>((p_maxImageX - imageIgnoreFieldArray[3]));
  float nRightTriangle = mRightTriangle * imageIgnoreFieldArray[3] - imageIgnoreFieldArray[4];
 
  if(p_position[1] > mRightTriangle * p_position[0] + nRightTriangle) {
    return true;
  }
 
  return false;
}

References galaxis::autopilot::imageIgnoreFieldArray.

Referenced by preprocessPoints().

Here is the caller graph for this function:

iterateThroughMap() [1/2]

void MapDataLayer::iterateThroughMap ( int  arrayStartX, int  arrayEndX, int  arrayStartY, int  arrayEndY, Vector2i const &  absoluteCarPosition  )

private

Definition at line 213 of file MapDataLayer.cpp.

                                                                                                                                        {
  MapCell const* cellPtr = nullptr;
  for(int x = arrayStartX; x < arrayEndX; ++x) {
    for(int y = arrayStartY; y <= arrayEndY; ++y) {
      // append cell information to linesObject
      cellPtr = &m_data[x][y]; 
 
      if(cellPtr->leftLine) {
        m_linesObjectSPtr->leftLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
      if(cellPtr->centerLine) {
        m_linesObjectSPtr->centerLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
      if(cellPtr->rightLine) {
        m_linesObjectSPtr->rightLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
    }
  }
}

References MapCell::centerLine, MapCell::leftLine, m_data, m_linesObjectSPtr, and MapCell::rightLine.

iterateThroughMap() [2/2]

void MapDataLayer::iterateThroughMap ( int  arrayStartX, int  arrayEndX, int  arrayStartY, int  arrayEndY, Vector2i const &  absoluteCarPosition, std::array< Vector2f, 4 > const &  windowEdges  )

private

Definition at line 334 of file MapDataLayer.cpp.

                                                                                                                                                                                  {
  MapCell* cellPtr = nullptr;
  for(int x = arrayStartX; x < arrayEndX; ++x) {
    for(int y = arrayStartY; y <= arrayEndY; ++y) {
      // check if point lies in smaller/real window NOT only in larger Window
      if ( checkPositionRelativeToEdge(windowEdges[0][0], windowEdges[0][1], windowEdges[2][0], windowEdges[2][1], x, y) > 0 // left edge
        || checkPositionRelativeToEdge(windowEdges[2][0], windowEdges[2][1], windowEdges[3][0], windowEdges[3][1], x, y) > 0 // upper edge
        || checkPositionRelativeToEdge(windowEdges[3][0], windowEdges[3][1], windowEdges[1][0], windowEdges[1][1], x, y) > 0 // right edge
        || checkPositionRelativeToEdge(windowEdges[1][0], windowEdges[1][1], windowEdges[0][0], windowEdges[0][1], x, y) > 0 // lower edge
         ){
          continue;
      }
 
      // append cell information to linesObject
      cellPtr = &m_data[x][y];
 
      // clean 
      // decreaseStepCell(cellPtr);
      if(m_updateStep - cellPtr->lastUpdateStep > 20) { //&& (!cellPtr->imageIgnoreField || m_updateStep - cellPtr->lastUpdateStep > 5)) {
        cellPtr->leftLine = 0;
        cellPtr->centerLine = 0;
        cellPtr->rightLine = 0;
        continue;
      }
 
      if(cellPtr->leftLine > m_cellThresholdExtract) {
        m_linesObjectSPtr->leftLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
      if(cellPtr->centerLine > m_cellThresholdExtract) {
        m_linesObjectSPtr->centerLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
      if(cellPtr->rightLine > m_cellThresholdExtract) {
        m_linesObjectSPtr->rightLine.insert(x - absoluteCarPosition[0], y - absoluteCarPosition[1]);
      }
    }
  }
 
  m_sharedThreadMutex.unlock_shared();
}

References MapCell::centerLine, checkPositionRelativeToEdge(), MapCell::lastUpdateStep, MapCell::leftLine, m_cellThresholdExtract, m_data, m_linesObjectSPtr, m_sharedThreadMutex, m_updateStep, and MapCell::rightLine.

Referenced by getLineObject().

Here is the call graph for this function:

Here is the caller graph for this function:

preprocessPoints()

void MapDataLayer::preprocessPoints ( const sensor_msgs::msg::Image::ConstSharedPtr &  laneDetectionSPtr, Vector2f const &  currentPositionCM, int  arrayOffsetStartIndex, int  arrayOffsetEndIndex, double  currentAbsoluteOrientation  )

private

Definition at line 142 of file MapDataLayer.cpp.

                                                                                                                                                                                                                        {
  // TODO: position and orientation of car at recorded MapContents Message (history)
 
  Eigen::Rotation2Df rotationMatrix(-currentAbsoluteOrientation);
 
  // num of rows == height == Y, num of columns == width == X
 
  int pixelLengthInBytes = laneDetectionSPtr->step / laneDetectionSPtr->width;
  for(int index = arrayOffsetStartIndex; index < arrayOffsetEndIndex; index += pixelLengthInBytes) {
    int posXOriginal = (index % laneDetectionSPtr->step) / pixelLengthInBytes;
    int posYOriginal = static_cast<int>(index / laneDetectionSPtr->step);
    
    // image position conversion to map positions
    int posX = posXOriginal - laneDetectionSPtr->width / 2;
    int posY = laneDetectionSPtr->height - posYOriginal;
    Vector2f absoluteCellPosition = Vector2f(posX, posY) * m_laneImagePixelToPositionConversionFactor;
    absoluteCellPosition = rotationMatrix * absoluteCellPosition;
    absoluteCellPosition += currentPositionCM + m_absoluteCarPositionOffset;
 
    MapCell cellUpdate;
    cellUpdate.leftLine   = laneDetectionSPtr->data[index + galaxis::autopilot::imageLLinePixelByteIndex];
    cellUpdate.centerLine = laneDetectionSPtr->data[index + galaxis::autopilot::imageCLinePixelByteIndex];
    cellUpdate.rightLine  = laneDetectionSPtr->data[index + galaxis::autopilot::imageRLinePixelByteIndex];
 
    Vector2i absoluteCellPositionOnGrid = Vector2i(std::round(absoluteCellPosition[0]), std::round(absoluteCellPosition[1]));
 
 
    // skip Image Ignore Fields
    if (!m_simulationMode && isInImageIgnoreField(Vector2f(posXOriginal, posYOriginal), laneDetectionSPtr->width)) {
      m_data[absoluteCellPositionOnGrid[0]][absoluteCellPositionOnGrid[1]].imageIgnoreField = true;
    }
    else {
      updateCell(cellUpdate, absoluteCellPositionOnGrid);
    }
  }
 
  m_sharedThreadMutex.unlock_shared();
}

References MapCell::centerLine, galaxis::autopilot::imageCLinePixelByteIndex, galaxis::autopilot::imageLLinePixelByteIndex, galaxis::autopilot::imageRLinePixelByteIndex, isInImageIgnoreField(), MapCell::leftLine, m_absoluteCarPositionOffset, m_data, m_laneImagePixelToPositionConversionFactor, m_sharedThreadMutex, m_simulationMode, MapCell::rightLine, and updateCell().

Referenced by updateMap().

Here is the call graph for this function:

Here is the caller graph for this function:

setCellLineDecreasePerView()

void MapDataLayer::setCellLineDecreasePerView

(

uint8_t

value

)

Definition at line 66 of file MapDataLayer.cpp.

                                                           {
  m_cellLineDecreasePerView = value;
}

References m_cellLineDecreasePerView.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

setCellLineIncreasePerView()

void MapDataLayer::setCellLineIncreasePerView

(

uint8_t

value

)

Definition at line 69 of file MapDataLayer.cpp.

                                                           {
  m_cellLineIncreasePerView = value;
}

References m_cellLineIncreasePerView.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

setCellThresholdExtract()

void MapDataLayer::setCellThresholdExtract

(

uint8_t

value

)

Definition at line 63 of file MapDataLayer.cpp.

                                                        {
  m_cellThresholdExtract = value;
}

References m_cellThresholdExtract.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

setCellThresholdRGBValue()

void MapDataLayer::setCellThresholdRGBValue

(

uint8_t

value

)

Definition at line 60 of file MapDataLayer.cpp.

                                                         {
  m_cellThresholdRGBValue = value;
}

References m_cellThresholdRGBValue.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

setLaneImageConversionFactor()

void MapDataLayer::setLaneImageConversionFactor

(

float

factor

)

Definition at line 72 of file MapDataLayer.cpp.

                                                            {
  m_laneImagePixelToPositionConversionFactor = factor;
}

References m_laneImagePixelToPositionConversionFactor.

Referenced by AutoPilotNode::AutoPilotNode().

Here is the caller graph for this function:

setLookupWindowSizeCMHeight()

void MapDataLayer::setLookupWindowSizeCMHeight

(

int

value

)

Definition at line 54 of file MapDataLayer.cpp.

                                                        {
 m_lookupWindowSizeCMHeight = value;
}

References m_lookupWindowSizeCMHeight.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

setLookupWindowSizeCMWidth()

void MapDataLayer::setLookupWindowSizeCMWidth

(

int

value

)

Definition at line 51 of file MapDataLayer.cpp.

                                                       {
  m_lookupWindowSizeCMWidth = value;
}

References m_lookupWindowSizeCMWidth.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

setObstaclePositionDiffForRerecognition()

void MapDataLayer::setObstaclePositionDiffForRerecognition

(

float

diff

)

Definition at line 75 of file MapDataLayer.cpp.

                                                                     {
  m_obstaclePositionDiffForRerecognitionCM = diff;
}

References m_obstaclePositionDiffForRerecognitionCM.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

setObstacleTrackTimeDuration()

void MapDataLayer::setObstacleTrackTimeDuration	(	int	sec,
		int	nsec
	)

Definition at line 78 of file MapDataLayer.cpp.

                                                                 {
  m_obstacleKeepDuration = rclcpp::Duration(sec, nsec);
}

References m_obstacleKeepDuration.

Referenced by AutoPilotNode::new_parameters_callback().

Here is the caller graph for this function:

updateCell() [1/2]

void MapDataLayer::updateCell ( MapCell const &  cellUpdate, Vector2i  position  )

inlineprivate

Definition at line 181 of file MapDataLayer.cpp.

                                                                          {
  if(position[0] < 0 || position[0] >= m_mapSize[0] || position[1] < 0 || position[1] >= m_mapSize[1]) {
    log_warning("MapDataLayer::updateCell: cannot update Cell (out of bounds) at Position (", position[0], ",", position[1], ")");
    return;
  }
 
  MapCell* cellPtr    = &m_data[position[0]][position[1]];
 
  // decrease
  // decreaseStepCell(cellPtr);
 
  // increase
  if(cellUpdate.leftLine > m_cellThresholdRGBValue) {
    // int value = cellPtr->leftLine;
    // value += m_cellLineIncreasePerView;
    // cellPtr->leftLine = value > MAPCELL_MAXVALUE ? MAPCELL_MAXVALUE : value;
    cellPtr->leftLine = 255;
  }
  else {
    cellPtr->leftLine = 0;
  }
  if(cellUpdate.centerLine > m_cellThresholdRGBValue) {
    // int value = cellPtr->centerLine;
    // value += m_cellLineIncreasePerView;
    // cellPtr->centerLine = value > MAPCELL_MAXVALUE ? MAPCELL_MAXVALUE : value;
    cellPtr->centerLine = 255;
  }
  else {
    cellPtr->centerLine = 0;
  }
  if(cellUpdate.rightLine > m_cellThresholdRGBValue) {
    // int value = cellPtr->rightLine;
    // value += m_cellLineIncreasePerView;
    // cellPtr->rightLine = value > MAPCELL_MAXVALUE ? MAPCELL_MAXVALUE : value;
    cellPtr->rightLine = 255;
  }
  else {
    cellPtr->rightLine = 0;
  }
 
  cellPtr->imageIgnoreField = false;
 
  cellPtr->lastUpdateStep = m_updateStep;
}

References MapCell::centerLine, MapCell::imageIgnoreField, MapCell::lastUpdateStep, MapCell::leftLine, galaxis::utils::log_warning(), m_cellThresholdRGBValue, m_data, m_mapSize, m_updateStep, and MapCell::rightLine.

Referenced by preprocessPoints(), and updateMap().

Here is the call graph for this function:

Here is the caller graph for this function:

updateCell() [2/2]

void MapDataLayer::updateCell ( Vector2i  position, char  laneType  )

private

Definition at line 116 of file MapDataLayer.cpp.

                                                              {
  if(position[0] < 0 || position[0] >= m_mapSize[0] || position[1] < 0 || position[1] >= m_mapSize[1]) {
    log_warning("MapDataLayer::updateCell: cannot update Cell (out of bounds) at Position (", position[0], ",", position[1], ")");
    return;
  }
 
  MapCell* cellPtr = &m_data[position[0]][position[1]];
  if (laneType == 'l')
    cellPtr->leftLine = true;
  else if (laneType == 'c')
    cellPtr->centerLine = true;
  else if (laneType == 'r')
    cellPtr->rightLine = true;
  else 
    log_warning("Failure to updateMap. Wrong lanetype parameter value (l,c,r)");
}

References MapCell::centerLine, MapCell::leftLine, galaxis::utils::log_warning(), m_data, m_mapSize, and MapCell::rightLine.

Here is the call graph for this function:

updateMap() [1/2]

void MapDataLayer::updateMap	(	const sensor_msgs::msg::Image::ConstSharedPtr &	laneDetectionSPtr,
		Vector2f	currentAbsoluteCarPositionCM,
		double	currentAbsoluteOrientation
	)

Definition at line 83 of file MapDataLayer.cpp.

                                                                                                                                                                 {
  // TODO: check update rate of vision (MapContentsType); maybe check at runtime
  m_mapMutex.lock();
  // Increment Update Step
  m_updateStep++;
 
  int pixelLengthInBytes = laneDetectionSPtr->step / laneDetectionSPtr->width;
 
  int arrayStartIndex          = 0;
  int arrayWindowSizePerThread = (laneDetectionSPtr->height * laneDetectionSPtr->step) / m_amountOfThreads;
  arrayWindowSizePerThread -= arrayWindowSizePerThread % pixelLengthInBytes;
  int arrayEndIndex = arrayStartIndex + arrayWindowSizePerThread;
  // split message and run preprocessPoints function on multiple threads in pool
  for(int i = 0; i < m_amountOfThreads; ++i) {
    if(i == m_amountOfThreads - 1) {
      arrayEndIndex = (laneDetectionSPtr->height * laneDetectionSPtr->step) - 1;
    } else {
      arrayEndIndex = arrayStartIndex + arrayWindowSizePerThread;
    }
 
    m_sharedThreadMutex.lock_shared();
    boost::asio::post(m_threadPool, std::bind(&MapDataLayer::preprocessPoints, this, laneDetectionSPtr, currentAbsoluteCarPositionCM, arrayStartIndex, arrayEndIndex, currentAbsoluteOrientation));
    arrayStartIndex = arrayEndIndex;
  }
 
  // wait for threads to complete
  m_sharedThreadMutex.lock();
  m_sharedThreadMutex.unlock();
 
  m_mapMutex.unlock();
}

References m_amountOfThreads, m_mapMutex, m_sharedThreadMutex, m_threadPool, m_updateStep, and preprocessPoints().

Referenced by LaneDetectionGT::groundtruthCallback(), and AutoPilotNode::on_vci_lane_detection_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

updateMap() [2/2]

void MapDataLayer::updateMap	(	visualization_msgs::msg::Marker::SharedPtr	laneDetectionSPtr,
		char	laneType
	)

Definition at line 49 of file MapDataLayer.cpp.

                                                                                                  {
  // single-threaded
  m_mapMutex.lock();
 
  bool usable = false;
  geometry_msgs::msg::Point* lastPoint;
  
  if (laneType == 'l') {
    usable = m_lastPointLeftLaneUsable;
    lastPoint = &m_lastPointLeftLane;
  }
  else if (laneType == 'c') {
    usable = m_lastPointCenterLaneUsable;
    lastPoint = &m_lastPointCenterLane;
  }
  else if (laneType == 'r') {
    usable = m_lastPointRightLaneUsable;
    lastPoint = &m_lastPointRightLane;
  }
 
  for(geometry_msgs::msg::Point& point : laneDetectionSPtr->points) {
    if (usable == false) {
      lastPoint = &point;
      usable = true;
      continue;
    }
 
    // approximate between points in 1cm steps
    double distanceCM = sqrt((point.x - lastPoint->x)*(point.x - lastPoint->x) + (point.y - lastPoint->y)*(point.y - lastPoint->y)) * 100;
    if (distanceCM <= 1) {
      distanceCM = 1;
      // log_warning("Fix distanceCM.");
    }
    
    double distanceX = lastPoint->x - point.x; // in meter
    double distanceY = lastPoint->y - point.y; // in meter
    for (double p = 0; p < 1; p += 1/distanceCM) {
      double pointX = point.x + p * distanceX;
      double pointY = point.y + p * distanceY;
      pointX *= 100; // to CM
      pointY *= 100; // to CM
 
      Vector2i absoluteCellPositionOnGrid = Vector2i(std::round(pointX), std::round(pointY));
      absoluteCellPositionOnGrid[0] += static_cast<int>(m_absoluteCarPositionOffset[0]);
      absoluteCellPositionOnGrid[1] += static_cast<int>(m_absoluteCarPositionOffset[1]);
 
      updateCell(absoluteCellPositionOnGrid, laneType);
    }
 
    lastPoint = &point;
  }
 
  if (laneType == 'l') {
    m_lastPointLeftLane = *lastPoint;
    m_lastPointLeftLaneUsable = true;
  }
  else if (laneType == 'c') {
    m_lastPointCenterLane = *lastPoint;
    m_lastPointCenterLaneUsable = true;
  }
  else if (laneType == 'r') {
    m_lastPointRightLane = *lastPoint;
    m_lastPointRightLaneUsable = true;
  }
  m_mapMutex.unlock();
}

References m_absoluteCarPositionOffset, m_lastPointCenterLane, m_lastPointCenterLaneUsable, m_lastPointLeftLane, m_lastPointLeftLaneUsable, m_lastPointRightLane, m_lastPointRightLaneUsable, m_mapMutex, and updateCell().

Here is the call graph for this function:

Member Data Documentation

m_absoluteCarPositionOffset

Vector2f MapDataLayer::m_absoluteCarPositionOffset

private

Definition at line 72 of file MapDataLayer.hpp.

Referenced by getLineObject(), init(), preprocessPoints(), and updateMap().

m_amountOfThreads

int MapDataLayer::m_amountOfThreads

private

Definition at line 63 of file MapDataLayer.hpp.

Referenced by getLineObject(), init(), MapDataLayer(), and updateMap().

m_cellLineDecreasePerView

uint8_t MapDataLayer::m_cellLineDecreasePerView = 100

Definition at line 83 of file MapDataLayer.hpp.

Referenced by decreaseStepCell(), and setCellLineDecreasePerView().

m_cellLineIncreasePerView

uint8_t MapDataLayer::m_cellLineIncreasePerView = 100

Definition at line 84 of file MapDataLayer.hpp.

Referenced by setCellLineIncreasePerView().

m_cellThresholdExtract

uint8_t MapDataLayer::m_cellThresholdExtract = 99

Definition at line 82 of file MapDataLayer.hpp.

Referenced by iterateThroughMap(), and setCellThresholdExtract().

m_cellThresholdRGBValue

uint8_t MapDataLayer::m_cellThresholdRGBValue = 80

Definition at line 81 of file MapDataLayer.hpp.

Referenced by setCellThresholdRGBValue(), and updateCell().

m_data

MapArray MapDataLayer::m_data

private

Definition at line 67 of file MapDataLayer.hpp.

Referenced by clear(), iterateThroughMap(), preprocessPoints(), and updateCell().

m_laneImagePixelToPositionConversionFactor

float MapDataLayer::m_laneImagePixelToPositionConversionFactor = 1.5625f

Definition at line 85 of file MapDataLayer.hpp.

Referenced by preprocessPoints(), and setLaneImageConversionFactor().

m_lastPointCenterLane

geometry_msgs::msg::Point MapDataLayer::m_lastPointCenterLane

private

Definition at line 49 of file MapDataLayer.hpp.

Referenced by updateMap().

m_lastPointCenterLaneUsable

bool MapDataLayer::m_lastPointCenterLaneUsable = false

private

Definition at line 50 of file MapDataLayer.hpp.

Referenced by updateMap().

m_lastPointLeftLane

geometry_msgs::msg::Point MapDataLayer::m_lastPointLeftLane

private

Definition at line 47 of file MapDataLayer.hpp.

Referenced by updateMap().

m_lastPointLeftLaneUsable

bool MapDataLayer::m_lastPointLeftLaneUsable = false

private

Definition at line 48 of file MapDataLayer.hpp.

Referenced by updateMap().

m_lastPointRightLane

geometry_msgs::msg::Point MapDataLayer::m_lastPointRightLane

private

Definition at line 51 of file MapDataLayer.hpp.

Referenced by updateMap().

m_lastPointRightLaneUsable

bool MapDataLayer::m_lastPointRightLaneUsable = false

private

Definition at line 52 of file MapDataLayer.hpp.

Referenced by updateMap().

m_linesObjectSPtr

std::shared_ptr< LinesObject< int16_t > > MapDataLayer::m_linesObjectSPtr

private

Definition at line 74 of file MapDataLayer.hpp.

Referenced by getLineObject(), init(), and iterateThroughMap().

m_lookupWindowSizeCMHeight

int MapDataLayer::m_lookupWindowSizeCMHeight = 150

Definition at line 79 of file MapDataLayer.hpp.

Referenced by AutoPilot::get_drive_information(), AutoPilot::get_lane_centers(), AutoPilot::get_lane_centers_via_polynom_thread_helper(), AutoPilotNode::send_map_debug_image(), and setLookupWindowSizeCMHeight().

m_lookupWindowSizeCMWidth

int MapDataLayer::m_lookupWindowSizeCMWidth = 160

Definition at line 78 of file MapDataLayer.hpp.

Referenced by AutoPilot::get_lane_centers(), AutoPilotNode::send_map_debug_image(), and setLookupWindowSizeCMWidth().

m_mapMutex

std::mutex MapDataLayer::m_mapMutex

private

Definition at line 69 of file MapDataLayer.hpp.

Referenced by getLineObject(), and updateMap().

m_mapSize

Vector2f MapDataLayer::m_mapSize

private

Definition at line 68 of file MapDataLayer.hpp.

Referenced by getLineObject(), init(), and updateCell().

m_obstacleKeepDuration

rclcpp::Duration MapDataLayer::m_obstacleKeepDuration

Definition at line 90 of file MapDataLayer.hpp.

Referenced by getObstacles(), and setObstacleTrackTimeDuration().

m_obstaclePositionDiffForRerecognitionCM

float MapDataLayer::m_obstaclePositionDiffForRerecognitionCM = 40.0

Definition at line 86 of file MapDataLayer.hpp.

Referenced by addOrUpdateStaticObstacle(), and setObstaclePositionDiffForRerecognition().

m_sharedThreadMutex

std::shared_mutex MapDataLayer::m_sharedThreadMutex

private

Definition at line 64 of file MapDataLayer.hpp.

Referenced by getLineObject(), iterateThroughMap(), preprocessPoints(), and updateMap().

m_simulationMode

bool MapDataLayer::m_simulationMode = false

private

Definition at line 60 of file MapDataLayer.hpp.

Referenced by enableSimulationMode(), and preprocessPoints().

m_staticObstacleVecSPtr

std::shared_ptr<std::vector<StaticObstacle> > MapDataLayer::m_staticObstacleVecSPtr

Definition at line 89 of file MapDataLayer.hpp.

Referenced by addOrUpdateStaticObstacle(), getObstacles(), and init().

m_threadPool

boost::asio::thread_pool MapDataLayer::m_threadPool

private

Definition at line 62 of file MapDataLayer.hpp.

Referenced by getLineObject(), updateMap(), and ~MapDataLayer().

m_updateStep

std::atomic<uint32_t> MapDataLayer::m_updateStep = 0

private

Definition at line 70 of file MapDataLayer.hpp.

Referenced by decreaseStepCell(), iterateThroughMap(), updateCell(), and updateMap().

---

The documentation for this class was generated from the following files:

• nodes/Autopilot/include/Map/MapDataLayer.hpp
• nodes/Autopilot/src/Map/MapDataLayer.cpp