Базовый класс для всех планировщиков на сетке с нодами Базовый класс для всех планировщиков на сетке с нодами, у каждой из которых указывается предок и координаты на сетке. Суть планирования на сетке заключается в том, что каждой вещественной координате состояния мы ставим в соответствие целочисленную координату из заданного диапазона. В данном планировщике предполагается, что каждая координата состояния разбивается на равное число точек (_gridSize) More...

#include <node_grid_path_finder.h>

Inheritance diagram for bmpf::NodeGridPathFinder:

[legend]

Collaboration diagram for bmpf::NodeGridPathFinder:

[legend]

Public Member Functions
	NodeGridPathFinder (const std::shared_ptr< bmpf::Scene > &scene, bool showTrace, int gridSize, unsigned int maxNodeCnt, unsigned int kG=1, unsigned int kD=0, int threadCnt=1)

std::vector< std::vector< double > >	getAllProcessedStates ()

bool	findTick (std::vector< double > &state) override

virtual std::shared_ptr< PathNode >	_forEachNeighbor (std::shared_ptr< PathNode > currentNode, std::vector< int > &endCoords)=0

std::shared_ptr< PathNode >	tryToGetNeighborPtr (std::vector< int > newCoords, const std::shared_ptr< PathNode > &parentNode, double sum)

void	buildPath () override

void	prepare (const std::vector< double > &startState, const std::vector< double > &endState) override

void	prepare (std::vector< int > &startState, std::vector< int > &endState) override

std::vector< double >	getCurrentState ()

void	nextNode ()

double	_findLinkDistance (std::vector< int > &a, std::vector< int > &b)

Public Member Functions inherited from bmpf::GridPathFinder
	GridPathFinder (const std::shared_ptr< bmpf::Scene > &scene, bool showTrace, int gridSize, int threadCnt=1)

std::vector< double >	coordsToState (std::vector< int > &coords) const

std::vector< double >	coordsToState (std::vector< int > coords, unsigned long robotNum)

bool	checkCoords (std::vector< int > coords)

std::vector< std::vector< double > >	findGridPath (std::vector< int > &startCoords, std::vector< int > &endCoords, int &errorCode)

bool	checkCoords (const std::vector< int > &coords, unsigned int robotNum)

virtual std::vector< int >	stateToCoords (std::vector< double > state)

std::vector< std::vector< double > >	checkTask (const std::vector< double > &startState, const std::vector< double > &endState, double opacity)
	проверка задания проверка задания, возвращает пустой вектор, если маршрут можно строить штатным образом, в противном случае сохраняет в _buildedPath готовый маршрут (из стартового и конечного положений) и возвращает его More...

const std::vector< std::vector< int > > &	getBuildedGridPath () const

const std::vector< double > &	getStartStateFromCoords () const

const std::vector< double > &	getEndStateFromCoords () const

const std::vector< double > &	getGridSteps () const

const double &	getMaxDist () const

const std::vector< std::vector< double > > &	getGroupedGridSteps () const

Public Member Functions inherited from bmpf::PathFinder
	PathFinder (const std::shared_ptr< bmpf::Scene > &scene, bool showTrace, int threadCnt=1)

std::vector< std::vector< double > >	findPath (const std::vector< double > &startState, const std::vector< double > &endState, int &errorCode)

void	updateCollider ()

virtual void	paint (const std::vector< double > &state, bool onlyRobot)

bool	divideCheckPathSegment (const std::vector< double > &prevPoint, std::vector< double > nextPoint, int checkCnt)

int	divideCheckPath (std::vector< std::vector< double >> path, int checkCnt)

bool	simpleCheckPath (const std::vector< std::vector< double >> &path, double maxDist)

bool	checkState (const std::vector< double > &state)
	проверяет доступность состояния Проверяет доступность углов, после проверяет состояние на коллизии More...

std::vector< double >	getRandomState ()

std::vector< double >	getPathStateFromTM (double tm)
	получить состояние по времени две соседние точки считаются разделёнными единичным временным интервалом. More...

bool	checkCollision (const std::vector< double > &state)

void	addObjectToScene (std::string path)

void	deleteObjectFromScene (long robotNum)

double	getPathLength () const

bool	isReady () const

bool	getThreadCnt () const

