Робот-тележка на ROS. Часть 7. Локализация робота: gmapping, AMCL, реперные точки на карте помещения

Посты серии:
8. Управляем с телефона-ROS Control, GPS-нода
7. Локализация робота: gmapping, AMCL, реперные точки на карте помещения
6. Одометрия с энкодеров колес, карта помещения, лидар
5. Работаем в rviz и gazebo: xacro, новые сенсоры.
4. Создаем симуляцию робота, используя редакторы rviz и gazebo.
3. Ускоряемся, меняем камеру, исправляем походку
2. Софт
1. Железо

В прошлый раз, после установки бюджетного лидара RPlidar-A1 удалось построить карту помещения, поработать с одометрией. Однако робот, несмотря на наличие карты и настройку одометрии с оптических датчиков все еще чувствует себя неуверенно в окружающей обстановке.
Вернее, он ее вообще не видит. И ездит по готовой карте вдоль и поперек, препятствия не для него. Это и радует, и огорчает одновременно. С одной стороны не стоит беспокоится о препятствиях и ездить, куда душа пожелает, с другой стороны — поехать в другую комнату или на кухню, вряд ли получится. Поэтому поговорим о локализации робота в пространстве, используя алгоритмы, которые предоставляет ROS, а также наш джентельменский набор из лидара и энкодеров. Но перед тем как перейдем непосредственно к локализации, поговорим еще об одном пакете ROS, который также позволяет строить 2D карты помещения и порой это у него получается получше, чем у ROS пакета из предыдущего поста. Познакомимся с gmapping.

Gmapping из клана ROS

Не будем оригиналами и воспользуемся наработками из уже существующей статьи Хабра по теме, но расширим, обновим и углубим, содержащиеся в ней сведения. Статья называется Построение карты и локализация мобильного робота в ROS без одометрии с использованием laser_scan_matcher.

Часть манипуляций (высоконагруженные приложения — rviz) будут производиться на Компьютере (вне робота), остальное (драйвер движения, нода запуска лидара) — на Роботе.

Установим по сценарию вышеуказанной статьи сначала laser_scan_matcher для ROS-kinetic на Компьютере (в статье было indigo):

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

Теперь запустим.

Ноду с лидаром на Роботе:

roslaunch rplidar_ros rplidar.launch

На Компьютере:

roslaunch laser_scan_matcher demo.launch

*roscore запускать нет необходимости, так как master-нода стартует каждый раз при загрузке Робота.

В запустившемся rviz будут видны контуры комнаты и мусора в ней:



laser_scan_matcher понадобится нам для работы с gmapping пакетом ROS. Сам gmapping устанавливать нет необходимости, он уже есть на виртуальной машине в составе ROS Kinetic. Проверим наличие пакета в системе:



Теперь создадим на Компьютере (не на Роботе), как и в вышеупомянутой статье, launch файл, использующий gmapping:

roscd roscd rosbots_description/launch
nano my_gmapping_launch.launch
 Поместим туда тот же 

код

<?xml version="1.0"?>
<launch>
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser"
           args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />

  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">
   <param name="fixed_frame" value = "odom"/>
   <param name="use_odom" value="true"/>
   <param name="publish_odom" value = "true"/>
   <param name="use_alpha_beta" value="true"/>
   <param name="max_iterations" value="10"/>
  </node>

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
   <param name="map_udpate_interval" value="1.0"/>
   <param name="delta" value="0.02"/>
  </node>
</launch>

Как видно из кода, автор запускает 3-и ноды: tf, laser_scan_matcher и gmapping.

Cнова запустим лидар на Роботе:

roslaunch rplidar_ros rplidar.launch

На Компьютере вновь испеченный launch файл и редактор rviz:

roslaunch rosbots_description my_gmapping_launch.launch

rosrun rviz rviz

В rviz получим картинку, схожую с той, что была получена при построении карты в нашем предыдущем посте о Роботе-тележке. Только в этот раз работает пакет gmapping.

И надо признать, работает он не дурно. Если hector_slam оставлял многочисленные артефакты на карте при повороте лидара вокруг своей оси, то в этот раз артефактов почти нет:



После поездок по помещению, построенная карта сохраняется также:
rosrun map_server map_saver -f map-1, где map-1 имя карты для сохранения.

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

