Эта часть цикла статей по навигации домашнего автономного робота на базе open-source linorobot будет суховата на картинки, так как будет большей частью посвящена теории. «Теория, мой друг, суха, но зеленеет жизни древо», -как говорил классик. Заглянем под капот linorobot, разберем подробно составляющие его навигационного стека ROS, а также n-е количество параметров, стандартно используемых в ROS.

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

Предыдущие статьи цикла:

Робот-тележка 2.0. Часть 2. Управление в rviz и без. Элементы красоты в rviz
Робот-тележка 2.0. Часть 1. Автономная навигация домашнего робота на базе ROS



Итак, при старте робота на нем запускаются всего 2 launch-файла:

roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch

Помним, что launch-файл в ROS — это своеобразная сборная солянка для нод, в один launch файл можно поместить одну или несколько из них. Посмотрим на первый launch из списка.

bringup.launch

Как видно, запускаются 6 нод, часть из них вынесены в отдельные launch-файлы:

• rosserial_lino — нода, ответственная за общение с teenzy;
• imu — гироскоп;
• lino_base_node — база робота;
• base_footprint_to_base_link — «привязка» робота к пространству (0.065м — расстояние от пола до базы робота);
• ekf_localization — нода, транслирующая «очищенную» одометрию;
• laser — лидар.

Все ноды, которые участвуют в данном launch файле важны. Но они больше относятся к одометрии робота, чем к его навигации.

Заглянем во второй launch файл, участвующий при старте робота — navigate.launch.

navigate.launch

Видно, что из navigate.launch запускаются еще 3 launch-файла:

• map_server
• amcl
• move_base.

Именно они и отвечают за всю навигацию робота. Рассмотрим каждый из них подробнее.

Map_server

В задачу map_servera входит предоставление статической карты пространства по запросу. Ранее сохраненная карта, построенная через slam, которая в данном случае находится по пути $(find linorobot)/maps/my-map-4.yaml предоставляется сервису, ее запросившему.

*Чтобы быть точным — по пути находятся параметры, определяющие карту, а сама карта имеет имя my-map-4.pgm.

Если карты нет или ее сложно построить, то можно «скормить» пустую карту — белый лист, границы препятствий нанести затем вручную в обычном paint либо в rviz.

Amcl

Это одна из главных нод в этом оркестре. Заглянем в launch, который ее запускает:


Параметров тут как на приборной панели самолета. Но, как правило, что-то править здесь нет необходимости.

Разве что, задать стартовую позицию робота (как мы делали в предыдущем посте):

        <param name="initial_pose_x" value="0.548767569629"/>
        <param name="initial_pose_y" value="0.218281839179"/>
        <param name="initial_pose_z" value="0.0"/>
        <param name="initial_orientation_z" value="0.128591756735"/>
        <param name="initial_orientation_w" value="0.991697615254"/>

максимальную границу лидара:

<param name="laser_max_range" value="12.0"/>

Количество зеленых стрелочек particle cloud swarm (чем больше — тем тяжелее raspberry, меньше — тем хуже навигация робота):

 <param name="max_particles" value="1000"/>
 <param name="min_particles" value="500"/>

Диффиренциальный ли робот или с omni-колесами:

<param name="odom_model_type" value="diff"/>

Определить фреймы одометрии и базы робота:

<param name="odom_frame_id" value="odom"/>
<param name="base_frame_id" value="base_footprint"/> 

Все остальное можно не трогать, но справочно можно посмотреть:

Move_base

Вторая по важности составляющая.

Главная функция move_base — переместить робота из текущей позиции в целевую позицию.
Каждый раз, когда в rviz мы используем «2D Nav Goal», в топик move_base/goal попадает сообщение, которое используется для перемещения робота.

Мove_base по сути простой action-server, состоящий из 5 топиков (помним, что в ROS кроме topicов, сервисов, есть еще action):

•	move_base/goal (move_base_msgs/MoveBaseActionGoal)
•	move_base/cancel (actionlib_msgs/GoalID)
•	move_base/feedback (move_base_msgs/MoveBaseActionFeedback)
•	move_base/status (actionlib_msgs/GoalStatusArray)
•	move_base/result (move_base_msgs/MoveBaseActionResult)

Зная это, можно напрямую отправлять ему сообщения (в топик move_base/goal) и перемещать робота, минуя rviz.

*Частично этого мы уже касались в предыдущей статье.

Формат сообщения будет примерно следующий:

rostopic pub /move_base/goal geometry_msgs/PoseStamped '{ header: { frame_id: "map" }, pose: { position: { x:  2.49339078005, y: 0.0666679775475, z: 0 }, orientation: { x: 0, y: 0, z: -0.999261709946, w: 0.0384192013861 } } }'

*Здесь мы отправляем робота в точку с кодовым названием «коридор».

Мove_base, хотя и состоит из одной ноды, в свою очередь, представляет из себя «матрешку» и содержит нескольких файлов с параметрами:

• costmap_common_params.yaml
• local_costmap_params.yaml
• global_costmap_params.yaml
• base_local_planner_default_params.yaml
• move_base_params.yaml

Рассматривая код move_base можно понять, как он связан с этими параметрами.

В текущем проекте 2-х колесного робота move_base запускается как


После того, как обозначена цель поездки в rviz (или через скрипт) и move_base получило соответствующее сообщение, оно отсылает его в глобальный планировщик(global planner)(далее ГП). ГП в свою очередь вычисляет безопасный путь поездки до цели. Этот путь расчитывается ДО того как робот поедет, и этот план не будет учитывать те данные, которые будут поступать от сенсоров робота во время движения.

Каждый раз как ГП составляет план движения, этот план публикуется в топик /move_base/DWAPlannerROS/global_plan (посмотрим, что туда попадает, используя аргумент echo):

Как рассчитывается глобальный план ?

ГП отвечает за всю «магию». Он использует Dijkstra алгоритм (как правило) для вычисления кратчашего пути между стартовой позицией (initial pose) и конечной точкой (goal pose).

ГП существует несколько типов:

• Navfn
• Carrot Planner
• Global Planner

в данном проекте используется последний. Убедиться в этом можно заглянув в файл проекта:

nano linorobot_ws/src/linorobot/param/navigation/move_base_params.yaml

Поменяем ГП, например на navfn/NavfnROS, сохраним и проверим, что ГП теперь другой:



У ГП есть параметры, определяющие его поведение, они содержатся в вышеуказанном yaml файле настроек.

Costmaps

ГП при построении плана поездки использует статическую карту местности, которую ему отдает сервис map. Однако кроме этой карты в бой вступают еще две дополнительные, так называемые «карты затрат» (costmaps).

Это 2 карты:

• global costmap, создаваемая из статической карты map;
• local costmap — формируется из данных, полученных с сенсоров робота по мере их поступления.

Назначение этих карт — показать роботу, где на карте есть препятствия, а где их нет. Без этих карт, робот будет ездить по карте, не видя препятствий.

Упрощенно, global costmap используется для ГП (глобального планировщика,global planner), local costmap — для планировщика локального (local planner).

Global Costmap

Global costmap, как упомянуто ранее, создается из статической карты, предоставляемой по сути пользователем (map.pgm) и определяет размеры и иную информацию о препятствиях.

Параметры определены в файле проекта:

/param/navigation/global_costmap_params.yaml


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

И здесь же нам необходимо уменьшить inflation_radius хотя бы до 0.2м. Так как при текущих показателях и узких проходах в помещении хрущевки, планировщик просто не сможет построить план:



*На рисунке видно, что «тромбы» инфляции закупорили свободное пространство в дверном проеме.

После уменьшения «инфляции»:



Почему бы просто не обнулить инфляцию? Резонный вопрос. Но в этом случае, ГП будет строить маршруты слишком плотно прилегающие с препятствиям и дифференциальному роботу будет не просто их объехать.

Local Planner

Локальный планировщик (далее «ЛП») работает в паре с ГП (глобальным планировщиком) и имеет как схожие с ним черты, так и те, что его отличают.

Он так же как и ГП отвечает за построение маршрута, но на его плечи ложатся другие задачи: следовать проложенному ГП маршруту и избегать препятствия по пути (avoids obstacles).

Как только ГП рассчитал план пути, этот план отравляется в ЛП. ЛП, в свою очередь, выполняет каждый сегмент этого плана. Таким образом, имея маршрут и карту, ЛП отправляет «команды движения» и двигает робота.

В отличие от ГП, ЛП следит за одометрией, данными с лидара и выбирает свободный от препятствий маршрут.

Как только локальный план рассчитан, он публикуется в топик /local_plan, кроме того, ЛП публикует часть глобального плана, по которому он следует в топик /global_plan.

Локальных планировщиков так же как и глобальных существует несколько видов:

• base_local_planner
• dwa_local_planner
• eband_local_planner
• teb_local_planner

В данном проекте используется dwa_local_planner.

Параметры ЛП определены в файле проекта:

/param/navigation/base_local_planner_default_params.yaml



Параметров много, разберем.

Local Costmap

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

В отличие от Global costmap локальная карта затрат строится исходя из данных сенсоров робота.
Учитывая ширину и высоту карты затрат (которые определяются пользователем), ЛП удерживает робота в центре данной карты затрат при перемещении.

Сравнивая ГП и ЛП можно провести между ними линию следующим образом: ГП не видит временного препятствия, которого изначально нет на карте, в то время как ЛП его определяет.

Local Costmap Parameters(Параметры локальной карты затрат)

Параметров, определяющих данную карту не так много (linorobot/param/navigation/local_costmap_params.yaml):


Здесь нам необходимо уменьшить height до 1 метра, т.к. карта у нас большей частью 2d и максимальная высота — это лидар, который находится над уровнем пола не более 1 м. Поэтому нет смысла производить расчеты выше одного метра.

Поправим также resolution, уменьшив ее.

И снизим также инфляцию — inflation_radius:0.3 метра. Полностью ее лучше не убирать, как и в случае с Global Costmap, т.к. робот будет цепляться за препятствия, пытаясь их объехать.

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

Local costmap самообновляется. Это происходит с частотой, указанной в параметрах (update_frequency).

Каждый цикл обновления включает в себя следующие шаги:

• поступили данные с сенсоров робота;
• проводятся операции маркировки и очистки (каждый из сенсоров сообщил есть или нет препятствия);
• каждой ячейке на карте присвоены определенные значения (занята/свободна);
• препятствия обозначены с помощью «инфляции».

Итак, помимо статической карты существуют карты затрат — global costmap и local costmap, с которыми надо считаться.

Но есть еще параметры, которые распространяются на обе карты затрат.

Common costmap parameters (Общие параметры затрат)

Находятся по пути linorobot/param/navigation/costmap_common_params.yaml


В этом файле описывается как сенсоры робота видят препятствия. Например, obstacle_range: 2.5, означает, что при обнаружении на расстоянии 2.5 м, препятствие, если оно обнаружено должно быть нанесено на карту затрат. footprint — размеры робота в пространстве.
Observation_sources: scan — источник, с которого обрабатывается информация (у нас лидар публикует в топик /scan), он же выполняет маркировку и очистку препятствий на карте затрат.

Как в целом выглядит процесс навигации в ROS и в linorobot в частности?

После того, как определена текущая позиция робота (робот локализован) мы можем отправить команду роботу для передвижения в целевую точку (goal position) через редактор rviz или скрипт. Данная команда попадет в move_base node. Далее move_base node отправляет goal position в глобальный планировщик (global planner), который рассчитывает путь (path planning) от текущий позиции до целевой. Этот план учитывает глобальную карту затрат (global costmap) c «нанесенными препятствиями», получаемую от map_server.

Далее глобальный планировщик отправляет рассчитанный путь в локальный планировщик (local planner), который, в свою очередь, исполняет каждый сегмент глобального плана.

Локальный планировщик также получает одометрию и данные с лидара для составления плана (collision-free local plan) для беспрепятственного перемещения робота.

Локальный планировщик «привязан» к локальной карте затрат (local costmap) для мониторинга препятствий по пути следования.

Локальный планировщик генерирует команды движения (velocity commands) и отправляет их в базовый контролер движения (base controller). Базовый контролер конвертирует эти команды в непосредственные команды управления роботом.

Здесь будет кстати классическая картинка навигационного стека ROS:

Бонус. Как подключить робота к majordomo.

Роботом можно управлять не только из редактора rviz либо с помощью скриптов напрямую.
Есть возможность посылать команды управления через web-browser.

Для этих целей будет весьма кстати open-source продукт — webviz.

Попробуем скрестить его с другим известным проектом — majordomo, чтобы поднять автоматизацию своего дома на новый уровень.

Majordomo — самодостаточный открытый проект автоматизации управления домом, не раз упоминался на ресурсе. И так как автор несколько лет уже наблюдает и использует проект для личных некорыстных целей, предлагается остановить выбор на нем.
Поговорим как вывод с топиков ROS подключить на статическую страницу в majordomo.

Шаг 1.

Устанавливаем на робота пакет rosbridge:

sudo apt-get install ros-kinetic-rosbridge-suite

*скорее всего установка rosbridge провалится, тогда пробуем такой вариант:

cd ~/linorobot_ws/src
git clone https://github.com/RobotWebTools/rosbridge_suite.git
git clone https://github.com/GT-RAIL/rosauth.git
cd ..
catkin_make
sudo pip install tornado
sudo pip install pymongo
sudo apt-get install python-twisted

Запускаем (после запуска основных launch файлов):

roslaunch rosbridge_server rosbridge_websocket.launch

Проверим, что bridge работает на 9090 порту:

netstat -a | grep 9090

Шаг 2.

Пробуем подключиться с любого ПК, находящегося в локальной сети с роботом, зайдя через Chrome-браузер:

https://webviz.io/app/?rosbridge-websocket-url=ws://192.168.1.110:9090

Где ip — ip-робота.

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

Если все пошло удачно, то будет примерно следующая картинка с роботом на странице:



Осталось вставить строку в majordomo.

Шаг 3.

Настраиваем страницу в majordomo.

Зайдем на панели администратора в домашние страницы, выберем или создадим страницу, на которую будем выводит информацию:



Внесем целевую ссылку в настройки и проверим, что она работает, перейдя по ней:



Браузер может выдать такую ошибку, с которой сталкивались ранее:



Для ее устранения необходимо разрешить небезопасные скрипты в настройках Chrome:
chrome://settings/content/siteDetails?site=https%3A%2F%2Fwebviz.io

Небезопасный контент: разрешить
Как сделать все безопасным пока остается за кадром.