void	setReady (bool ready)

std::vector< double > &	getStartState ()

std::vector< double > &	getEndState ()

int	getErrorCode () const

const std::vector< std::vector< double > > &	getBuildedPath () const

const std::shared_ptr< bmpf::Scene > &	getScene () const

const std::shared_ptr< bmpf::Collider > &	getCollider () const

double	getCalculationTimeInSeconds () const

Static Public Attributes
static const int	ERROR_REACHED_MAX_NODE_CNT = 4

Static Public Attributes inherited from bmpf::GridPathFinder
static const int	ERROR_CAN_NOT_FIND_FREE_START_POINT = 2

static const int	ERROR_CAN_NOT_FIND_FREE_END_POINT = 3

Static Public Attributes inherited from bmpf::PathFinder
static const int	NO_ERROR = -1

static const int	ERROR_CAN_NOT_FIND_PATH = 1

Protected Member Functions
void	_moveNodeFromOpenedToClosed (const std::shared_ptr< PathNode > &node)

bool	_findCoordsInClosedList (std::vector< int > &coords)

bool	_findCoordsInOpenedList (std::vector< int > &coords)

double	_getPathNodeWeight (std::vector< int > curCoords, std::vector< int > &endCoords)

Protected Member Functions inherited from bmpf::GridPathFinder
virtual std::vector< int >	_findFreePoint (std::vector< int > coords, unsigned long pos, unsigned long maxPos, const std::vector< double > &state)
	поиск координат соседней свободной точки Это - рекуррентный метод, он пробует прибавить -1, 0 и 1 к каждой из координат из интервала [pos, maxPos] включительно More...

Protected Attributes
unsigned int	_kG

unsigned int	_kD

std::shared_ptr< PathNode >	_endNode

std::vector< std::shared_ptr< PathNode > >	_closedNodes

std::unordered_set< long >	_closedStateConvCodeSet

std::set< PathNodePtr >	_openSet

unsigned int	_maxNodeCnt

Protected Attributes inherited from bmpf::GridPathFinder
std::vector< double >	_gridSteps

std::vector< std::vector< double > >	_groupedGridSteps

int	_gridSize

std::vector< int >	_startCoords

std::vector< int >	_endCoords

std::vector< double >	_startStateFromCoords

std::vector< double >	_endStateFromCoords

double	_maxDist

bool	_coordsUsed = false

std::vector< std::vector< int > >	_buildedGridPath

Protected Attributes inherited from bmpf::PathFinder
double	_pathLength {}

int	_errorCode

std::shared_ptr< bmpf::Scene >	_scene

bool	_showTrace

std::shared_ptr< bmpf::Collider >	_collider

bool	_ready

bool	_threadCnt

std::vector< double >	_endState

std::vector< double >	_startState

std::vector< std::vector< double > >	_buildedPath

std::chrono::time_point< std::chrono::system_clock >	_startTime

double	_calculationTimeInSeconds

Additional Inherited Members
Static Public Member Functions inherited from bmpf::PathFinder
static void	infoPath (const std::vector< std::vector< double >> &path)

static Json::Value	getJSONPath (std::vector< std::vector< double >> path)

static std::vector< std::vector< double > >	getPathFromJSON (const Json::Value &json)

static void	savePathToFile (std::vector< std::vector< double >> path, const std::string &filename)

static std::vector< std::vector< double > >	loadPathFromFile (const std::string &filename, std::string &scenePath)

static std::vector< std::vector< std::vector< double > > >	loadPathsFromFile (const std::string &filename, std::string &scenePath)

static std::vector< std::vector< double > >	splitPath (std::vector< std::vector< double >> path, unsigned long partCnt)

static std::vector< double >	getPathStateFromTM (std::vector< std::vector< double >> &path, double tm)
	получить состояние по времени две соседние точки считаются разделёнными единичным временным интервалом. More...

static double	calculatePathLength (std::vector< std::vector< double >> path)

Detailed Description

Базовый класс для всех планировщиков на сетке с нодами Базовый класс для всех планировщиков на сетке с нодами, у каждой из которых указывается предок и координаты на сетке. Суть планирования на сетке заключается в том, что каждой вещественной координате состояния мы ставим в соответствие целочисленную координату из заданного диапазона. В данном планировщике предполагается, что каждая координата состояния разбивается на равное число точек (_gridSize)