Алгоритм, который применяется для определения местоположения робота на карте, называется AMCL. AMCL использует многочастичный фильтр для отслеживания положения робота на карте. В нашем роботе для реализации AMCL мы используем пакет ROS (http://wiki.ros.org/amcl).

Запустим AMCL для нашего робота.

Для этого на Компьютере в папке проекта создадим еще один launch файл.

Назовем его

amcl-2.launch

<launch>
  <param name="/use_sim_time" value="false"/>

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />

 <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
    name="laser_scan_matcher_node" output="screen">
    <param name="fixed_frame" value = "odom"/>
    <param name="use_alpha_beta" value="true"/>
    <param name="max_iterations" value="10"/>
  </node>

 <node name="map_server" pkg="map_server" type="map_server" args="/home/pi/catkin_ws/src/rosbots_description/maps/map-3.yaml"/>

  <node pkg="amcl" type="amcl" name="amcl" output="screen" >
  <!-- Publish scans from best pose at a max of 10 Hz -->
 <param name="odom_model_type" value="diff"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.2"/>
  <param name="odom_alpha2" value="0.2"/>

  <!-- laser, translation std dev, m -->
  <param name="laser_min_range" value="-1"/>
  <param name="laser_max_range" value="-1"/>
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <param name="laser_z_hit" value="0.5"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>

  <!-- <param name="laser_model_type" value="beam"/> -->
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>
  <param name="odom_frame_id" value="odom"/>

  <param name="base_frame_id" type="str" value="base_link" />
  <param name="global_frame_id" type="str" value="map" />

  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>
  <param name="use_map_topic" value="true" />
  <param name="first_map_only" value="true" />
</node>
</launch>

Код полностью идентичен уже упомянутой статье, за исключением:
— исключена нода, запускающая лидар hokuyo (он запускается на Роботе)
— путь и название карты помещения иное (map-3.yaml)
В коде запускаются 4-е ноды:
— tf
— map_server
— laser_scan_matcher
— amcl
Нода amcl использует карту, которую публикует map_server для последующей локализации робота.

Запустим launch файлы и посмотрим на результат.

Но Роботе:

roslaunch rplidar_ros rplidar.launch

На Компьютере:
1-й терминал: roslaunch rosbots_description amcl-2.launch
2-й терминал: roslaunch rosbots_description rviz.launch

После того как запустится rviz, следующие шаги в этом редакторе будут следующие:
— добавить displays в rviz:
• LaserScan
• Map
• PoseArray


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



Данную операцию необходимо проделать, выбрав Frame «map» в rviz:



В терминале получим координаты (pose) робота:

[ INFO] [1572374324.454855505]: Setting pose (1572374324.454806): -0.014 -0.012 0.655

Выставлять позиция робота на карте с помощью зеленой стрелки на карте можно многократно.
При этом желательно, чтобы данные с лидара (красная окантовка) на карте сопадала или была близка к реальному расположению стен помещения:



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



*в качестве робота у нас оси для наглядности (вся расписная rviz модель пока скрыта).
**Если стрелки не появились, то можно попробовать снять и вновь поставить галочку в добавленном в rviz дисплее PoseArray.

Несмотря на то, что мы прямо указали на карте, где расположен робот, система все равно предполагает, что он может находиться в тех местах, где нарисованы красные стрелки. Это вероятное расположение робота на карте. Большое количество стрелок и их разброс на карте говорит о том, что система все еще не точно знает местонахождение робота. Однако то место, где стрелок погуще, робот находится с большей степенью вероятности.

Чтобы система более точно поняла, где именно робот, надо поездить по карте с запущенными нодами, которые определяют расположение робота. У нас из этого набора: лидар и энкодеры.
Но мы поездим по карте только с запущенным лидаром, заодно выяснив можно ли достоверно локализовать робота только с его (лидара) помощью.
—

поездить на роботе по карте

На роботе:

rosrun rosbots_driver part2_cmr.py

На Компьютере:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel

Во время поездки amcl начнет читать топики /scan, /map,/tf и публиковать расположение робота в топики /amcl_pose и /particlecloud.

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

На картинке видно как путешествует модель (в виде дерева связей). И также видно, что лидар не достаточно точно в одиночку справляется с локализацией при неоднозначных границах помещения.

Что означают другие параметры в коде ноды amcl?

Условно они делятся на основные (General), параметры фильтров (Filter), Параметры лазера (лидара) (Laser Parameters).

Основные параметры:

• odom_model_type (default: «diff»): определяет какую модель одометрии использовать. У нас diff, что означает дифференциальная. Может быть изменен на «omni,» «diff-corrected,» или «omni-corrected.»
• odom_frame_id (default: «odom»): определяет frame (читай топик), с которым будет аасоциироваться одометрия. Она обычно публикуется в топик odom.
• base_frame_id (default: «base_link»): frame для базы робота.
• global_frame_id (default: «map»): frame карты, как правило map-server ее публикует в топик map
• use_map_topic (default: false): определяет будет ли загружена карта через топик или путем вызова сервиса (мы помним, что кроме топиков в ROS есть еще сервисы(services) и действия (actions).

Параметры фильтров (Filter Parameters).

Эти параметры позволят вам настроить способ работы фильтра частиц.

• min_particles (default: 100): Устанавливает минимально допустимое количество частиц для фильтра. У нас 500.
• max_particles (default: 5000): Устанавливает максимально допустимое количество частиц для фильтра.
• kld_err (default: 0.01): Устанавливает максимально допустимую ошибку между истинным распределением и расчетным распределением. У нас 0.05
• update_min_d (default: 0.2): Устанавливает линейное расстояние (в метрах), которое должен пройти робот, чтобы выполнить обновление фильтра.
• update_min_a (default: пи/6.0): Устанавливает угловое расстояние (в радианах), которое робот должен переместить, чтобы выполнить обновление фильтра. У нас 0.5
• resample_interval (default: 2): Устанавливает количество обновлений фильтра, необходимых перед повторной выборкой. У нас 1.
• transform_tolerance (default: 0.1): Время (в секундах), в течение которого следует датировать опубликованное преобразование, чтобы указать, что это преобразование действительно в будущем.
• gui_publish_rate (default: -1.0): Максимальная скорость (в Гц), с которой сканы и пути публикуются для визуализации. Если это значение -1.0, эта функция отключена. У нас 10.

Параметры лазера (лидара) (Laser Parameters)

Эти параметры позволяют настроить способ взаимодействия amcl с лазером лидара.

• laser_min_range (default: -1.0): Минимальный диапазон сканирования, который следует учитывать; -1.0 приведет к использованию минимального диапазона, указанного в отчете лазера.
• laser_max_range (default: -1.0): Максимальный диапазон сканирования, который следует учитывать; -1.0 приведет к использованию максимальной дальности лазера.
• laser_max_beams (default: 30): Сколько равномерно распределенных лучей в каждом сканировании будет использоваться при обновлении фильтра.
• laser_z_hit (default: 0.95): Веса z_hit составляющей модели робота.
• laser_z_short (default: 0.1): Веса z_short составляющей модели робота.
• laser_z_max (default: 0.05): Веса z_max составляющей модели робота.
• laser_z_rand (default: 0.05): Веса z_rand составляющей модели робота.

Посмотрим на что влияют параметры min_particles и max_particles. Если уменьшить их значения, то при запуске launch файла количество частиц в визуальном редакторе будет наглядно меньше.

Все параметры несут смысловую нагрузку, но разобрать эффект изменения каждого из них в рамках статьи сложно.

Реперные точки на карте помещения

Название уловное и подразумевает под собой позицию робота на карте в данный конкретный момент.

Для чего они нужны? Для того, чтобы понять, что робот приехал из точки А в комнате в точку В на кухне.

Данные о позиции (pose) робота можно получить при работающей ноде amcl (то, что запускалось в статье выше).

И посмотреть в топике /amcl_pose:

rostopic echo -n1 /amcl_pose

*ключ n1 — для «фиксации» потока сообщений в топике.



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

1.Создадим новый ros пакет.

cd  catkin_ws/src
catkin_create_pkg get_pose rospy
cd get_pose/src

2. В папке создадим файл:

get_pose_service.py

#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty, EmptyResponse # Import the service message python classes generated from Empty.srv.
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose

robot_pose = Pose()

def service_callback(request):
    print "Robot Pose:"
    print robot_pose
    return EmptyResponse() # the service Response class, in this case EmptyResponse
    
def sub_callback(msg):
    global robot_pose
    robot_pose = msg.pose.pose
    

rospy.init_node('service_server') 
my_service = rospy.Service('/get_pose_service', Empty , service_callback) # create the Service called get_pose_service with the defined callback
sub_pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, sub_callback)
rospy.spin() # mantain the service open.

*Не забудем сделать его исполнимым chmod +x get_pose_service.py
3.Создадим launch для файла с кодом ноды:

cd ..
mkdir launch && cd launch

nano

get_pose_service.launch

<launch>
    <node pkg="get_pose" type="get_pose_service.py" name="service_server" output="screen">       
    </node>
</launch>

4.Не забудем пересобрать catkin:

cd catkin_ws
catkin_make

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

Но Роботе:

roslaunch rplidar_ros rplidar.launch

На Компьютере:

1-й терминал: roslaunch rosbots_description amcl-2.launch
2-й терминал: roslaunch rosbots_description rviz.launch
3-й терминал: roslaunch get_pose get_pose_service.launch

Обратимся к новому сервису, который должен отдавать нам текущую позицию робота на карте (вызовем сервис ROS):

rosservice call /get_pose_service

В терминале с запущенным launch get_pose, получим координаты робота на карте:



Возможные ошибки.

[rviz.launch] is neither a launch file in package [rosbots_description] nor is [rosbots_description] a launch file name
решение:

cd  catkin_ws
source devel/setup.bash