Планирование пути
Чтобы можно было следить за работой планировщиков, написаны демо-приложения
в пакете project.find_path.find_path_core.demo
. Они
используются класс GLScene
.
Почти все нужные методы уже прописаны в нём.
Для его использования необходимо написать класс-потомок, у которого должны быть определены следующие методы:
- инициализация
init()
- рисование
render()
- изменение состояния сцены
changeState()
По пробелу запускается обработка сцены и приостанавливается. Простейший пример
использования GLScene
лежит по адресу: project/core/gl_scene/src/demo_gl_scene.cpp
Красным рисуется начальная конфигурация робота, зелёным - конечная. Синим обозначается текущая перебираемая конфигурация.
Базовый планировщик
Все планировщики пути должны наследовать базовый класс
project.core.find_path.find_path_core.base.PathFinder
.
Он является основным связующим звеном между коллайдерами и сценами.
Конструктор базового планировщика:
/**
* конструктор
* @param scene сцена
* @param showTrace флаг, нужно ли выводить информацию во время поиска пути
* @param threadCnt количество потоков планировщика
*/
PathFinder(const std::shared_ptr<bmpf::Scene> &scene, bool showTrace, int threadCnt = 1);
Хотя на данный момент многопоточные режимы не используются, тем не менее планировщик сразу проектировался как многопоточный. Поэтому в зависимости от количества потоков, он инициализирует в конструкторе коллайдер либо однопоточным, либо синхронным:
if (threadCnt != 1)
_collider = std::make_shared<SolidSyncCollider>(threadCnt);
else
_collider = std::make_shared<SolidCollider>();
Планирование нужно выполнять пошагово для рисования, а обычные задачи требуют сразу сформировать путь:
/**
* Поиск пути
* @param startState стартовое состояние
* @param endState конечное состояние
* @param errorCode в эту переменную записывается код ошибки
* @return построенный путь
*/
std::vector<std::vector<double>>
PathFinder::findPath(const std::vector<double> &startState, const std::vector<double> &endState, int &errorCode) {
_startState = startState;
_endState = endState;
_startTime = std::chrono::high_resolution_clock::now();
prepare(startState, endState);
if (_errorCode != NO_ERROR) {
errorCode = _errorCode;
return {};
}
std::vector<double> actualState;
// если очередной такт поиска пути не последний
while (!findTick(actualState)) {};
if (_errorCode != NO_ERROR)
return {};
// строим путь
buildPath();
auto endTime = std::chrono::high_resolution_clock::now();
_calculationTimeInSeconds =
(double) std::chrono::duration_cast<std::chrono::milliseconds>(endTime - _startTime).count() / 1000;
errorCode = _errorCode;
return _buildedPath;
}
Сначала выполняется подготовка планировщика
/**
* подготовка к планированию
* @param startState начальное состояние
* @param endState конечное состояние
*/
virtual void prepare(const std::vector<double> &startState, const std::vector<double> &endState) = 0;
Потом в бесконечном цикле запускается такт планирования, пока оно не будет окончено:
/**
* такт поиска
* @param state текущее состояние планировщика
* @return возвращает true, если планирование закончено
*/
virtual bool findTick(std::vector<double> &state) = 0;
Когда планирование закончено, вызывается построение пути
/**
* построить путь, построенный путь должен быть сохранён в переменную _buildedPath
*/
virtual void buildPath() = 0;
Все три последних метода должны быть реализованы в потомке, т.к. являются чисто виртуальными.
Сетка планирования
Суть планирования на сетке заключается в том, что каждой вещественной
координате состояния мы ставим в соответствие целочисленную
координату из заданного диапазона.
Данная логика реализована в классе
project.core.find_path.find_path_core.base.GridPathFinder
.
В этом планировщике предполагается, что каждая координата
состояния разбивается на равное число точек (_gridSize
)
Получается, что в данном планировщике одно и тоже пространство конфигураций описывается двумя пространствами: вещественным пространством состояний и целочисленным пространством координат. При этом размерности пространств совпадают.
Путь ищется за счёт перемещения по сетке планирования. Когда путь на сетке найден, каждой его точке сопоставляется состояние робота, и на их основе строится путь.
После того как путь построен, необходимо добавить исходные точки планирования (начальную и конечную) по краям пути.
В таком планировщике добавляются две новые ошибки: не удалось определить стартовую точку на сетке планирования или конечную:
/**
* Ошибка планирования: не удалось найти стартовую точку на
* сетке планирования
*/
static const int ERROR_CAN_NOT_FIND_FREE_START_POINT = 2;
/**
* Ошибка планирования: не удалось найти конечную точку на
* сетке планирования
*/
static const int ERROR_CAN_NOT_FIND_FREE_END_POINT = 3;
В начале планирования состояния переводятся в соответствующие координаты на сетке планирования. Если состояние, соответствующее сетке планирования приводит к коллизии, то ищется соседняя свободная от коллизий точка на сетке планирования.
/**
* @brief поиск координат соседней свободной точки
* Это - рекуррентный метод, он пробует прибавить -1, 0 и 1
* к каждой из координат из интервала [pos, maxPos] включительно
* @param coords координаты исходной точки
* @param pos начало интервала перебора
* @param maxPos конец интервала перебора
* @param state состояние
* @return пустой вектор, если точку не удалось найти, в противном случае - вектор
* координат
*/
std::vector<int> GridPathFinder::_findFreePoint(
std::vector<int> coords, unsigned long pos, unsigned long maxPos, const std::vector<double> &state
) {
if (pos > maxPos) return {};
// рекуррентно для каждой координаты перебираем три варианта: вычесть из неё 1,
// прибавить 1 или ничего не делать; для каждого варианта запускается новый такт рекурсии
// по следующей координате
std::vector<int> tmpCoords = coords;
for (int i = -1; i <= 1; i++) {
tmpCoords.at(pos) = coords.at(pos) + i;
if (checkCoords(tmpCoords))
return tmpCoords;
else {
if (pos < coords.size() - 1) {
auto newCoords = _findFreePoint(tmpCoords, pos + 1, maxPos, state);
if (!newCoords.empty())
return newCoords;
}
}
}
return {};
}
Если среди соседних точек не нашлось свободной от коллизий, то задаётся соответствующий код ошибки и возвращается пустой путь.
Также добавлена возможность строить маршрут по сетке планирования, задавая сразу координаты на ней. При этом всё равно возвращается путь в операционном пространстве:
/**
* поиск пути из состояния `startCoords` в состояние `endCoords`,
*
* @param startCoords начальные координаты
* @param endCoords конечные координаты
* @param errorCode в эту переменную записывается код ошибки
* @return путь по сетке планирования
*/
std::vector<std::vector<double>>
findGridPath(std::vector<int> &startCoords, std::vector<int> &endCoords, int &errorCode);
Ноды планирования
Часть планировщиков работает с использованием нод.
Нода планирования описывается структурой
project.core.find_path.find_path_core.base.PathNode
.
Сам класс планировщика -
project.core.find_path.find_path_core.base.NodeGridPathFinder
.
Нодами называются записи об обработанных точках на сетке планирования и некоторая вспомогательная информация. В данном фреймворке у нод хранится метрика расстояния от стартовой точки до рассматриваемой.
/**
* метрика расстояния от стартовой точки до рассматриваемой
*/
double sum;
Можно вычислять расстояния между углами поворота звеньев,
а также расстояния между положениями звеньев. За эти компоненты отвечают
коэффициенты _kG
(1) и _kD
(0) соответственно. В скобках указаны
значения по умолчанию.
Чтобы не использовать ту или иную компоненту, сделайте соответствующий
коэффициент нулевым.
Также хранится порядок ноды
/**
* сколько нод отделяют рассматриваемую от стартовой
*/
unsigned int order;
Чтобы восстановить путь, у каждой ноды указывается предок
/**
* Указатель на ноду предка
*/
std::shared_ptr<PathNode> parent;
Суть алгоритма планирования с использованием нод сводится к взятию самой удачной ноды из списка на обработку, обработке её и добавлению в список всех её ещё не рассмотренных соседей. Этот список называется открытым множеством.
Чтобы сделать его упорядоченным, необходимо использовать структуру-указатель на ноду с возможностью сравнивания метрик:
/**
* Указатель на ноду с возможностью сравнивания
*/
struct PathNodePtr {
explicit PathNodePtr(std::shared_ptr<PathNode> ptr) : ptr(std::move(ptr)) {}
std::shared_ptr<PathNode> ptr;
bool operator<(const PathNodePtr &obj) const {
return (this->ptr->sum < obj.ptr->sum);
}
};
Множества хранения
В самом планировщике открытое множество хранится в поле _openSet
:
/**
* Открытое множество нод для обработки (упорядочено по метрике)
*/
std::set<PathNodePtr> _openSet;
Чтобы хранить все обработанные точки в закрытом множестве, используется множество на хэшах, при этом в качестве исходного значения хранится свёртка координат
/**
* свернуть координаты coords в скаляр (size - размер СК)
* @param coords координаты
* @param size размер вдоль каждой оси
* @return свёртка
*/
unsigned long bmpf::convState(std::vector<int> &coords, int size) {
unsigned long code = 0;
int m = 1;
for (int coord: coords) {
code += m * coord;
m *= size;
}
return code;
}
Такой алгоритм гарантирует биективное (один к одному) отображение множества координат и множества свёрток, поэтому вместо высчитывания хэша от объекта ноды, высчитывается хэш от свёртки
/**
* поиск координат в списке закрытых нод
* @param coords координаты
* @return флаг, содержатся ли координаты в закрытом списке
*/
bool NodeGridPathFinder::_findCoordsInClosedList(std::vector<int> &coords) {
long code = (long) convState(coords, _gridSize);
return _closedStateConvCodeSet.find(code) != _closedStateConvCodeSet.end();
}
Перебор нод
Все простые планировщики наследуются от NodeGridPathFinder
. В каждом из
них реализуется метод
/**
* для всех соседей текущей ноды метод должен добавить только
* подходящих в множество _openSet, если один из соседей
* имеет целевые указания, то возвращаем указатель на эту ноду,
* в противном случае должен быть возвращён nullptr
* @param currentNode
* @param endCoords
* @return
*/
virtual std::shared_ptr<PathNode> _forEachNeighbor(
std::shared_ptr<PathNode> currentNode, std::vector<int> &endCoords
) = 0;
Иными словами, для реализации планировщика на основе NodeGridPathFinder
,
требуется описать, как именно на основе текущей ноды следует перебирать всех
её соседей.
Такой алгоритм перебора принято называть
Более подробно об алгоритмах планирования подобного рода можно прочитать здесь.