Построение карты и локализация мобильного робота в ROS без одометрии с использованием laser_scan_matcher

Добрый день хабрачитатели!
Мы уже однажды коснулись темы локализации и SLAM в статье о Hector SLAM. Продолжим знакомство с алгоритмами построения карт местности и локализации в ROS. Сегодня мы попробуем построить карту местности без источника одометрии, используя только лидар Hokuyo URG-04LX-UG01 и алгоритм gmapping и локализовать робота на построенной карте с помощью алгоритма amcl. В этом нам поможет laser_scan_matcher. Кому интересно, прошу под кат.

Установка пакета laser_scan_matcher

Итак, начнем! Для экспериментов будем использовать ROS Indigo, но вы можете использовать другую версию ROS (Jade, Kinetic, Hydro). Установка пакетов должна выполняться аналогично (возможно только в ROS Kinetic не будут доступны некоторые пакеты через apt-get).
Пакет laser_scan_matcher это инструмент для инкрементальной регистрации данных лазера, который реализован на основе метода Canonical Scan Matcher, о котором можно почитать здесь. О пакете можно прочитать здесь. Пакет может быть использован без данных одометрии, он сам выполняет оценку одометрии.
Установим пакет:

sudo apt-get install ros-indigo-laser-scan-matcher 

Попробуем демо:

roscore roslaunch laser_scan_matcher demo.launch 

Мы увидим нечто подобное в rviz:

Здесь показан laser_scan_matcher в действии на данных лидара, записанных в bag файл.
Теперь попробуем пакет вживую. Чтобы не терять время, сразу попробуем его с алгоритмом gmapping. Создадим пакет с файлом my_gmapping_launch.launch для запуска gmapping:

cd ~/catkin_ws/src catkin_create_pkg my_laser_matcher cd src/my_laser_matcher mkdir launch vim src/my_gmapping_launch.launch 

Скопируйте следующий код в файл my_gmapping_launch.launch:

Код my_gmapping_launch.launch

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 

Здесь мы запускаем static_transform_publisher из пакета tf для публикации трансформации между системами координат base_link > laser, узлы laser_scan_matcher и slam_gmapping.
Исходный код файла можно скачать отсюда.
Для использования лидара Hokuyo нам нужно установить пакет ROShokuyo_node:

sudo apt-get install ros-indigo-hokuyo-node 

Запустим узел getID из пакета hokuyo_node чтобы получить информацию о лидаре:

rosrun hokuyo_node getID /dev/ttyACM0 

Может возникнуть ошибка:

Error: Failed to open port. Permission denied. [ERROR] 1263843357.793873000: Exception thrown while opening Hokuyo. Failed to open port: /dev/ttyACM0. Permission denied (errno = 13). You probably don't have premission to open the port for reading and writing. (in hokuyo::laser::open) You may find further details at http://www.ros.org/wiki/hokuyo_node/Troubleshooting 

В этом случае нам нужно добавить права доступа для порта /dev/ttyACM0:

sudo chmod a+rw /dev/ttyACM0 

Запустим getID из пакета hokuyo_node еще раз и получим подобный вывод:

Device at /dev/ttyACM0 has ID H0906078 

Теперь запустим узел hokuyo_node:

rosrun hokuyo_node hokuyo_node 

И наконец запустим наш лаунч файл my_gmapping_launch.launch:

roslaunch my_laser_matcher my_gmapping_launch.launch rosrun rviz rviz 

Выведем список топиков:

rostopic list 

Среди топиков мы увидим следующие:

/initialpose /move_base_simple/goal /odom /pose2D ... /imu/data 

Таким образом мы получаем одометрию и позицию робота благодаря laser_scan_matcher.
Добавим в rviz дисплей типа LaserScan с топиком /scan так, как это описано в статье. Также добавим дисплей для карты Map и для трансформации TF. Развернем секцию TF и внутри нее Frames, затем отметим пункты: odom, map, base_link. Напомню, что это системы координат одометрии, карты и робота соответственно. Не забудьте установить значение /map для поля Fixed frame в левой панели Displays в секции Global options.
В rviz мы увидим подобную картинку:

Построение карты и локализация мобильного робота в ROS без одометрии с использованием laser_scan_matcher

Дальше просто перемещаем робота в пространстве для построения полной карты местности. Используем утилиту map_saver из пакета map_server для сохранения карты:

rosrun map_server map_saver 

Локализация с amcl

Сейчас попробуем локализовать робота с помощью алгоритма amcl. Создадим файл my_localize.launch внутри нашего пакета со следующим содержимым:

Код my_localize.launch

                                                                                                                                                                                

Здесь мы аналогично лаунчеру для gmapping публикуем трансформацию /laser → /base_link с помощью узла static_transform_publisher, узлы hokuyo_node и laser_scan_matcher. Затем мы запускаем map_server для публикации нашей построенной карты, где в args передаем путь до карты в файле yaml. Наконец запускаем узел amcl с параметрами. О параметрах amcl можно прочитать на официальной странице алгоритма.
Код лаунч файла также можно скачать из github репозитория.
Запустим наш лаунч файл:

roslaunch my_laser_matcher my_localize.launch 

Теперь перейдем в rviz. Установим значение map для Fixed frame в секции Global options. Выведем список топиков:

rostopic list 

В списке появятся новые топики:

... /amcl/parameter_descriptions /amcl/parameter_updates /amcl_pose ... /map /map_metadata /map_updates ... /particlecloud 

Топик amcl_pose соответствует позиции робота, публикуемой amcl.
Посмотрим сообщения в топике:

rostopic echo /amcl_pose 

Получим данные о позиции робота:

header:    seq: 15   stamp:      secs: 1482430591     nsecs:  39625000   frame_id: map pose:    pose:      position:        x: 0.781399671581       y: 0.273353260585       z: 0.0     orientation:        x: 0.0       y: 0.0       z: -0.636073020536       w: 0.771628869694   covariance: [0.2187289446708912, -0.010178711317316846, 0.0, 0.0, 0.0, 0.0, -0.010178711317316819, 0.23720047371620548, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.07106236846890918] --- 

В rviz мы получим такую картинку:

image

Как видим точки скана с лидара частично совпадают со стенами на карте. Вот как выглядела моя установка в действительности:

image

Попробуем переместить робота. Позиция робота и точка обзора карты должны одновременно измениться в окне rviz. После перемещения робота позиция робота может быть определена алгоритмом amcl не точно. Нам нужно выполнить корректировку положения робота с помощью инструмента 2D Pose Estimate. Для этого нажмем в верхней панели инструментов в rviz на кнопку 2D Pose Estimate, кликнем на приблизительную точку центра системы координат робота на карте в rviz (система координат base_link) и будем удерживать зажатой кнопку мышки. Появится зеленая стрелка, исходящая из центра робота:

image

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

image

Мы получим такие сообщения в терминале, где запущен my_localize.launch:

[ INFO] [1482431993.717411186]: Setting pose (1482431993.717383): -0.413 -0.071 0.057 

На коротком видео можно посмотреть все в действии:

Топик /particlecloud представляет данные о неопределенности положения робота в виде ориентированных позиций (Pose) или так называемое облако частиц. Тип сообщений — geometry_msgs/PoseArray.

Добавим дисплей по названию топика /particlecloud:

image

В rviz отобразится облако частиц в виде густого скопления красных стрелок:

image

Чем гуще скопление частиц, тем выше вероятность положения робота в этой позиции.
Об инструменте 2D Pose estimate, облаке частиц и других понятиях в amcl можно прочитать в туториале на портале ros.org.

На этом все! Все рассмотренные алгоритмы (gmapping и amcl) являются частью большого стека Navigation stack в ROS, о нем можно найти много информации в Интернете. Сегодня мы попробовали в действии инструмент laser_scan_matcher, алгоритмы построения карт gmapping и локализации amcl. Теперь можно легко начать работать над локализацией и навигацией мобильного робота и создать полностью автономного робота, способного ориентироваться в пространстве без необходимости в ручном управлении. Желаю всем удачи в экспериментах и до новых встреч!

PS: Всех с наступающим 2017 годом!


Источник

localization, mapping, ros, SLAM

Читайте также