В этом репозитории есть реализация алгоритма планирования траектории в среде с динамическими препятствиями Sipp и его модификации Anytime Sipp, визуализатор работы алгоритма. Реализация протестирована на большой выборке тестов, сгенерированных рандомно. Результаты тестирования также доступны в репозитории в ноутбуке. Примеры заданий, результатов работы алгоритмов и видео можно найти здесь.
den101d_200_1_log.mp4
- Examples
- Постановка задачи
- Алгоритм Sipp и Anytime Sipp
- Чекер
- Визулизация
- Тестирование и его результаты
- Ссылки
Директория содержит 4 папки:
- tasks - задания. Подробнее о том, в каком виде должно быть представлено задание, написано ниже.
- logs - решения этих заданий алгоритмом Sipp
- logs_anytime - решения этих заданий алгоритмом Anytime Sipp
- videos - визуализация найденого пути алгоритмом Sipp
Можно прогнать эти тесты с помощью команды (Запустятся алгоритмы на заданиях, и чекер на полученных результатах):
ctest -R 'examples'Вывод будет такой:
Start 1: examples
1/2 Test #1: examples ......................... Passed 0.65 sec
Start 2: examples/anytime
2/2 Test #2: examples/anytime ................. Passed 0.88 sec
100% tests passed, 0 tests failed out of 2
Total Test time (real) = 1.53 sec
Пусть есть некоторая среда – двумерное поле со свободными и занятыми клетками, где занятые клетки – статические препятствия. На этом поле также есть динамические препятствия. У них нет размеров, поэтому будет считать, что это точки. Динамические препятствия двигаются равномерно и прямолинейно между клетками с общей стороной, причем из любой клетки они могут переходить только в 4 направлениях: вверх, вниз, вправо, влево. Переход между клетками занимает 1 единицу времени. При целом числе единиц времени любое динамическое препятствие должно находиться в центре какой-нибудь клетки.
Есть агент, для которого нужно найти путь из данной клетки S (start) в данную клетку F (finish). У агента нет размеров, поэтому тоже считаем, что агент – точка. Агент двигается равномерно и прямолинейно только по свободным клеткам, и так же, как и динамические препятствия, может переходить между клетками с общей стороной только в тех же 4 направлениях. Аналогично динамическим препятствиям, агент перемещается в соседнюю клетку за 1 единицу времени, и при целом числе времени должен находиться в центре какой-нибудь клетки.
Задача: для агента нужно найти путь из клетки S в клетку F, проходящий только по свободным клеткам, так что в любой момент времени (необязательно целый) координаты агента (необязательно целые) не совпадают ни с каким динамическим препятствием.
На рисунке пример поля на старте, белые клетки – свободные, черные – занятые. Красная точка – искомый агент, стоит на старте. Клетка, помеченная буквой F, – финиш, куда нужно прийти агенту. 3 синие точки – динамические препятствия на своих стартовых позициях.
GCC 10 или выше Apple LLVM version 11.0.3 (clang-1103.0.32.62) или выше Boost
mkdir Build
cd build
cmake ..
make SippSearchНаходимся в директории Build.
./Src/Planning/SippSearch input.xml [-w hweight] [-l logLevel]В аргументах командной строки нужно передать путь от Build до xml-файла с заданием либо до директории с заданиями (тогда сразу протестируются все задания из этой директории). Если передать параметр hweight - начальный вес эвристики - запустится алгоритм Anytime Sipp. Если этого параметра не будет,то Sipp. Опционально, можно передать уровень логирования (logLevel):
- -1 - в консоль ничего не выведится
- 0 - только путь до файла с заданием или пути, если был передан путь до директории
- 1 - выведутся результаты (основные показатели: pathlength, nodescreated, numberofsteps, searchtime)
- 2 - всё, что в предыдущих, а также слова, сигнализирующие о конце какого-то этапа работы программы
- 3 - всё, что в предыдущих, а также на каждом шаге алгоритма будут выводиться параметры текущего node и списки безопасных интервалов его successors
./Src/Planning/SippSearch ../Examples/tasks/ # запуск Sipp на всех тестах, лежащих в директории ../Examples/tasks/
./Src/Planning/SippSearch ../Examples/tasks/ -w 2 # запуск Anytime Sipp на тех же тестахПримеры заданий можно найти здесь. Теперь подробнее о значении тегов:
- <map> – тег открывает секцию, описывающую карту
- <height> и <width> – высота и ширина карты соответственно.
- <startx>, <starty> – координаты старта агента, для которого будет строиться путь
- <finishx>, <finishy> – координаты финиша агента
- <grid> – описание свободных и занятых статическими препятствиями клеток карты.
- <row> – одна строчка карты, значение клетки может быть равным 1, если в клетке статическое препятствие, или 0, если клетка свободна
- <dynamicobstacles> - описание траекторий движения динамических препятствий
- <obstacle> – внутри этого тега описание пути препятствия, таких тегов может быть несколько в теге <dynamicobstacles>
Траектория пути представляется в виде последовательности троек из целых неотрицательных чисел (x, y, time) – координаты клетки и время, в которое препятствие или агент там находится. Эта тройка записывается в XML файле как атрибуты тега <point>. Любые 2 соседние тройки должны образовывать либо прямолинейный отрезок пути так, что время движения между этими координатами должно быть в точности равно разнице координат времени в этих тройках, либо координаты клетки в соседних тройках должны быть равны, а координаты времени отличаются на целое число больше 0.
Пусть на рисунке траектория движения препятствия (или агента), начинающаяся в клетке (3, 2), конец – в клетке (0, 0). Пусть также в клетке (2, 1), он ждал 2 единицы времени. Тогда будет записано так:
<obstacle id="1">
<point x="3" y="2" time="0"/>
<point x="3" y="1" time="1"/>
<point x="2" y="1" time="3"/>
<point x="2" y="1" time="5"/>
<point x="0" y="1" time="7"/>
<point x="0" y="0" time="8"/>
</obstacle>Находимся в директории Build
Сборка:
make GenerateЗапуск:
./Tests/Generate maps_directory tasks_directory a_1 a_2 a_3 ... a_nДля каждой карты из директории maps_directory генерирует по 5 тестов с a_i количеством динамических препятствий, все тесты кладет в директорию tasks_directory. Здесь карта - это файл, строго соблюдающий следующий формат:
type octile
height 4
width 5
map
...11
...11
11.11
11...После слов height и width положительные числа, равные высоте и ширине карты соответственно. После строки "map" идет описание карты: количество строк равно высоте карты, количество символов в каждой строке равно ширине карты. Символ '.' означает пустую клетку, любой другой непробельный символ - занятая клетка.
Тесты на городских картах размера 256x256 с маленьким числом динамических препятствий(10, 20, 50, 100 и 200), лежащие в папке, были сгенерированы следующей командой:
./Tests/Generate ../TestsData/City/tests_256/maps/ ../TestsData/City/tests_256/small_tasks/tasks/ 10 20 50 100 200make Check./Tests/Check logs_directorylogs_directory - директория с файлами, полученными на выходе при работе программы SippSearch. Проверяется, что все полученные пути агентов записаны в правильном виде, действительно начинаются и заканчиваются в клетках старта и финиша соответственно, а также не пересекают ни статические, ни динамические препятствия.
Даны минимальные версии: Python 3.10 BeautifulSoup4 4.10.0 ffmpeg 1.4 Matplotlib 3.5.1 NumPy 1.22.2
Python3 ../Src/Visualization/visualize.py log_file.xml [-o output_file.mp4]log_file.xml - файл, полученный работой Sipp. Опционально, можно передать путь, куда будет записнао видео. По умолчанию, оно появляется в той же директории, что и log_file.xml
Примеры визуализаций можно найти здесь На всех видео синими точками обозначены динамические препятствия, красной - агент, для которого ищется путь. Белые клетки - свободные, черные - занятые.
den101d_200_1_log.mp4
Были сгенерированы тесты на картах размеров 256x256, 512x512 и 1024x1024 (по 30 карт каждого размера), для каждой из карт было сгенерировано задание с 10, 20, 50, 100, 200, 500, 1000 динамических препятствий (по 5 тестов для каждой конфигурации). Итого получилось 3150 тестов. Sipp запускался на этих 3150 тестах, а Anytime Sipp на каждом из этих тестов по 3 раза: с разными начальными весами эвристики (2, 5, 10). Результаты тестирования оформлены в качестве таблиц и графиков в ноутбуке.
Запустить алгоритм поиска пути sipp и чекер, проверяющий полученный результат на всех 3150 заданиях, лежащих в репозитории, можно следующей командой:
ctest -R 'run'Результат работы будет примерно таким:
Test project
Start 1: preparation/run
1/10 Test #1: preparation/run .................. Passed 0.01 sec
Start 2: 256/small_tasks/run
2/10 Test #2: 256/small_tasks/run .............. Passed 150.37 sec
Start 3: 256/tasks_500/run
3/10 Test #3: 256/tasks_500/run ................ Passed 170.99 sec
Start 4: 256/tasks_1000/run
4/10 Test #4: 256/tasks_1000/run ............... Passed 385.77 sec
Start 5: 512/small_tasks/run
5/10 Test #5: 512/small_tasks/run .............. Passed 426.71 sec
Start 6: 512/tasks_500/run
6/10 Test #6: 512/tasks_500/run ................ Passed 384.81 sec
Start 7: 512/tasks_1000/run
7/10 Test #7: 512/tasks_1000/run ............... Passed 779.00 sec
Start 8: 1024/small_tasks/run
8/10 Test #8: 1024/small_tasks/run ............. Passed 1696.57 sec
Start 9: 1024/tasks_500/run
9/10 Test #9: 1024/tasks_500/run ............... Passed 1451.54 sec
Start 10: 1024/tasks_1000/run
10/10 Test #10: 1024/tasks_1000/run .............. Passed 3027.33 sec
100% tests passed, 0 tests failed out of 10
Total Test time (real) = 8473.15 secКоманды, выполняющиеся в каждом тесте, можно посмотреть здесь. Обратите внимание на время тестирования! Из-за большого числа тестов (3150) оно довольно большое. Если не хочется ждать, можно запустить меньшее число тестов, заменив 'run' на часть имени или полное имя теста. Например, прогоним один блок тестов, где проверим работу на картах 256x256 c маленьким числом динамических препятствий (до 200), то есть программа будет запущена всего на 750 заданиях, а время работы этого блока тестов не более 5 минут:
ctest -R '256/small_tasks/run'Либо, если не хочется ждать, можно запустить блоки тестов, в которых не планируются траектории, а только запускается чекер, проверяющий уже полученные мной результаты работы алгоритма, лежащие в репозитории. Здесь проверяется, что все полученные пути агентов записаны в правильном виде, действительно начинаются и заканчиваются в клетках старта и финиша соответственно, а также не пересекают ни статические, ни динамические препятствия.
ctest -R 'check'В консоли видим примерно такой результат:
Test project
Start 11: 256/small_tasks/check
1/9 Test #11: 256/small_tasks/check ............ Passed 20.78 sec
Start 12: 256/tasks_500/check
2/9 Test #12: 256/tasks_500/check .............. Passed 17.12 sec
Start 13: 256/tasks_1000/check
3/9 Test #13: 256/tasks_1000/check ............. Passed 39.78 sec
Start 14: 512/small_tasks/check
4/9 Test #14: 512/small_tasks/check ............ Passed 49.89 sec
Start 15: 512/tasks_500/check
5/9 Test #15: 512/tasks_500/check .............. Passed 27.52 sec
Start 16: 512/tasks_1000/check
6/9 Test #16: 512/tasks_1000/check ............. Passed 53.43 sec
Start 17: 1024/small_tasks/check
7/9 Test #17: 1024/small_tasks/check ........... Passed 163.67 sec
Start 18: 1024/tasks_500/check
8/9 Test #18: 1024/tasks_500/check ............. Passed 59.60 sec
Start 19: 1024/tasks_1000/check
9/9 Test #19: 1024/tasks_1000/check ............ Passed 93.41 sec
100% tests passed, 0 tests failed out of 9
Total Test time (real) = 525.22 sec- SIPP: Safe Interval Path Planning for Dynamic Environments
- Anytime Safe Interval Path Planning for Dynamic Environments
- Sturtevant, N. R. Benchmarks for grid-based pathfinding // IEEE Transactions on Computational Intelligence and AI in Games, vol.4,n.2 – 2012 - с.144–148