Close

28.11.2017

Скрипт генерации .world и .launch файлов для среды визуализации роботов ROS — Gazebo + модуль управления роботами типа Turtlebot

 

Постановка задачи

 

Кратко: необходимо визуализировать результат работы алгоритма поиска путей с помощью ROS-Gazebo.

Задача состояла из двух частей:

— из исходных данных, полученных от алгоритма, рассчитывающего траекторию движения роботов по некоторому полю с учетом передвижения других роботов и препятствий, написать генератор необходимых файлов для среды Gazebo

— Написать дополнительный модуль, который будет посылать команды роботам через ROS на передвижение согласно исходным данным.

 

На входе — xml файл, содержащий в себе:

  • количество роботов
  • позиции роботов
  • скорость движения роботов
  • размер одной клетки относительно стандартной клетки в системе координат Gazebo  для данной сцены
  • позиции препятствий на поле
  • блоки команд, которые были посланы роботам

 

Немного об инструментах

 

ROS (Robot Operating System) — фреймворк для программирования роботов, предоставляющий функционал для распределенной работы. ROS обеспечивает стандартные службы операционной системы, такие как:

  • аппаратную абстракцию
  • низкоуровневый контроль устройств
  • реализацию часто используемых функций
  • передачу сообщений между процессами
  • управление пакетами.

 

ROS основан на архитектуре графа, где обработка данных происходит в узлах, которые могут получать и передавать сообщения между собой. Библиотека ориентирована на Unix-подобные системы. Я использовал Linux Ubuntu 16.04.

Gazebo — опенсорсный симулятор роботов. Тесно взаимодействует с ROS, что позволяет протестировать робота, без самого робота 🙂

 

Поставленная задача решалась на языке Python 2.7 с использованием дополнительных библиотек.

Были избраны следующие доп. библиотеки:

  • stringtemplate3 — мощная библиотека для генерации файлов согласно представленным шаблонам.
  • antlr 2.7 — необходима для работы stringtemplate3

 

Решение поставленной задачи

 

Весь проект был разбит на 3 отдельных пакета — pathplanning_gazebo, pathplanning_generator, pathplanning_mover. Это было сделано для удобного запуска проекта и удобной структуризации всех файлов проекта, дабы не пришлось что-либо искать при необходимости.

 

Генерация

Gazebo для визуализации сцены использует файлы разметки. Основные файлы имеют расширения .world и .launch.

.world файл представляет собой статические объекты внутри сцены. Там должны были быть описаны все препятствия, полученные из входных данных. Для генерации этого файла были применены  — корневой stringtemplate шаблон, представляющий собой root-элементы разметки, и внутренний stringtemplate шаблон, представляющий собой одно препятствие с задаваемыми параметрами: позиция в двумерной декартовой системе координат и высота. Расположение препятствий на определенной высоте не предусмотрено, поэтому ось y трехмерной системы координат в данном случае не учитывалась — все объекты появляются на земле.

.launch файл отличается тем, что в нем содержатся динамические объекты — сами роботы, а также описание узлов, необходимых для работы всей системы в целом. Помимо динамических объектов — роботов , .launch файл содержит ссылку на файл мира со статическими объектами (.world), описание вызова различных узлов для управления, считывания специфических данных (в газебо можно использовать симуляторы различных датчиков — это может быть гироскоп, лазерный сканер для определения препятствий и т.д.). Однако, .launch может не содержать внутри себя ни одного динамического объекта: например, я использовал .launch для вызова необходимого количества узлов управления роботами.

 

Эта подзадача была простой: она представляла собой простой парсинг принятого .xml файла, разбиение его на части и формирование на его базе множества других файлов.

Конечно, в бочке меда есть ложка дёгтя — например, stringtemplate3 в качестве ключевого символа использует “$”. В .launch файлах он-же используется для передачи аргументов или команд. В результате я столкнулся с конфликтами во время генерации. Некоторые файлы пришлось всё же генерировать не с помощью шаблонов, а захардкоженными кусками шаблона.

 

Так же, нужно было решить, как именно мы запишем команды для роботов на передвижение, и как мы эти команды впоследствии передадим в модуль управления.

Было принято решение сохранять команды по стандарту CSV — он хорошо подходил конкретно для нашего кейса.

 

В результате работы по данной части задачи я сделал скрипт, считывающий данные из указанной папки, и на базе него генерирующий следующий список файлов:

 

  • playground.world — статика
  • turtlebot_world.launch — динамические объекты и другие необходимые базовые объекты (базовый мир, земля, физика)
  • {порядковый номер робота}.csv — список команд, которые требуется передать на вход в модуль управления. Каждая строка файла — команда, имеющая структуру типа {стартовая позиция x},{стартовая позиция y},{конечная позиция x},{конечная позиция y},{время, выделенное на выполнение этой команды}. Один файл на одного робота.
  • run_agents.launch — скрипт, запускающий необходимое количество экземпляров модуля управления, в качестве аргументов которым передается порядковый номер робота и размер одной единицы относительно одной единицы в стандартной системе координат Gazebo (размер клетки может меняться в зависимости от того, который будет указан в полученном логе работы алгоритма)

 

