Добрый день уважаемые читатели. В своей последней статье я рассказывал о двух алгоритмах SLAM, предназначенных для камер глубины: rtabmap и RGBD-SLAM. Тогда мы попробовали только построение карты местности. В данной статье я расскажу о возможностях локализации и навигации робота с использованием алгоритма rtabmap. Кого заинтересовало, прошу под кат.

Локализация в rtabmap с визуализацией в rviz

Для начала если не запущен ROS мастер, то запускаем его:

roscore

Запускаем драйвер для камеры:

roslaunch freenect_launch freenect.launch depth_registration:=true

Запустим rtabmap для построения карты с визуализацией в rviz и удалением старой карты:

roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rtabmapviz:=false rviz:=true

Когда процедура построения карты закончена, завершим работу программы с помощью Ctrl+C для сохранения карты и перезапустим rtabmap в режиме локализации:

roslaunch rtabmap_ros rtabmap.launch localization:=true rtabmapviz:=false rviz:=true

Для того, чтобы показать карту в rviz необходимо нажать галочку в пункте Download Map на дисплее MapCloud:





Переместим камеру в то место, где находится робот на карте в rviz. После этого будет опубликована трансформация между системами координат /map > /odom.

Построение карты и навигация с использованием rtabmap на примере Turtlebot

Построение карты и локализацию робота методом rtabmap можно попробовать на симуляторе робота Turtlebot. В rtabmap есть для этого специальные пакеты. Для дополнительной информации даю ссылку на источник материала. Для начала установим необходимые пакеты:

sudo apt-get install ros-<ros_version>-turtlebot-bringup ros-<ros_version>-turtlebot-navigation ros-<ros_version>-rtabmap-ros

По умолчанию используется драйвер OpenNI2, который указан в файле 3dsensor.launch (TURTLEBOT_3D_SENSOR=asus_xtion_pro). В своих экспериментах я использовал камеру Microsoft Kinect. Давайте установим драйвер для него в переменной TURTLEBOT_3D_SENSOR:

echo ‘export TURTLEBOT_3D_SENSOR=kinect’ >> ~/.bashrc
source ~/.bashrc

Сейчас запустим построение карты, используя специальные пакеты для симулятора Turtlebot в rtabmap_ros:

roslaunch turtlebot_bringup minimal.launch
roslaunch rtabmap_ros demo_turtlebot_mapping.launch args:="--delete_db_on_start" rgbd_odometry:=true
roslaunch rtabmap_ros demo_turtlebot_rviz.launch

Мы увидим в окне rviz:



А теперь с облаком точек с камеры Kinect:



Вы должны увидеть двухмерную карту, трехмерную карту и еще некоторые выходные данные топиков, необходимые для навигации. Кроме того, в панели дисплеев слева отображаются стандартные дисплеи rviz и дополнительные спечифичные для rtabmap (Local Map, Global Map, Rtabmap Cloud).



Переместим камеру в пространстве и через некоторое время мы увидим:



По умолчанию rtabmap использует ту же самую базу данных при каждом запуске demo_turtlebot_rviz.launch. Чтобы удалить старую карту и начать построение карты заново (с чистого старта), можно либо вручную удалить базу данных, сохраненную в файле ~/.ros/rtabmap.db, либо запустить demo_turtlebot_rviz.launch с аргументом args:="--delete_db_on_start".

Запустим построение карты без удаления базы данных:

roslaunch rtabmap_ros demo_turtlebot_mapping.launch

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

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

Теперь у нас есть карта, которая сохранена в базе данных в файле ~/.ros/rtabmap.db. Перезапустим demo_turtlebot_mapping.launch в режиме локализации с аргументом localization:=true:

roslaunch rtabmap_ros demo_turtlebot_mapping.launch rgbd_odometry:=true localization:=true

Мы увидим такую картинку:



Переместим Kinect в сторону. Робот определит свое новое местоположение как только обнаружит цикл (loop closure):



Для успешной детекции цикла потребуется время, поэтому наберитесь терпения.
Работает довольно быстро:

Автономная навигация с Turtlebot

Теперь, когда карта создана, можно попробовать автономную навигацию на известной карте с использованием Navigation stack. Для этого нужно просто указать для робота текущую цель на карте в rviz, в направлении которой должен двигаться робот в автономном режиме. Обо всем остальном позаботится Navigation stack. Нам для начала достаточно нажать кнопку 2D Nav Goal в rviz



и указать цель кликнув в произвольном месте на карте



Добавим дисплей Pose, выбрав для него топик “/move_base/current_goal” и дисплей Path выбрав топик “/move_base/NavfnROS/plan”. Вы должны увидеть запланированный путь (зеленая линия на карте) и установленную цель, обозначенную красной стрелкой:



Пакет move_base отвечает за управление роботом в направлении цели. Он будет публиковать команды движения типа geometry_msgs/Twist в топик /mobile_base/commands/velocity:

rostopic echo /mobile_base/commands/velocity

Дополнительную информацию о работе с Navigation stack можно найти в туториале на официальной странице turtlebot_navigation. Для использования rtabmap на своем собственном роботе есть также туториал.

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