Робот-тележка 2.0. Часть 3. Внутри навигационного стека, немного majordomo

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

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

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

roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch


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

bringup.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

    
    
    

    
    

    
    
    



Видно, что из 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, который ее запускает:

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 запускается как

move_base_2wd.launch

    
        
        
        
        
        
        
    


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

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


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

ГП существует несколько типов:
-Navfn
-Carrot Planner
-Global Planner
в данном проекте используется последний. Убедиться в этом можно заглянув в файл проекта:

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


move_base_params.yaml
base_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, сохраним и проверим, что ГП теперь другой:
eujvmexuz_llryk8_7t6gl8tt6a.png
У ГП есть параметры, определяющие его поведение, они содержатся в вышеуказанном 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_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 м. Так как при текущих показателях и узких проходах в помещении хрущевки, планировщик просто не сможет построить план:
8oxfttulliklktovmxqz7pwsene.png
*На рисунке видно, что «тромбы» инфляции закупорили свободное пространство в дверном проеме.
После уменьшения «инфляции»:
vrgndcmkkc3niynr9kd7z4ajato.gif
Почему бы просто не обнулить инфляцию? Резонный вопрос. Но в этом случае, ГП будет строить маршруты слишком плотно прилегающие с препятствиям и дифференциальному роботу будет не просто их объехать.

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

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

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

Параметры конфигурации робота (Robot Configuration Parameters).
  
 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 - лимит ускорения передаваемый

Параметры цели (Goal Tolerance Parameters)
xy_goal_tolerance: 0.25 - (в метрах) как близко приблизиться к цели по осям x и y, после получения команды поездки 
yaw_goal_tolerance: 0.1 - (в радианах) то же, но для угловых показателей (yaw/rotation).

Параметры Прямого Моделирования (Forward Simulation Parameters)
sim_time: 3.0 - количество времени для forward-моделирования траектории в секундах
sim_granularity: 0.1 - размер шага, занимаемого между точками на заданной траектории (шаг продвижения)
vx_samples: 20 - количество выборок, используемых при исследовании пространства скоростей x
vy_samples: 0 - то же, но по y
vth_samples: 40  - то же, но по theta.

Параметры Оценки Траектории (Trajectory Scoring Parameters)
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 - насколько масштабировать след робота во время движения.

Oscillation Prevention Parameters (Параметры предотвращения колебаний)
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):

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

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:
kufaxyl7kqfn_vbqcuhe9nxumfa.png

Бонус.
Как подключить робота к 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 выдаст сообщение, что используются небезопасные скрипты. Чтобы идти дальше надо разрешить их исполнение на странице, кликнув адресной строке на соответствующий значок — uu3sznroksu9tlxosnpz5tkgr1c.png

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

Шаг 3.
Настраиваем страницу в majordomo.
Зайдем на панели администратора в домашние страницы, выберем или создадим страницу, на которую будем выводит информацию:
19xlxxslazhg2t7xv-4t94ecxuq.png
Внесем целевую ссылку в настройки и проверим, что она работает, перейдя по ней:
xtyp3qpt1vh9mf4mw812vj18abc.png
Браузер может выдать такую ошибку, с которой сталкивались ранее:
q45bclqblzisgczloljefa2hucq.png

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

© Habrahabr.ru