Управление роботами

Данная подзадача решалась в несколько этапов. В качестве первого этапа нужно было добиться того, что мы сможем хоть как-то подвигать робота по сцене из кода.

Пришлось изрядно штудировать документацию по ROS.

 

В результате мы поняли, как использовать узел Publisher из кода,  как слать сообщения в определенный topic (читай — канал связи), и смогли из кода сдвинуть робота с места.

Дальше началось самое интересное — сложные траектории и повороты. Выяснилось, что задача не такая уж тривиальная, как мы предполагали: появились весомые нюансы, замедляющие продвижение к поставленной цели. Мы столкнулись с проблемами, а именно — с существованием физики в Gazebo: робот терял устойчивость на поворотах, его заносило, и он начинал двигаться вокруг своей оси. Плюс, из-за инерции появилась дополнительная проблема — погрешность. Так, робот проезжал большее расстояние, поворачивал на больший угол чем нужно, из-за чего проходимый маршрут (в нашем случае — мы пробовали описать “квадрат”) вовсе не совпадал с нашими ожиданиями. Прилагаю ссылку на форум с аналогичной проблемой. Уменьшение скорости передвижения частично избавляло от проблемы, но добавляло другую — мы не укладывались во время, выделенное на выполнение команды. В таком случае, роботы во время движения могут столкнуться. Отключение физики вело к совершенно неадекватному поведению — они просто взлетали или переставали ездить.

Но это лишь одна из проблем: так же конкретно в нашем случае, по неизвестной причине (предположительно из-за специфики физического движка Gazebo) — роботы очень медленно смещались по всем осям, даже без какого-либо взаимодействия на них. Смещение едва заметно без движения — примерно по 0.002 в секунду, однако при попытке даже просто провести робота по прямой на расстояние 8-10 клеток — он смещается правее, описывая не прямую, а дугу.

На эту проблему ссылку так же прилагаю.

Пришлось пересмотреть алгоритм перемещения.

Алгоритм перемещения был переписан под топик set_model_state, что позволяло изменить местоположение робота, но не с помощью его колес, а буквально “телепортируя” его в нужную точку.

Но как реализовать плавное перемещение, если нам известны только начальная и конечная точка? Просчитаем промежуточные точки, используя школьные знания линейной алгебры, а конкретно — линейное уравнение y = kx + b. Вот так:

def calculate_coords(self, command):
#command - список, хранящий в себе угол поворота, координаты начальной и конечной точек
# находим k и x для формулы линейного уравнения y = kx +b
points = [(command[2], command[3]), (command[4], command[5])] # известные точки
x_coords, y_coords = zip(*points)
A = vstack([x_coords, ones(len(x_coords))]).T
m, c = lstsq(A, y_coords)[0]
# если стартовые позиции численно больше конечных - нам нужно двигаться влево или вниз
if command[2] > command[4]:
deltax = -0.02
elif command[2] < command[4]: deltax = 0.02 # delta нельзя присвоить 0, тк итератор должен быть ненулевым числом # на самом деле не важно, тк если начальная точка равна конечной - до этой строки мы даже не дойдем else: deltax = 0.02 # то же самое касается и y if command[3] > command[5]:
deltay = -0.02
elif command[3] < command[5]:
deltay = 0.02
else:
deltay = 0.02
# определяем список, который будет хранить в себе промежуточные точки
array_of_coords = []
x = command[2]
y = command[3]
# пробегаем по x от стартовых до конечных позиций, с шагом deltax
for x in np.arange(command[2], command[4], deltax):
print x
y = m * x + c
x += deltax
array_of_coords.append([x, y])
# если x примерно равна finish x: ( равна быть не может, незначительная погрешность всё равно есть)
if (command[4] - 0.005) < x < (command[4] + 0.005):
# пробегаем по y, инкрементируя его на дельту. тк
# x не изменяется - просто изменяем y
for y in np.arange(y, command[5], deltay):
y += deltay
array_of_coords.append([x, y])
print array_of_coords
# возвращаем готовый список промежуточных точек
return array_of_coords

 

Таким образом, циклически перемещая модели по полю на малое расстояние, мы получили видимость плавного перемещения. Это не самое красивое решение — однако оно удовлетворяло условиям поставленной задачи.

 

Результаты проведенной работы:

 

YouTube — движение 16 роботов по сцене с препятствиями

 

Роботы с препятствиями:

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *