Робот-тележка 2.0. Часть 3. Внутри навигационного стека, немного majordomo
Эта часть цикла статей по навигации домашнего автономного робота на базе 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, который ее запускает:
Параметров тут как на приборной панели самолета. Но, как правило, что-то править здесь нет необходимости.
Разве что, задать стартовую позицию робота (как мы делали в предыдущем посте):
максимальную границу лидара:
количество зеленых стрелочек particle cloud swarm (чем больше — тем тяжелее raspberry, меньше — тем хуже навигация робота):
диффиренциальный ли робот или с omni-колесами:
определить фреймы одометрии и базы робота:
Все остальное можно не трогать, но справочно можно посмотреть:
-laser_min_range (default: -1.0): минимум расстояния, устанавливаемый для лидара; -1.0 означает, что параметр не активен и используется min лидара согласно его характеристикам.
-laser_max_range (default: -1.0): то же самое, только для max лидара.
-laser_max_beams: - сколько равномерно расположенных лучей в каждом сканировании будет использоваться при обновлении фильтра.
-laser_z_hit : масса весов для z_hit части модели.
-laser_z_short: то же для z_short.
-laser_z_max: то же z_max.
-laser_z_rand: то же для z_rand.
-update_min_d-задает линейное расстояние (в метрах), которое робот должен пройти для выполнения обновления фильтра.
-update_min_a: задает угловое расстояние (в радианах), которое робот должен переместить для выполнения обновления фильтра.
-resample_interval : задает количество обновлений фильтра, необходимых перед повторной выборкой.
-transform_tolerance: время (в секундах), с помощью которого можно перенести дату опубликованного преобразования, чтобы указать, что это преобразование действительно в будущем.
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.yamlbase_global_planner: global_planner/GlobalPlanner
base_local_planner: dwa_local_planner/DWAPlannerROS
shutdown_costmaps: false
controller_frequency: 5.0
controller_patience: 3.0
planner_frequency: 0.5
planner_patience: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2
conservative_reset_dist: 0.1 # distance from an obstacle at which it will unstuck itself
cost_factor: 1.0
neutral_cost: 55
lethal_cost: 253
Поменяем ГП, например на 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
global_costmap:
global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 1.0 #before: 5.0
publish_frequency: 0.5 #before 0.5
static_map: true
transform_tolerance: 0.5
cost_scaling_factor: 10.0
inflation_radius: 0.55
Здесь видны, какие фреймы используются, с какой частотой обновляется карта, определено что она является статической, точность с которой планировщик будет создавать план, радиус препятствий.
И здесь же нам необходимо уменьшить 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
DWAPlannerROS:
max_trans_vel: 0.50
min_trans_vel: 0.01
max_vel_x: 0.50
min_vel_x: -0.025
max_vel_y: 0.0
min_vel_y: 0.0
max_rot_vel: 0.30
min_rot_vel: -0.30
acc_lim_x: 1.25
acc_lim_y: 0.0
acc_lim_theta: 5
acc_lim_trans: 1.25
prune_plan: false
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.1
trans_stopped_vel: 0.1
rot_stopped_vel: 0.1
sim_time: 3.0
sim_granularity: 0.1
angular_sim_granularity: 0.1
vx_samples: 20
vy_samples: 0
vth_samples: 40
path_distance_bias: 34.0
goal_distance_bias: 24.0
occdist_scale: 0.05
forward_point_distance: 0.3
stop_time_buffer: 0.5
scaling_speed: 0.25
max_scaling_factor: 0.2
twirling_scale: 0.0
oscillation_reset_dist: 0.05
oscillation_reset_angle: 0.2
use_dwa: true
restore_defaults: false
Параметров много, разберем.
max_trans_vel: 0.50 - max скорость робота в м/c.
min_trans_vel: 0.01 - min скорость робота в м/c.
max_vel_x: 0.50 - max скорость робота в м/c.
min_vel_x: -0.025 - min скорость робота в м/c.
max_vel_y: 0.0 - движение вдоль оси y (holonomic)
min_vel_y: 0.0 - движение вдоль оси y (holonomic)
max_rot_vel: 0.30 - max скорость при поворотах.
min_rot_vel: -0.30 - min скорость при поворотах.
acc_lim_x: 1.25 - лимит ускорения по оси x
acc_lim_y: 0.0 - лимит ускорения по оси y
acc_lim_theta: 5 - лимит ускорения поворотов в радиан/сек
acc_lim_trans: 1.25 - лимит ускорения передаваемый
xy_goal_tolerance: 0.25 - (в метрах) как близко приблизиться к цели по осям x и y, после получения команды поездки
yaw_goal_tolerance: 0.1 - (в радианах) то же, но для угловых показателей (yaw/rotation).sim_time: 3.0 - количество времени для forward-моделирования траектории в секундах
sim_granularity: 0.1 - размер шага, занимаемого между точками на заданной траектории (шаг продвижения)
vx_samples: 20 - количество выборок, используемых при исследовании пространства скоростей x
vy_samples: 0 - то же, но по y
vth_samples: 40 - то же, но по theta.
path_distance_bias: 34.0 - веса, на сколько контроллер должен оставаться близко к заданному пути
goal_distance_bias: 24.0 - веса для того, насколько контроллер должен попытаться достичь своей локальной цели; также контролирует скорость
occdist_scale: 0.05 - веса, на сколько контроллер должен попытаться избежать препятствий
forward_point_distance: 0.3 - как далеко разместить дополнительную рассчетную точку
stop_time_buffer: 0.5 - количество времени, в течение которого робот должен остановиться перед столкновением для получения правильной траектории.
scaling_speed: 0.25 - абсолютная скорость, с которой начинается масштабирование следа робота
max_scaling_factor: 0.2 - насколько масштабировать след робота во время движения.twirling_scale: 0.0 - степень вращения
oscillation_reset_dist: 0.05 - как далеко пройти до сброса флагов колебаний
oscillation_reset_angle: 0.2 - "как далеко повернуть" до сброса флагов колебаний
use_dwa: true - использовать ли алгоритмы DWA
restore_defaults: false - восстанавливать ли параметры настройки по умолчанию.publish_traj_pc : true
publish_cost_grid_pc: true
Local Costmap
Локальная «карта затрат», используется ЛП (локальным пларировщиком) для рассчета локального пути.
В отличие от Global costmap локальная карта затрат строится исходя из данных сенсоров робота.
Учитывая ширину и высоту карты затрат (которые определяются пользователем), ЛП удерживает робота в центре данной карты затрат при перемещении.
Сравнивая ГП и ЛП можно провести между ними линию следующим образом: ГП не видит временного препятствия, которого изначально нет на карте, в то время как ЛП его определяет.
Local Costmap Parameters (Параметры локальной карты затрат).
Параметров, определяющих данную карту не так много (linorobot/param/navigation/local_costmap_params.yaml):
global_frame: /odom # Глобальный фрейм, в котором оперирует ЛП, должно быть odom.
robot_base_frame: /base_footprint #Фрейм робота.
update_frequency: 1.0 #before 5.0 # частота обновления карты в Гц
publish_frequency: 2.0 #before 2.0 # частота публикации карты в Гц
static_map: false # использовать или нет статическую карту. Если поставить true, то ЛП
превратится в ГП.
rolling_window: true #использовать ли скользящее окно для целей ЛП.
width: 2.5 # ширина скользящего окна
height: 2.5 # высота скользящего окна
resolution: 0.05 #increase to for higher res 0.025 #разрешение в скользящем окне
transform_tolerance: 0.5
cost_scaling_factor: 5
inflation_radius: 0.55# радиус инфляции препятствий
Здесь нам необходимо уменьшить 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
raytrace_range: 3.0
footprint: [[-0.24, -0.22], [-0.24, 0.22], [0.24, 0.22], [0.24, -0.22]]
inflation_radius: 0.55
transform_tolerance: 0.5
observation_sources: scan
scan:
data_type: LaserScan
topic: scan
marking: true
clearing: true
map_type: costmap
В этом файле описывается как сенсоры робота видят препятствия. Например, 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
Небезопасный контент: разрешить
Как сделать все безопасным пока остается за кадром.