Планировщик основан на алгоритме A*, при этом в качестве ошибки используется две величины: ошибка по углам поворота робота и ошибка по положениям звеньев. Им соответствуют коэффициенты kG и kD

Получается, что в данном планировщике одно и тоже пространство конфигураций описывается двумя пространствами: вещественным пространством состояний и целочисленным пространством координат. При этом размерности пространств совпадают.

Методы, реализованные в этом классе, позволяют переходить от вещественного пространства к целочисленному и наоброт.

Логика работы планировщика следующая: 1) подготовка к тактам поиска пути 2) выполнение тактов поиска пути с попутным заполнением тех или иных структур 3) если путь найден из этих структур собирается путь и сохраняется в переменную _buildedPath

Constructor & Destructor Documentation

◆ NodeGridPathFinder()

bmpf::NodeGridPathFinder::NodeGridPathFinder	(	const std::shared_ptr< bmpf::Scene > &	scene,
		bool	showTrace,
		int	gridSize,
		unsigned int	maxNodeCnt,
		unsigned int	kG = `1`,
		unsigned int	kD = `0`,
		int	threadCnt = `1`
	)

inline

конструктор

Parameters

scene	сцена
showTrace	флаг, нужно ли выводить информацию во время поиска пути
maxOpenSetSize	максимальный размер открытого множества
gridSize	размер сетки планирования
maxNodeCnt	максимальное кол-во нод в закрытом множестве
kG	коэффициент разницы в углах поворота сочленений робота
kD	коэффициент разницы в положениях звеньев робота
threadCnt	количество потоков планировщика

Member Function Documentation

◆ _findCoordsInClosedList()

bool NodeGridPathFinder::_findCoordsInClosedList ( std::vector< int > & coords )

protected

поиск координат в списке закрытых нод

Parameters

coords координаты

Returns: флаг, содержатся ли координаты в закрытом списке

◆ _findCoordsInOpenedList()

bool NodeGridPathFinder::_findCoordsInOpenedList ( std::vector< int > & coords )

protected

поиск координат в списке открытых нод

Parameters

coords

Returns: флаг, содержатся ли координаты в открытом списке

◆ _findLinkDistance()

double NodeGridPathFinder::_findLinkDistance	(	std::vector< int > &	a,
		std::vector< int > &	b
	)

получить расстояние между звеньями (по координатам планировщика)

Parameters

a	координаты первой точки
b	координаты второй точки

Returns

◆ _forEachNeighbor()

virtual std::shared_ptr<PathNode> bmpf::NodeGridPathFinder::_forEachNeighbor	(	std::shared_ptr< PathNode >	currentNode,
		std::vector< int > &	endCoords
	)

pure virtual

для всех соседей текущей ноды метод должен добавить только подходящих в множество _openSet, если один из соседей имеет целевые указания (т.е. найден путь), то возвращаем указатель на эту ноду, в противном случае должен быть возвращён nullptr

Parameters

currentNode	текущая нода
endCoords	целевые координаты

Returns: найденная нода или null

Implemented in bmpf::OneDirectionOrderedPathFinder, bmpf::OneDirectionSyncPathFinder, bmpf::OneDirectionPathFinder, and bmpf::AllDirectionsPathFinder.

◆ _getPathNodeWeight()

double NodeGridPathFinder::_getPathNodeWeight	(	std::vector< int >	curCoords,
		std::vector< int > &	endCoords
	)

protected

Получить метрику ноды

Parameters

curCoords	текущие координаты
endCoords	целевые координаты

Returns: метрика ноды

◆ _moveNodeFromOpenedToClosed()

void NodeGridPathFinder::_moveNodeFromOpenedToClosed ( const std::shared_ptr< PathNode > & node )

protected

переместить ноду из открытого множества открытых в множество закрытых

Parameters

node нода

◆ buildPath()

void NodeGridPathFinder::buildPath ( )

overridevirtual

построить путь, построенный путь должен быть сохранён в переменную _buildedPath

Implements bmpf::PathFinder.

◆ findTick()

bool NodeGridPathFinder::findTick ( std::vector< double > & state )

overridevirtual

такт поиска

Parameters

state текущее состояние планировщика

Returns: возвращает true, если планирование закончено

Implements bmpf::PathFinder.

◆ getAllProcessedStates()

std::vector< std::vector< double > > NodeGridPathFinder::getAllProcessedStates ( )

возвращает координаты всех перебранных точек пространства планирования (из закрытого множества)

Returns: координаты всех перебранных точек

◆ getCurrentState()

std::vector< double > NodeGridPathFinder::getCurrentState ( )

получить состояние по первой ноде открытого множества

Returns: текущее состояние

◆ nextNode()

void bmpf::NodeGridPathFinder::nextNode ( )

inline

перейти к следующей ноде из открытого списка

◆ prepare() [1/2]

void NodeGridPathFinder::prepare	(	const std::vector< double > &	startState,
		const std::vector< double > &	endState
	)

overridevirtual

подготовка к планированию

Parameters

startState	начальное состояние
endState	конечное состояние

Reimplemented from bmpf::GridPathFinder.

◆ prepare() [2/2]

void NodeGridPathFinder::prepare	(	std::vector< int > &	startCoords,
		std::vector< int > &	endCoords
	)

overridevirtual

построить путь, построенный путь должен быть сохранён в переменную _buildedPath

Parameters

startCoords	начальные координаты
endCoords	конечные координаты

Reimplemented from bmpf::GridPathFinder.

◆ tryToGetNeighborPtr()

std::shared_ptr< PathNode > NodeGridPathFinder::tryToGetNeighborPtr	(	std::vector< int >	newCoords,
		const std::shared_ptr< PathNode > &	parentNode,
		double	sum
	)

вспомогательный метод, возвращающий указатель на новую ноду только, если её можно создать

Parameters

newCoords	координаты
parentNode	указатель на предка
sum	значение метрики

Returns: указатель на новую ноду

Member Data Documentation

◆ _closedNodes

std::vector<std::shared_ptr<PathNode> > bmpf::NodeGridPathFinder::_closedNodes

protected

список закрытых нод

◆ _closedStateConvCodeSet

std::unordered_set<long> bmpf::NodeGridPathFinder::_closedStateConvCodeSet

protected

неупорядоченное множество свёрток обработанных состояний (так быстрее)

◆ _endNode

std::shared_ptr<PathNode> bmpf::NodeGridPathFinder::_endNode

protected

Последняя нода планирования

◆ _kD

unsigned int bmpf::NodeGridPathFinder::_kD

protected

коэффициент разницы в положениях звеньев робота

◆ _kG

unsigned int bmpf::NodeGridPathFinder::_kG

protected

коэффициент разницы в углах поворота сочленений робота

◆ _maxNodeCnt

unsigned int bmpf::NodeGridPathFinder::_maxNodeCnt

protected

максимальное количество нод

◆ _openSet

std::set<PathNodePtr> bmpf::NodeGridPathFinder::_openSet

protected

Открытое множество нод для обработки (упорядочено по метрике)

◆ ERROR_REACHED_MAX_NODE_CNT

const int bmpf::NodeGridPathFinder::ERROR_REACHED_MAX_NODE_CNT = 4

static

Ошибка планирования: достигнут предельный размер открытого множества

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

project/find_path/find_path_core/include/base/node_grid_path_finder.h
project/find_path/find_path_core/src/base/node_grid_path_finder.cpp

Public Member Functions

Static Public Attributes

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ NodeGridPathFinder()

Member Function Documentation

◆ _findCoordsInClosedList()

◆ _findCoordsInOpenedList()

◆ _findLinkDistance()

◆ _forEachNeighbor()

◆ _getPathNodeWeight()

◆ _moveNodeFromOpenedToClosed()

◆ buildPath()

◆ findTick()

◆ getAllProcessedStates()

◆ getCurrentState()

◆ nextNode()

◆ prepare() [1/2]

◆ prepare() [2/2]

◆ tryToGetNeighborPtr()

Member Data Documentation

◆ _closedNodes

◆ _closedStateConvCodeSet

◆ _endNode

◆ _kD

◆ _kG

◆ _maxNodeCnt

◆ _openSet

◆ ERROR_REACHED_MAX_NODE_CNT