Как сделать робота на ROS своими руками. Часть 2: дистанционное управление и навигация

Привет!

В прошлой части проекта мы создали шасси мобильного робота на ROS и запрограммировали управление движением.

Пора прикрутить ему дистанционное управление и автономную навигацию. Поехали!

Содержание

• Введение
• Цель робота
• Привод робота
  • Типы приводов
• Выбор шасси
  • Шасси Turtle
  • Энкодеры
• ROS
• Бортовой компьютер
  • Выбор железа
  • Выбор и установка дистрибутива Linux
  • Первый запуск
  • Настройка Wi-Fi и графической оболочки
• Установка и настройка ROS
  • Установка ROS на Raspberry Pi
  • Установка ROS на настольный компьютер
  • Создание рабочего пространства ROS
  • Процесс разработки
  • Настойка локальной сети и сети ROS
• Описание робота
  • Формат URDF
  • 3D-модель робота в САПР
  • Экспорт в URDF
• Пакет robot_description
  • Приводим в порядок URDF-файл описания робота
  • Визуализация URDF-модели
  • Footprint робота
• Raspberry Pi HAT и крепление электроники
• Бортовое питание
• Актуализация модели робота
• Низкоуровневые драйверы
  • Пакет abot_driver
  • Библиотека WiringPi
  • Драйвер энкодеров
  • Драйвер моторов
  • Запуск драйверов
  • Тест моторов и настройка параметров PID-контроллера
• Контроль движения
  • Пакет robot_control
  • Пакет robot_base
  • Тестируем движение робота
  • Визуализация одометрии
• Дистанционное управление
  • Установка драйверов
  • Пакет abot_teleop
  • Тестируем дистанционное управление
• Навигация
  • Лидар RPLIDAR A1
  • Теория навигации
  • Построение карты
  • Стек навигации ROS
  • Тестируем автономную навигацию
• Ответы на вопросы
• Заключение

Дистанционное управление

Мы можем управлять роботом через плагин rqt_robot_steering, однако в «боевых условиях» неудобно двигать ползунки слайдеров на компьютере. А ещё через плагин неудобно поворачивать роботом во время движения. Нам нужно удобное дистанционное управление роботом.

В будущем наш робот будет ездить самостоятельно. Однако нам всё равно понадобится ручное дистанционное управление. Прежде всего, для отладки программного обеспечения, и чтобы мы могли в случае чего перехватить автономное управление роботом.

Мы сделаем дистанционное управление для робота с помощью геймпада DualShock 4 от консоли Sony PlayStation 4 по Bluetooth. Если ранее вы где-то использовали свой DualShock 4, сбросьте его настройки до заводских, ткнув иголкой в кнопку Reset в маленьком отверстии на задней крышке геймпада.

Установка драйверов

Настройка Bluetooth на Raspberry Pi

Плата Raspberry Pi 4 уже имеет на борту Bluetooth-модуль, и нам не нужно покупать какие-либо дополнительные шилды и платы.

Чип Bluetooth на плате PRi общается с процессором Broadcom по аппаратному интерфейсу UART. Изначально эта аппаратная шина UART доступна для ввода-вывода пользователю. То есть нам нужно отказаться от этого UART-интерфейса и назначить туда Bluetooth-модуль.

Устанавливаем специальный пакет pi-bluetooth, который перенастроит аппаратный UART, подключит драйверы для модуля Bluetooth и создаст новое устройство в системе.

sudo apt-get install pi-bluetooth

Установим ПО для управления Bluetooth и при желании графический интерфейс:

sudo apt-get install bluetooth bluez bluez-tools
sudo apt-get install blueman 

Перезагружаем Raspberry. Проверяем, что Bluetooth-модуль RPi включился и работает (UP RUNNING):

hciconfig -a

Настройка Bluetooth-соединения

Теперь нам нужно связать геймпад и Bluetooth-модуль.

Чтобы геймпад перешёл в режим спаривания, нажмите одновременно кнопки «Share» и «PS4» и держите их одну-две секунды. Светодиодный индикатор геймпада начнёт быстро мигать (быстрее обычного).

Запускаем bluetoothctl и режим сканирования.

bluetoothctl
power on
scan on

DualShock 4 должен определиться как Wireless Controller. Нам нужен MAC-адрес геймпада, в нашем случае это 1C:66:6D:EB:88:DE.

Теперь, пока светодиод DualShock 4 быстро мигает (и идёт режим спаривания), создадим новую пару устройств. Также добавим наш геймпад в список доверенных устройств, чтобы при включении девайса подключение устанавливалось автоматически.

В том же bluetoothctl продолжаем:

trust 1C:66:6D:EB:88:DE
pair 1C:66:6D:EB:88:DE
connect 1C:66:6D:EB:88:DE

Создастся новая пара, а светодиод геймпада начнёт гореть ярко-синим цветом.

Установка драйвера DualShock 4

Теперь установим Linux-драйвер конкретно для геймпада DualShock 4.

Мы используем отличный драйвер ds4drv. Во-первых, потому что у него открытый исходный код, а во-вторых, потому что в ROS уже есть пакет-обёртка для этого драйвера.

Драйвер написан на Python, поэтому сперва установим pip, а уже используя его, установим ds4drv.

В терминале на Raspberry вводим:

sudo apt-get install python3-venv python3-pip
sudo apt-get install python-is-python3
sudo pip3 install ds4drv

Запустим драйвер ds4drv под root:

su root
ds4drv --hidraw

Пока драйвер запущен, в новом терминале проверим, появилось ли новое устройство ввода. У нас оно появилось под именем js0:

ls /dev/input | grep js

При желании проверить все кнопки джойстика можно утилитой jstest:

sudo apt-get install jstest-gtk
jstest /dev/input/js0

Устанавливем пакет ROS для DualShock 4

В ROS есть пакет ds4_driver, который является обёрткой драйвера ds4drv.

Пакет неофициальный — он создан и поддерживается пользователем naoki-mizuno. В сборке ROS Noetic этого пакета нет, поэтому мы соберём его вручную.

Для этого скачаем его и просто добавим в наш ROS-проект ros:

cd ~/ros/src
git clone https://github.com/naoki-mizuno/ds4_driver.git
cd ~/ros
catkin_make

Также нужно установить ROS-пакет для управления джойстиками — joy:

sudo apt-get install ros-noetic-joy

Пакет abot_teleop

Создадим в нашем проекте новый пакет для дистанционного управления роботом. Назовём его abot_teleop. В этом пакете будут храниться все наши ROS-ноды, которые каким-либо образом относятся к дистанционному управлению. Установим пакеты-зависимости:

• roscpp
• geometry_msgs
• sensor_msgs
• joy

Как это работает? Конвенционально в ROS ноды джойстиков публикуют сообщения типа sensor_msgs/Joy. Пакет ds4_driver, который мы добавили в наш проект, cобирает информацию о статусе всех кнопок джойстика через Linux-драйвер ds4drv. Затем пакет ds4_driver помещает состояние всех кнопок в сообщение и отправляет его в ROS-топик /joy. Соответственно, мы можем подписаться на этот топик и отслеживать, нажата ли какая-либо клавиша геймпада.

На джойстике много кнопок, и нам понадобятся не все из них. Пока мы будем использовать только левый и правый аналоговый манипулятор-грибок для задания скоростей робота. Вертикальное перемещение левого грибка будет отвечать за линейный вектор скорости робота вдоль оси X. Горизонтальное перемещение правого грибка будет отвечать за угловую скорость робота вокруг оси Z.

Класс AbotTeleop

Напишем простой класс C++, который реагирует на изменения состояний кнопок геймпада. Назовём его AbotTeleop.

В пакете abot_teleop создаём папку src, а в ней новый заголовочный С++ файл about_teleop.h.

Что делает этот класс? Он подписывается на топик /joy и получает данные о состояниях всех кнопок DualShock 4. Для грибков на геймпаде значения положения находятся диапазоне от -1 до 1, где 0 — это нейтральное положение.

Затем положения левого и правого грибка умножаются на коэффициенты _linear_speed_scale и _angular_speed_scale для преобразования позиции грибка в значение скорости. Коэффициенты задаются через параметрический сервер.

Полученные значения скорости преобразуются в вектора и помещаются в сообщение типа geometry_msgs::Twist. Готовое сообщение со скоростями для робота отправляется непосредственно в топик контроллера дифференциального привода — /mobile_about/cmd_vel.

#ifndef ABOT_TELEOP_H_
#define ABOT_TELEOP_H_

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>

constexpr uint8_t PS4_AXIS_STICK_LEFT_LEFTWARDS = 0;
constexpr uint8_t PS4_AXIS_STICK_LEFT_UPWARDS = 1;
constexpr uint8_t PS4_AXIS_STICK_RIGHT_LEFTWARDS = 2;
constexpr uint8_t PS4_AXIS_STICK_RIGHT_UPWARDS = 3;

class AbotTeleop {
public:
		AbotTeleop(ros::NodeHandle private_node);
private:
		ros::NodeHandle _node;
		ros::NodeHandle _private_node;
		ros::Subscriber _joy_sub;
		ros::Publisher _cmd_vel_pub;

		bool _last_zero_twist = true; 
		double _linear_speed_scale;
		double _angular_speed_scale;
		
		void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
};

AbotTeleop::AbotTeleop(ros::NodeHandle private_node) :
		_private_node(private_node) {
		_private_node.param<double>("linear_speed_scale", _linear_speed_scale, 0.0);
		_private_node.param<double>("angular_speed_scale", _angular_speed_scale, 0.0);
		_cmd_vel_pub = _node.advertise<geometry_msgs::Twist>("/mobile_abot/cmd_vel", 1);
		_joy_sub = _node.subscribe<sensor_msgs::Joy>("joy", 10, &AbotTeleop::joyCallback, this);
		ROS_INFO("Abot teleop node: Start");
}

void AbotTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) {
		geometry_msgs::Twist twist;

		double twist_linear_x_vel =  _linear_speed_scale * joy->axes[PS4_AXIS_STICK_LEFT_UPWARDS];
		double twist_angular_z_vel = _angular_speed_scale * joy->axes[PS4_AXIS_STICK_RIGHT_LEFTWARDS];

		twist.linear.x = twist_linear_x_vel;
		twist.angular.z = twist_angular_z_vel;

		if (twist_linear_x_vel == 0 && twist_angular_z_vel == 0) {
				if (_last_zero_twist == false) {
						_cmd_vel_pub.publish(twist);
						_last_zero_twist = true;
				} 
		} else {
				_last_zero_twist = false;
				_cmd_vel_pub.publish(twist);
		}
}

#endif // ABOT_TELEOP_H_

Нода abot_teleop

Создадим простоую ROS-ноду, которая будет работать с нашим классом AbotTeleop.

В папке src пакета abot_teleop создадим следующий файл abot_teleop.cpp:

#include "abot_teleop.h"

int main(int argc, char **argv) {
	ros::init(argc, argv, "abot_teleop");
	ros::NodeHandle private_node("~");
	AbotTeleop abotTeleop(private_node);
	ros::spin();
}

Добавим исполняемый файл в правило сборки пакета CMakelists.txt:

add_executable(abot_teleop src/abot_teleop.cpp)
target_link_libraries(abot_teleop ${catkin_LIBRARIES})

Соберём проект с новым пакетом:

cd ~/ros
catkin_make

Запуск ноды abot_teleop

Cоздадим новый файл запуска для новых нод, ответственных за дистанционное управление.

В пакете abot_teleop создадим папку launch, а в ней файл abot_teleop.launch.

Эти файлом мы будем запускать две ноды из двух новых пакетов нашего проекта — abot_teleop и ds4_driver.

<launch>
	<arg name="addr" default="" />
	<arg name="use_standard_msgs" default="true" />
	<arg name="autorepeat_rate" default="50" if="$(arg use_standard_msgs)" />
	<node pkg="ds4_driver" type="ds4_driver_node.py" name="ds4_driver" output="screen" >
		<param name="device_addr" value="$(arg addr)" />
		<param name="use_standard_msgs" value="$(arg use_standard_msgs)" />
		<param name="autorepeat_rate" value="$(arg autorepeat_rate)" if="$(arg use_standard_msgs)" />
		<param name="deadzone" value="0.1" />
	</node>
	<node pkg="abot_teleop" type="abot_teleop" name="abot_teleop" >
		<param name="linear_speed_scale" type="double" value="0.20"/> 
		<param name="angular_speed_scale" type="double" value="1.57"/>
	</node>
</launch>

Параметр deadzone отвечает за «мёртвую зону» грибков геймпада. Установим его равным 0.1. Так сигналы грибков будут не равны нулю после 10% их хода.

Настраиваем коэффициенты преобразования скоростей из положений грибков. В linear_speed_scale устанавливаем максимальную линейную скорость робота 0.2 м/с, а в angular_speed_scale угловую скорость 1.57 рад/с. С такими скоростями вполне удобно рулить роботом с геймпада.

Не забудьте, что нельзя задавать скорости больше тех, что указаны в файле настроек контроллера дифференциального привода abot_controllers.yaml! Также не стоит задавать слишком большие скорости, особенно угловые — иначе роботом станет тяжело рулить.

Включим новый файл запуска в общий файл запуска робота abot_description/bringup.launch:

<include file="$(find abot_teleop)/launch/abot_teleop.launch" />

Тестируем дистанционное управление

Всё готово для управления роботом с помощью DualShock 4.

На Raspberry запустим главный файл запуска робота bringup.launch от root:

cd ~/ros
su root
source devel/setup.bash
roslaunch abot_description bringup.launch

Сейчас граф наших ROS-нод и топиков выглядит так:

На настольном компьютере запустим визуализацию движения робота (display_movement.launch). Всё то же, что и в прошлый раз:

cd ~/ros
source devel/setup.bash
roslaunch abot_description display_movement.launch

Управляем роботом с помощью джойстика и наблюдаем одометрию в rviz.

Навигация

Давайте разбираться с навигацией робота.

Предположим, что вокруг робота есть какая-то среда. Чтобы ориентироваться в этой среде, робот должен «видеть» её и локализоваться в ней. Для этого роботы используют различные датчики и сенсоры. От набора имеющихся у робота датчиков зависит, как он видит окружающую среду.

Эти датчики могут быть простыми или сложными, работать в 2D- или 3D-пространстве. Это могут быть цифровые камеры, камеры глубины (Time-of-Flight), лазерные / ультразвуковые / инфракрасные дальномеры, простые бинарные контактные датчики (бамперы) и так далее. Чем больше датчиков установлено на роботе и чем лучше они синхронизированы друг с другом, тем больше информации об окружающем мире может получить робот. ROS поддерживает широкий спектр датчиков. Подробнее читайте в документации на сенсоры в ROS.

Лидар RPLIDAR A1

Новичкам, как мы, лучше всего начинать с простых, но эффективных датчиков. Самым популярным датчиком среди новичков является лидар с полем зрения в 360°.

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

Мы выбрали самый популярный и недорогой лидар RPLIDAR A1 от SLAMTECH.

Этот лидар обеспечивает 360-градусное поле сканирования с частотой обновления от 2 Гц до 10 Гц. Дальность действия RPLIDAR A1 составляет около 8 метров. Лидар поставляется в виде готового устройства. Вы можете просто закрепить датчик на своём роботе и подключить его к бортовому компьютеру. Самый простой способ подключения RPLIDAR — USB-порт, тем более, что преобразователь USB-UART идёт в комплекте.

Но главная причина, по которой мы используем конкретно этот лидар — это официальный пакет rplidar в ROS, который поддерживается производителем датчика — SLAMTECH.

Крепление лидара

Чтобы закрепить лидар на роботе, мы разработали ещё одну деталь-площадку и напечатали её на 3D-принтере. Как и раньше, печатали мы на Prusa i3 MK3S из серого PLA-пластика eSUN.

В эту площадку мы встроили лидар и закрепили его винтами M2,5×25 с гайками, шайбами и гроверными шайбами.

Саму площадку установили на робота через стойки М3×60.

Когда лидар вращается, вокруг него не должно быть никаких других частей робота, которые могли бы помешать сканирующему лучу. На разных роботах лидары устанавливаются множеством способов, и чаще всего лидар является самой высокой точкой робота. На наш взгляд, лучше всего установить лидар ближе к земле, чтобы повысить эффективность обнаружения препятствий. Однако низко установленный лидар имеет меньший угол обзора и помехи в виде других окружающих его частей робота. Мы решили установить лидар традиционно сверху робота. Таким образом, наш лидар не сможет обнаружить объекты высотой ниже робота, но зато сохранит полноценный круговой обзор.

Обновляем описание робота в 3D и URDF

Регистрируем изменения конструкции нашего реального робота в 3D-модели.

Также нужно обновить URDF-описание робота. В этот раз, помимо обновления визуальной части робота, у нас появится описание нового слоя URDF для датчиков и сенсоров.

Каждый сенсор или датчик в URDF-описании должен иметь собственный сегмент. Нам нужно создать сегмент для лидара.

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

Для RPLIDAR A1 в ROS рекомендуется следующая ориентация системы координат:

В SolidWorks создаём новую систему координат сегмента лидара CS_LIDAR. Начальная точка системы координат находится условно в центре лидара и в плоскости лазера.

Как и прежде, используем плагин solidworks_urdf_exporter для экспорта URDF-описания из 3D-модели.

Обновляем визуальную составляющую для нашего базового сегмента abot_base. Новый сегмент — abot_lidar. Теперь у базового сегмента будет четыре сегмента-наследника:

• abot_left_wheel
• abot_right_wheel
• abot_caster_wheel
• abot_lidar

Назовём сочленение сегмента лидара с базой робота lidar_to_base. В поле системы координат устанавливаем нашу систему координат SolidWorks CS_LIDAR. Тип сочлененния — fixed. В качестве визуальной составляющей сегмента выбираем 3D-модель лидара RPLIDAR A1.

Когда всё готово, нажимаем «Preview and Export...» и заканчиваем процесс экспорта URDF-описания.

Переносим новые сгенерированные экспортёром 3D-модели .STL в папку meshes в пакет описания нашего робота abot_description.

Как и прежде, экспортёр выдаёт нам всё в виде одного большого URDF-файла, а нам нужно разбить его и разложить по полочкам. Что изменилось в описании? Изменились инерционные характеристики сегмента abot_base и добавился новый сегмент abot_lidar c новым сочленением lidar_to_base.

Для описания всех сенсоров и датчиков нашего робота создадим новый отдельный файл, используя макросы xacro. Назовём его abot_sensors.xacro и поместим в папку urdf пакета abot_description.

В этот новый файл скопируем сгенерированное описание лидара. Пусть все сенсоры в нашем описании имеют жёлтый цвет.

<?xml version="1.0" encoding="utf-8"?>
<robot name="abot"
	xmlns:xacro="http://www.ros.org/wiki/xacro">
	<!-- lidar -->
	<link name="abot_lidar">
		<inertial>
		<origin xyz="0 0 0" rpy="0 0 0"/>
		<mass value="0"/>
		<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
	</inertial>
	<visual>
		<origin xyz="0 0 0" rpy="0 0 0"/>
		<geometry>
			<mesh filename="package://abot_description/meshes/abot_lidar.STL"/>
		</geometry>
		<material name="Yellow" />
	</visual>
	<collision>
		<origin xyz="0 0 0" rpy="0 0 0"/>
		<geometry>
			<mesh filename="package://abot_description/meshes/abot_lidar.STL"/>
		</geometry>
	</collision>
	</link>
	<joint name="lidar_to_base" type="fixed">
		<origin xyz="-0.01 0 0.1419" rpy="0 0 ${PI}"/>
		<parent link="abot_base"/>
		<child link="abot_lidar"/>
		<axis xyz="0 0 0"/>
	</joint>
</robot>

Включите новый файл описания датчиков abot_sensors.xacro в наш основной файл описания abot.xacro:

<xacro:include filename="$(find abot_description)/urdf/abot_sensors.xacro" />

Протестируем, как выглядит наше описание робота с лидаром в rviz. На настольном компьютере запустим файл визуализации модели display_model.launch:

cd ~/ros
source devel/setup.bash
roslaunch abot_description display_model.launch

Визуализация робота обновилась:

Далее проверяем положение и систему координат нового сегмента лидара в пространстве:

Теперь дерево сегментов и сочленений нашего робота выглядит так:

Подключение лидара и USB alias

RPLIDAR A1 подключается к Raspberry Pi по USB через преобразователь USB-UART. Давайте создадим alias — псевдоним для USB-устройства лидара в Linux.

Зачем это нужно? Прежде всего, чтобы операционная система всегда знала, к какому физическому USB-порту RPi подлючён именно лидар, а не другое оборудование. И чтобы наша программа не стала опрашивать лидар на том порту, где его физически нет, а подключена, например, клавиатура или мышь.

Подключим лидар в любой порт Raspberry, используя короткий кабель Micro-USB длиной 10 см.

Посмотрим, как устройство лидара определилось в операционной системе на Raspberry:

lsusb
ls /dev | grep ttyUSB

Наш лидар определился как Bus 001 Device 003: ID 10c4:ea60 Silicon Labs CP210x UART Bridge. Здесь закодирована определённая информация. 10c4 — это идентификтор поставщика продукта (ATTRS{idVendor}), а ea60 — это идентификатор самого продукта (ATTRS{idProduct}). Чаще всего эти идентификаторы уникальны для каждого USB-устройства. По ним операционная система и будет определять, что к ней подключили именно лидар.

Вы также можете получить больше информации об атрибутах USB-устройства командой:

udevadm info -a -n /dev/ttyUSB0

Создадим новое udev-правило. Под пользователем root в директории /etc/udev/rules.d создадим файл 99-usb-serial.rules.

su root
nano /etc/udev/rules.d/99-usb-serial.rules

В этот файл поместим следующее правило:

SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="lidar"

В соответствии с этим правилом любое подключённое USB-устройство с указанными параметрами idVendor и idProduct будет определяться в системе как lidar.

Перезагрузим udev-правила, переподключим RPLIDAR к другому порту и убедимся, что новый alias работает:

sudo udevadm control --reload-rules && udevadm trigger
ls -l /dev/lidar

ROS-пакет для RPLIDAR

Чтобы использовать RPLIDAR A1 в ROS, вам не нужно устанавливать никаких дополнительных драйверов, просто возьмите ROS-пакет rplidar_ros. В официальной сборке для ROS Noetic этого пакета нет, но мы можем просто склонировать его в наш ROS-проект — так же, как мы поступили с драйверами для геймпада.

cd ~/ros/src
git clone https://github.com/Slamtec/rplidar_ros.git
cd ~/ros
catkin_make

Чтобы запустить ноды для нашего лидара, создадим новый файл запуска abot_lidar.launch. Поскольку лидар — это сенсор, и он относится к аппаратному уровню нашего робота, разместим файл запуска в пакете abot_driver в папке launch.

Данный файл будет запускать ноду rplidar из пакета rplidar_ros. По умолчанию RPLIDAR отдаёт данные по интерфейсу UART со скоростью 115200 бод. В качестве пути к устройству указываем созданный нами ранее псевдоним /dev/lidar. Также здесь необходимо указать имя сегмента frame_id вашего лидара из описания URDF. Сегмент нашего лидара называется abot_lidar.

Значение Boost параметра scan_mode установит наивысшую скорость сканирования лидара RPLIDAR A1 — 10 герц.

<launch>
	<node name="rplidarNode" pkg="rplidar_ros"  type="rplidarNode" output="screen">
		<param name="serial_port" type="string" value="/dev/lidar"/>  
		<param name="serial_baudrate" type="int" value="115200"/>
		<param name="frame_id" type="string" value="abot_lidar"/>
		<param name="inverted" type="bool" value="false"/>
		<param name="angle_compensate" type="bool" value="true"/>
		<param name="scan_mode" type="string" value="Boost" />
	</node>
</launch>

Включим новый файл запуска нод лидара в общий файл запуска всех драйверов нашего робота abot_drivers.launch:

<include file="$(find abot_driver)/launch/abot_lidar.launch" />

Визуализация данных с лидара

Давайте проверим, как работает наш лидар и что он показывает.

На Raspberry Pi запускаем наш главный файл запуска робота bringup.launch от пользователя root:

cd ~/ros
su root
source devel/setup.bash
roslaunch abot_description bringup.launch

Сейчас граф наших ROS-нод и топиков выглядит так:

Проверим, что лидар заработал и начал отдавать данные. Конвенционально лидары в ROS отдают сообщения типа sensor_msgs/laserscan, которые публикуются в топик /scan. На настольном компьютере посмотрим список топиков, у нас должен появиться топик с именем /scan.

На настольном компьютере запускаем уже имеющийся у нас файл визуализации движения робота display_movement.launch.

source devel/setup.bash
roslaunch abot_description display_movement.launch

В уже привычном окне rviz добавим новые объекты для визуализации. На панели Displays нажмём «Add → By topic» и выберем визуализацию топика /scan.

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

Наш робот начинает видеть!

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

Теория навигации

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

Автономная навигация робота строится на трёх фундаментальных принципах:

• Построение карт — Mapping.
• Локализация в пространстве — Localization.
• Планирование пути — Path planning.

Задача маппинга состоит в том, чтобы ответить на вопрос робота: «Как выглядит окружающее меня пространство?» Во время картографирования данные с различных датчиков передаются роботу. На основе этих данных робот строит карту окружающего мира (map) в понятном для себя представлении — топологическом или метрическом.

Задача локализации состоит в том, чтобы ответить на вопрос робота: «Где я нахожусь в окружающем меня мире?» Во время локализации робот определяет своё положение относительно карты (map), которая может быть уже известна роботу или строиться в режиме реального времени. При локализации на заранее известной карте робот должен уметь определять своё положение, где бы он не находился.

Задача планирования пути состоит в том, чтобы ответить на вопрос робота: «Как я могу добраться до определённой точки на карте?» Целевая точка на карте может быть установлена оператором робота или самим роботом. Он должен уметь cамостоятельно прокладывать траекторию движения к целевой точке на карте и добираться до этой точки. Кроме этого, траектория движения должна быть оптимальна и безопасна для робота.

Различные комбинации этих трёх процессов позволяют мобильному роботу решать разнообразные навигационные задачи:

• SLAM (Simultaneous Localization and Mapping) — метод одновременной локализации и построения карты, самый популярный в робототехнике. Существует тесная связь между процессами маппинга и локализации: они не могут быть разделены в условиях неизвестной среды вокруг робота. Это происходит, потому что робот должен знать своё точное текущее положение, чтобы построить карту. В то же время роботу нужна качественная карта, чтобы определить своё текущее положение. Пример использования метода: оператор дистанционно управляет движением робота, который не имеет подготовленной карты, а строит её в режиме реального времени с помощью сенсоров и локализуется в пространстве через одометрию и сенсоры.

• Active localisation — метод активной локализации, который использует планирование пути, чтобы направлять робота к целевым точкам на карте для уточнения его текущего положения (локализации). Пример использования метода: робот имеет подготовленную карту окружающего его пространства и движется к целевой точке на карте самостоятельно (автономно без оператора). При этом, в процессе движения, робот может специально проезжать через контрольные точки на карте, например для того чтобы зафиксировать установленными на нем камерами специальные черно-белые маркеры расположенные на стенах. Анализируя каждый замеченный подобным образом маркер робот уточняет свое положение на карте.

• Exploration — метод исследования, который предполагает, что робот способен определять своё точное местоположение. Метод фокусируется на эффективном движении робота в неизвестной среде для построения им карты в реальном времени. Пример использования метода: робот не имеет подготовленной карты и движется самостоятельно без участия оператора. При этом в режиме реального времени робот движется в неизвестную для него территорию, непрерывно строит карту и планирует свой маршрут.

• SPLAM (Simultaneous Planning, Localization and Mapping) — метод одновременной локализации, построения карт и планирования пути. Комбинация всех вышеописанных методов. Пример использования метода: робот автономен, не имеет подготовленной карты и практически никакой готовой информации об окружающем мире. Робот в режиме реального времени самостоятельно строит карту, локализуется на ней и планирует своё дальнейшее движение.

Что из всего этого будем использовать мы? Для начала мы составим методом SLAM глобальную карту помещения, где будет работать наш робот.

Затем мы дадим роботу эту карту помещения и будем управлять им, задавая целевые точки (Goal points) на ней. До этих точек робот должен будет добираться самостоятельно, объезжая все препятствия на пути и при этом не теряя себя на карте. Таким образом, мы будем использовать метод Active localisation.

Построение карты

Существует несколько алгоритмов SLAM. Мы воспользуемся алгоритмом OpenSLAM, для которого в ROS есть готовый пакет — gmapping.

В пакете gmapping для решения задачи картографирования используются данные с лидара, а локализация на карте осуществляется с помощью фильтра частиц (particle filter).

Как будет строиться наша карта? Она будет строиться по принципу «Cетки занятости» (Occupacy grid). Мы возьмём всё пространство вокруг робота и разлинуем его на множество квадратных ячеек. Полученное таким образом разлинованное пространство называется сеткой занятости, а каждая ячейка — блоком (block). Каждый блок сетки может иметь три состояния: занято, свободно или неизвестно. Свободный блок на сетке означает, что в пространстве это место пусто. Занятый блок на сетке означает, что в пространстве на месте этого блока есть преграда или какой-то объект. Используя данные с лидара, наш робот будет в режиме реального времени сканировать окружающее пространство и заполнять блоки сетки занятости значениями «занято/свободно» через вероятностную оценку. Непросканированные блоки останутся в состоянии «неизвестно».

Пакет abot_slam

Сперва установим пакет gmapping для ROS Noetic:

sudo apt-get install ros-noetic-gmapping

Данный пакет предназначен для работы с роботами, построенными именно с использованием инфраструктуры ROS. Для любых других роботов данный пакет не подойдет. Чтобы пакет gmapping заработал, у вас должны быть:

• Мобильный робот, построенный на инфраструктуре ROS, описанный с использованием URDF и robot_description, и с визуализацией.
• Реализованное управление роботом для оператора.
• Установленный на роботе датчик-лидар, который отсылает сообщения в топик /scan.
• Одометрия с робота, которая собирается в топике /odom.

Как видите, у нас уже выполнены все пункты.

Создадим новый пакет для SLAM в нашем рабочем пространстве ros. Назовём его abot_slam. В качестве пакетов-зависимостей указываем:

• gmapping
• sensor_msgs

В пакете abot_slam создадим три папки: launch, config и maps. В папке launch будут храниться файлы запуска SLAM-ноды gmapping, в папке config — настройки пакета gmapping, а в папку maps мы будем сохранять готовые карты.

Настройка SLAM

В папке config создадим новый файл формата .yaml с параметрами пакета gmapping. Назовём его gmapping_params.yaml.

Настройка параметров SLAM-методов — долгое и кропотливое занятие. Параметров много, и от них зависит качество вашей карты. На практике большинство этих параметров выявляется подбором для каждого конкретного робота с его набором сенсоров. Подробнее о настройке параметров пакета gmapping для ROS вы можете прочесть в его документации.

Нас прежде всего интересуют следующие параметры:

• base_frame — имя базового сегмента описания вашего робота. У нас это base_footprint.
• odom_frame — имя фрейма, который используется для визуализации одометрии. У нас он называется odom.
• map_frame — имя фрейма, который используется для визуализации отображения карты. У нас это map.
• xmax, xmin, ymax, ymin — размеры области (в метрах), на которой будет строиться карта. Размеры задаются относительно нуля системы координат фрейма map_frame.
• particles — количество частиц в фильтре локализации. Мы ставим 30.
• delta — разрешение карты. Размер стороны квадратного блока сетки занятости (в метрах). Мы устанавливаем размер одного блока в 2,5 см или 0.025 метра. Очевидно, чем меньше блок, тем точнее карта.
• map_update_interval — частота обновления карты в процессе построения (в секундах). Уменьшение этого интервала заставит сетку занятости обновляться чаще ценой большей вычислительной нагрузки. Мы обновляем карту раз в секунду.
• maxUrange — максимальный радиус действия лидара (в метрах). Лидары работают на очень большие расстояния, что не всегда необходимо. Нам не нужно постоянно знать, что находится в 100 метрах от робота, если сам робот размером всего в 30 см. Желательно задать радиус действия меньше реального. Мы ставим радиус действия 7.0 метров.
• linearUpdate — при движении робота на такое расстояние (в метрах) алгоритм произведёт новую оценку данных, полученных с лидара. Устанавливаем параметр в 20 см или 0.2 метра.
• angularUpdate — при повороте робота на такой угол (в радианах) алгоритм произведёт новую оценку данных, полученных с лидара. Устанавливаем параметр в 0.5 радиан.
• temporalUpdate — при простое робота на месте через такое время (в секундах) алгоритм произведёт новую оценку данных с лидара. При простое будем считывать данные с лидара 5 раз в секунду — 0.2.

Наша готовая карта будет представлять собой изображение, состоящее из белых, чёрных или пустых блоков. Чёрный блок указывает на занятое пространство, белый — на свободное. Пустой блок говорит о том, что наличие преград в этой области неизвестно. Размер одного нашего блока равен 20 см, а вся карта будет размером 40×40 метров.

Так выглядит наш файл настроек gmapping_params.yaml:

base_frame: base_footprint
odom_frame: odom
map_frame: map

map_update_interval: 1.0
maxUrange: 7.0

linearUpdate: 0.2
angularUpdate: 0.5
temporalUpdate: 0.2
resampleThreshold: 0.5

particles: 30

xmax: 20.0
xmin: -20.0
ymax: 20.0
ymin: -20.0

delta: 0.025

# Keep default
sigma: 0.05
kernelSize: 1
lstep: 0.05
astep: 0.05
iterations: 5
lsigma: 0.075
ogain: 3.0
lskip: 0

srr: 0.1
srt: 0.2
str: 0.1
stt: 0.2

llsamplerange: 0.01
llsamplestep: 0.01
lasamplerange: 0.005
lasamplestep: 0.005

Файлы запуска SLAM и визуализации

В папке launch создадим новый файл запуска для метода SLAM. Назовём его так же, как и пакет abot_slam.launch. Этим файлом мы запустим ноду slam_gmapping пакета gmapping и загрузим SLAM-параметры на параметрический сервер ROS.

<launch>
	<node pkg="gmapping" type="slam_gmapping" name="abot_slam_gmapping" output="screen">
		<rosparam command="load" file="$(find abot_slam)/config/gmapping_params.yaml"/>
	</node>
</launch>

Теперь создадим файл запуска для визуализации SLAM. Визуализировать процесс построения карты будем, как обычно, через rviz на настольном компьютере. В пакете abot_description в папке launch создадим новый файл запуска display_slam.launch. В нём мы запустим ноду визуализации rviz с новыми настройками abot_slam.rviz и сам метод SLAM.

<launch>
	<arg name="rvizconfig" default="$(find abot_description)/rviz/abot_slam.rviz"/>
	<arg name="model" default="$(find abot_description)/urdf/abot.xacro"/>
	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)"/>
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="false" output="screen"/>
	<include file="$(find abot_slam)/launch/abot_slam.launch"/>
</launch>

В новых настройках about_slam.rviz ноды rviz нужно добавить новый элемент визуализации типа Map. Для этого в rviz на панели Displays нажмём «Add → By topic» и выберем визуализацию топика /map.

В настройках визуализации значение глобального кадра «Global Options → Fixed Frame» должно быть установлено в map. Для удобства отображения карт камеру можно расположить перпендикулярно земле. А ещё при желании можно отображать данные с лидара из топика /scan.

Тестируем SLAM

Давайте протестируем работу SLAM на нашем роботе. Алгоритм SLAM трудоёмкий и требует сложных вычислений. Мы можем запустить его на Raspberry Pi 4, но он «сожрёт» всю её вычислительнную мощность, и визуализация алгоритма будет иметь очень низкую частоту кадров (fps). В действительности SLAM и не нужно запускать на роботе, ведь мы используем его лишь для генерации постоянной, глобальной карты помещения, а сам робот в данном случае дистанционно управляется оператором. Поэтому SLAM запускается на настольном компьютере.

Убедимся, что на настольном компьютере запущено ядро roscore. На Raspberry запустим главный файл запуска робота bringup.launch:

cd ~/ros
su root
source devel/setup.bash
roslaunch abot_description bringup.launch

На настольном компьютере запустим файл запуска SLAM и визуализации — display_slam.launch:

cd ~/ros
source devel/setup.bash
roslaunch abot_description display_slam.launch

Примерно так выглядит наше окно rviz:

А так выглядит граф наших ROS-нод и топиков:

Как видно, некоторые блоки нашей карты занятости уже начали заполняться статусами «свободно / занято / неизвестно».

Построим карту!

Чем больше площади помещения вы охватите, тем лучше. Ездить роботом желательно плавно, без резких ускорений и торможений. Для составления более качественной карты нужно проехать роботом вдоль всех стен и вокруг всех предметов на полу. Для большей точности можно проехать по одним и тем же местам несколько раз.

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

Мы проехали по всему помещению всего один раз.

В процессе езды допускается незначительное проскальзывание колёс и заносы, такие ошибки компенсируются встроенной в gmapping локализацей. Однако нельзя вручную поднимать робота, переставлять его с места на место или поворачивать — такие ошибки исправить не получится, и вам придется запускать SLAM по новой.

Сохраняем карту

Когда карта будет готова, нужно её сохранить путём однократного запуска ноды map_saver пакета map_server. Готовые карты сохраняются в виде файла с расширением .yaml и графического изображения в формате .pgm.

Пакет map_server устанавливается командой:

sudo apt-get install ros-noetic-map-server

На настольном компьютере, пока активен SLAM и визуализация, запустим в новом терминале map_saver с указанием имени карты и пути сохранения. Карту сохраним в папку maps пакета abot_slam:

cd ~/ros/src/abot_slam/maps/
rosrun map_server map_saver -f map1

Вот такая карта у нас получилась:

Редактируем карту

Теперь нужно отредактировать созданную карту.

Наша карта не очень точная: это обусловлено качеством лидара, местом его установки и сложностью помещения. Заметьте, что робот отразил на карте всё, что он видел именно в плоскости лидара. Объекты и преграды, которые были ниже уровня лидара, робот не заметил. Наример, наш робот не может заметить провод на пути движения или полку шкафа, которая расположена ниже лидара.

При желании вы можете открыть карту и отредактировать её. Можно вручную закрасить блоки на сетке карты в чёрный или белый цвет (построить на карте «искусственные стены»). Чёрный цвет для мест, куда роботу не стоит ехать, а белый — для мест, куда ехать можно. Например, можно закрыть двери во всех дверных проёмах. Редактирование сгенерированных карт не считается каким-то «читом», скорее это правило хорошего тона.

Это важно! Не стоит злоупотреблять чисткой карты: на ней отражено именно то, как робот видит помещение, а не вы сами. Если слишком сильно изменить карту, робот не сможет на ней локализоваться.

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

Стек навигации ROS

Когда карта готова, можно начать реализацию навигации и планирования маршрута на ней.

Для навигации робота в ROS мы используем так называемый стек пакетов навигации navigation.

Установим стек пакетов командой:

sudo apt-get install ros-noetic-navigation

Этот стек содержит множество реализованных функций для 2D-навигации роботов. Стек navigation работает исключительно на роботах, построенных на инфраструктуре ROS. Для минимальной работы стека ваш робот должен иметь:

• Топик с одометрией — у нас это /odom.
• Контроллер движения — у нас это /mobile_abot.
• Топик с данными с лидара — у нас это /scan.
• Подготовленную карту.

Создадим новый пакет, который будет отвечать за навигацию нашего робота. Мы назвали его abot_navigation. В качестве пакетов зависимостей устанавливаем:

• geometry_msgs
• sensor_msgs

В пакете создадим две папки config и launch для хранения настроек навигации и для запуска нод навигации.

Навигация нашего робота будет основана на ROS-нодах из двух пакетов навигационного стека ROS:

• move_base
• amcl

Пакет move_base отвечает за планирование пути и достижение роботом указанной точки на карте.

Пакет amcl — это пакет активной локализации, название которого расшифровывается как Adaptive Monte Carlo Localization. В данном пакете реализована локализация робота на карте с использованием метода Монте-Карло.

Ознакомьтесь с официальной документацией ROS на эти пакеты. Попытайтесь понять, для чего предназначены ноды этих пакетов и как они работают.

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

Настройка локализации

Сперва настроим локализацию робота на карте. Как говорилось ранее, для локализации мы используем пакет amcl и ноду amcl с реализованным алгоритмом Монте-Карло.

Как работает алгоритм локализации Монте-Карло? Он использует теорию вероятности, математическое ожидание и фильтр частиц (particle filter). Попробуем объяснить на грубом примере, как можно проще.

Допустим, наш робот стоит где-то в помещении, и мы не знаем, где он. Но у нас есть текущие данные с лидара и подготовленная карта помещения. На первой итерации алгоритм amcl так же не знает, где находится робот, который неподвижно стоит на месте.

Используя данные с лидара, можно определить расстояния до всех окружающих робота преград. Алгоритм использует эти расстояния, чтобы определить, где бы мог оказаться робот на карте. Если алгоритм находит на карте такую позицию, в которой расстояния до окружающих преград соответствуют расстояниям, полученным с лидара, то такая позиция помещается в фильтр частиц.

Допустим, алгоритм нашел 3000 вероятных позиций робота на карте, то есть в нашем фильтре 3000 частиц (particles). Изначально «вес» всех этих частиц одинаков, то есть, робот может оказаться в любой из них с одинаковой вероятностью.

Затем представьте, что мы сдвинули робота, например, проехали им вперёд. Одометрия зафиксировала, что робот сдвинулся вперёд на 30 см. Алгоритм берёт известные ему 3000 вероятных позиций и так же сдвигает их на 30 см вперёд. Расстояния до преград, полученные с лидара, изменились. Алгоритм повторно начинает сравнивать текущие расстояния с лидара с уже сдвинутыми позициями робота в фильтре частиц и видит, что из изначальных 3000 позиций сейчас уже подходят, например, только 500. Остальные 2500 позиций выбрасываются из фильтра — предположение, что робот мог там находиться, было ложным. А вес оставшихся 500 позиций в фильтре растет — вероятность, что робот в этих точках, повышается.

После этого робот движется ещё раз. Алгоритм опять производит сравнения, и вот уже возможных позиций и частиц в фильтре осталось совсем не так много. Когда частиц в фильтре осталось мало, а их вес велик, алгоритм начинает считать среднюю частицу с неким усреднённым значением из всех оставшихся. С большой вероятностью робот находится на той позиции на карте, которая соответствует этой самой средней частице.

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

В папке launch пакета abot_navigation создадим новый файл запуска локализации amcl.launch. В нём мы запустим ноду amcl со следующими параметрами:

<launch>
	<!-- Arguments -->
	<arg name="scan_topic" default="scan" />
	<arg name="map_topic" default="map" />
	<arg name="initial_pose_x" default="0" />
	<arg name="initial_pose_y" default="0" />
	<arg name="initial_pose_a" default="0" />
	<!-- AMCL -->
	<node pkg="amcl" type="amcl" name="amcl">
		<param name="min_particles" value="500" />
		<param name="max_particles" value="5000" />
		<param name="kld_err" value="0.02" />
		<param name="kld_z" value="0.99" />
		<param name="update_min_d" value="0.20" />
		<param name="update_min_a" value="0.20" />
		<param name="resample_interval" value="1.0" />
		<param name="transform_tolerance" value="0.5" />
		<param name="recovery_alpha_slow" value="0.001" />
		<param name="recovery_alpha_fast" value="0.1" />
		<param name="initial_pose_x" value="$(arg initial_pose_x)" />
		<param name="initial_pose_y" value="$(arg initial_pose_y)" />
		<param name="initial_pose_a" value="$(arg initial_pose_a)" />
		<param name="gui_publish_rate" value="50.0" />
		<remap from="scan" to="$(arg scan_topic)" />
		<param name="laser_max_range" value="7.0" />
		<param name="laser_max_beams" value="200" />
		<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_likelihood_max_dist" value="2.0" />
		<param name="laser_model_type" value="likelihood_field" />
		<param name="odom_model_type" value="diff" />
		<param name="odom_alpha1" value="0.2" />
		<param name="odom_alpha2" value="0.2" />
		<param name="odom_alpha3" value="0.2" />
		<param name="odom_alpha4" value="0.2" />
		<param name="odom_frame_id" value="odom" />
		<param name="base_frame_id" value="base_footprint" />
		<param name="use_map_topic" value="true" />
		<remap from="map" to="$(arg map_topic)" />
	</node>
</launch>

Подробную информацию о ноде и её параметрах вы можете найти в документации на amcl. Параметров очень и очень много. Рассмотрим самые важные для нас:

• initial_pose_x,initial_pose_y, initial_pose_a — исходные координаты робота на карте и его ориентация относительно карты в начале работы алгоритма.
• min_particles, max_particles — минимальное и максимальное количество обсчитываемых вариантов, где может находиться робот (количество частиц в фильтре). В визуализации rviz это количество зелёных стрелочек (particle cloud swarm). Чем больше частиц в фильтре, тем больше понадобится вычислительной мощности. Чем меньше частиц — тем хуже локализация и, как следствие, навигация.
• update_min_d — линейное расстояние (в метрах), которое робот должен проехать для обновления фильтра частиц.
• update_min_a — угол (в радианах), на который робот должен повернуться для обновления фильтра частиц.
• resample_interval — количество обновлений фильтра перед повторной выборкой.
• laser_max_range, laser_max_beams — максимальное расстояние, на которое «бьют» лучи лидара, и количество этих лучей в выборке сравнения.
• odom_model_type — тип одометрии. У нас это дифференциальный привод — diff.
• odom_frame_id — имя фрейма одометрии. У нас это odom.
• base_frame_id — имя базового фрейма робота. У нас это сегмент base_footprint.

В той же папке launch созадим новый файл запуска для всех нод навигации робота. Назовем его abot_navigation.launch.

Этим файлом мы будем запускать нашу локализацию amcl.launch, а также загружать карту на ROS-сервер карт map_server:

<launch>
	<arg name="map_file" default="$(find abot_slam)/maps/map.yaml" />
	<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)" />
	<include file="$(find abot_navigation)/launch/amcl.launch" />
</launch>

Настраивать параметры навигации и локализации без визуализации практически невозможно. Поэтому в пакете abot_description в папке launch создадим файл запуска визуализации навигации. Назовем его display_navigation.launch.

<launch>
	<arg name="rvizconfig" default="$(find abot_description)/rviz/abot_navigation.rviz" />
	<arg name="model" default="$(find abot_description)/urdf/abot.xacro" />
	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="false" />
	<include file="$(find abot_navigation)/launch/abot_navigation.launch" />
</launch>

В этом файле мы запустим файлы запуска навигации и попутно пакет rviz с новыми настройками визуализации abot_navigation.rviz. Исходные настройки визуализации такие же, как и при визуализации SLAM (abot_slam.rviz).

Убедимся, что на настольном компьютере запущено ядро roscore. На Raspberry запустим главный файл запуска робота bringup.launch:

cd ~/ros
su root
source devel/setup.bash
roslaunch abot_description bringup.launch

На настольном компьютере запустим файл запуска визуализации навигации — display_navigation.launch:

cd ~/ros
source devel/setup.bash
roslaunch abot_description display_navigation.launch

Сейчас граф наших ROS-нод и топиков выглядит так:

Значение глобального кадра «Global Options → Fixed Frame» должно быть установлено в map. Добавим визуализацию работы нашего алгоритма локализации. Для этого отобразим на карте фильтр частиц алгоритма. В rviz на панели Displays нажмем «Add → By topic» и выберем визуализацию топика /particlecloud.

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

В настройках amcl мы указали, что изначально робот находится в точке с координатами (0, 0) на карте. Это та самая позиция, откуда робот начал движение при построении карты. Если при навигации и локализации робот начинает своё движение из другой точки на карте, то её нужно задать заранее. Для этого в rviz нужно нажать на кнопку 2D Pose Estimate и задать вектор текущей позиции робота на карте. Начало вектора — это, собственно, координата на карте, а направление — то, куда «смотрит» ваш робот.

Когда исходная точка задана, попробуйте порулить роботом дистанционно и понаблюдать за работой алгоритма локализации. Каждое движение робота будет уточнять его позицию на карте. Количество частиц в фильтре будет уменьшаться, как и количество стрелочек вокруг робота в визуализации. Чем меньше стелочек вокруг робота, и чем ближе они к его базовому сегменту — тем лучше робот осознаёт, где он находится.

На этом видео мы загружаем пакет навигации и визуализацию навигации. Затем мы обозначаем исходную точку и вектор положения робота кнопкой 2D Pose Estimate, включаем отображение облака частиц локализации и начинаем движение роботом с джойстика. На видео видно, как со временем уменьшается количество зелёных стрелочек вокруг робота, то есть его возможных позиций на карте.

Настройка планировщика пути

Теперь настроим ноду move_base. Но сперва разберёмся в фундаментальных основах.

Как работает нода move_base? Оператор робота (или же сам робот) задаёт целевую точку на плоскости, в которую робот должен добраться. Нода move_base строит маршрут до этой точки и движется по нему, объезжая препятствия и контролируя значения скоростей (cmd_vel) на входе контроллера движения робота (у нас это mobile_abot).

Это важно! При расчёте траектории движения робота move_base работает с двумя типами планировщиков пути одновременно: глобальным (global planner) и локальным (local planner).

Глобальный планировщик пути прокладывает маршрут к месту назначения, используя глобальную карту (/map). Она может быть статической (static), то есть заранее готовой, и задаваться пользователем через сервер карт (map_server), или может строиться в режиме реального времени (задача exploration). Наша глобальная карта статическая, и это та самая карта, которую мы построили методом SLAM.

Это важно! Задача глобального планировщика — вычисление безопасного маршрута движения робота из исходной точки (initial pose) в заданную точку (goal pose) на карте.

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

В ROS есть несколько реализованных глобальных планировщиков пути:

• global_planner
• carrot_planner
• navfn

Мы воспользуемся глобальным планировщиком navfn.

Но что делать, если робот едет из точки А в точку Б, а мы прямо на его пути внезапно установили преграду? Ведь этой преграды не было на глобальной карте, и глобальный планировщик пути не знает о ней. Робот будет думать, что преграды нет, и просто врежется в неё. Чтобы такого не случалось, существует локальный планировщик пути, который работает в паре с глобальным.

Это важно! Задача локального планировщика — избегать препятствия на пути движения, следуя по маршруту, созданному глобальным планировщиком.

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

В ROS есть несколько реализованных локальных планировщиков пути:

• base_local_planner
• dwa_local_planner
• eband_local_planner
• teb_local_planner

Мы будем пользоваться локальным планировщиком dwa_local_planner.

Создадим в нашем пакете abot_navigation в папке launch файл запуска ноды move_base. Назовём его move_base.launch.

<launch>
	<!-- Arguments -->
	<arg name="cmd_vel_topic" default="/mobile_abot/cmd_vel" />
	<arg name="base_global_planner" default="navfn/NavfnROS"/>
	<arg name="base_local_planner" default="dwa_local_planner/DWAPlannerROS"/>
	<!-- move_base -->
	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
		<param name="base_global_planner" value="$(arg base_global_planner)"/>
		<param name="base_local_planner" value="$(arg base_local_planner)"/>  

		<rosparam file="$(find abot_navigation)/config/planner.yaml" command="load"/>
		<rosparam file="$(find abot_navigation)/config/costmap_common.yaml" command="load" ns="global_costmap" />
		<rosparam file="$(find abot_navigation)/config/costmap_common.yaml" command="load" ns="local_costmap" />
		<rosparam file="$(find abot_navigation)/config/costmap_local.yaml" command="load" ns="local_costmap" />
		<rosparam file="$(find abot_navigation)/config/costmap_global.yaml" command="load" ns="global_costmap" />
		
		<remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
	</node>
</launch>

Здесь в качестве параметров мы указываем выбранный нами локальный и глобальный планировщик — navfn/NavfnROS и dwa_local_planner/DWAPlannerROS. Ещё нам надо указать топик, куда именно будут отправляться значения скоростей робота — /mobile_abot/cmd_vel.

Также мы «скормили» ноде move_base кучу файлов с параметрами. Что это за параметры?

При расчёте пути движения в ROS используются так называемые «карты затрат» (costmap), которые накладываются поверх карты занятости (occupancy grid). Карты затрат использутся для определения степени влияния (inflation) препятствий, встреченных на пути робота, на сам маршрут движения. С помощью карты затрат мы присваиваем каждому блоку на карте занятости (occupancy grid) свою «цену» в дапазоне [0, 255].

• Блоки, которые робот ну никак не заденет, являются свободными (Freespace), имеют нулевую цену, и на них можно не обращать внимание при движении.
• Блоки, которые вероятно могут помешать движению робота по маршруту (Possibly inscribed), имеют ненулевую цену, и за ними «нужно следить» по мере движения и роста их цены.
• Блоки, которые робот точно заденет, если продолжит движение по текущей траектории (inscriberd). Цена таких блоков ещё выше, чем у Possibly inscribed.
• «Летальные» блоки (Lethal) с максимальной ценой в 255. Если такой блок оказался в радиусе робота — значит, произошло столкновение и робот застрял (In collision). В этом случае он должен вызвать протокол восстановления (Recovery behavior).

Более подробно узнать о картах затрат можно в документации ROS.

Каждому планировщику пути требуется собственная карта затрат. Локальному планировщику — локальная карта затрат, глобальному — глобальная.

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

Локальная карта затрат формируется из данных с датчиков робота.

В папке config создадим четыре файла параметров:

• costmap_global.yaml — параметры глобальной карты затрат.
• costmap_local.yaml — параметры локальной карты затрат.
• costmap_common.yaml — общие параметры как для глобальной, так и для локальной карты затрат.
• planner.yaml — конкретные параметры расчёта траектории движении робота для глобального и локального планировщиков пути.

Это важно! Настройка всех этих параметров — долгая и кропотливая работа. Заранее предупреждаем вас, что не всё получится настроить с первого раза, а может быть и с сотого. Параметров очень и очень много, и мы не будем рассказывать, как и почему мы выбрали то или иное значение. Запаситесь терпением, изуйчайте документацию, пробуйте и подбирайте значения параметров, пока ваш робот не начнёт ездить самостоятельно.

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

• Basic Navigation Tuning Guide — гид ROS по настройке навигации.
• ROS Navigation Tuning Guide — крайне полезный гид от Кайю Чжэна о влиянии того или иного параметра при настройке планировщика пути.

Наш файл costmap_common.yaml выглядит так:

robot_radius: 0.1
robot_base_frame: base_footprint
resolution: 0.025
obstacle_range: 6.5
raytrace_range: 7.0

#layer definitions
static:
	map_topic: /map

obstacles:
	observation_sources: abot_lidar

	abot_lidar:
		data_type: LaserScan
		clearing: true
		marking: true
		topic: scan
		inf_is_valid: true

inflation:
	inflation_radius: 1.0

Здесь важно указать максимальное расстояние действия лидара raytrace_range (в метрах) и расстояние определения препятствий obstacle_range (в метрах). Лучше задать obstacle_range чуть меньше, чем raytrace_range. Попутно указываем:

• robot_radius — приблизительный радиус робота.
• resolution — разрешение карты (размер одного блока карты занятости в метрах).
• map_topic — топик, в котором хранится наша карта на сервере ROS.
• observation_sources — места, откуда брать информацию о препятствиях. Пока у нас только один лидар.
• inflation_radius — расстояние в метрах, на которое (условно) роботу нужно держаться подальше от преграды.

Заметьте, что все параметры в этом файле мы разбили на три условных слоя static, obstacles и inflation. Параметры разных слоёв используются в разных картах затрат.

Так выглядит файл costmap_global.yaml:

global_frame: map

rolling_window: false
track_unknown_space: true
static_map: true

update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5

cost_scaling_factor: 10.0

plugins:
	- {name: static,                  type: "costmap_2d::StaticLayer"}
	- {name: inflation,               type: "costmap_2d::InflationLayer"}

Так выглядит файл costmap_local.yaml:

global_frame: odom
rolling_window: true

update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5

width: 3.0
height: 3.0

cost_scaling_factor: 1.0

plugins:
	- {name: obstacles,                 type: "costmap_2d::ObstacleLayer"}
	- {name: inflation,                 type: "costmap_2d::InflationLayer"}

Локальную карту затрат будем строить вокруг робота на расстоянии 1,5 метров. То есть наш робот будет находиться в центре локальной карты затрат, а сама карта будет размером 3×3 метра.

Так выглядят настройки планировщиков траектории движения planner.yaml:

# Move base
controller_frequency: 10.0 
controller_patience: 1.0 

planner_frequency: 10.0
planner_patience: 5.0

oscillation_timeout: 0.0
oscillation_distance: 0.2

# Local planner
DWAPlannerROS:
	# Robot configuration parameters DWA
	acc_lim_x: 1.8 
	acc_lim_y: 0.0
	acc_lim_theta: 10.0

	max_vel_trans: 1.0
	min_vel_trans: 0.2

	max_vel_x: 0.6
	min_vel_x: -0.6

	max_vel_y: 0.0
	min_vel_y: 0.0

	max_vel_theta: 1.57
	min_vel_theta: 0.01

	# Robot configuration parameters Base local
	min_in_place_vel_theta: 0.01
	escape_vel: -0.2

	# Goal Tolerance Parameters
	xy_goal_tolerance: 0.1
	yaw_goal_tolerance: 0.3

	# Forward Simulation Parameters
	sim_time: 1.2
	vx_samples: 20
	vy_samples: 0
	vth_samples: 40
	# ! Forward Simulation Parameters Base local
	angular_sim_granularity: 0.17

	# Trajectory Scoring Parameters
	path_distance_bias: 32.0
	goal_distance_bias: 20.0
	occdist_scale: 0.02
	forward_point_distance: 0.325
	stop_time_buffer: 0.2
	scaling_speed: 0.25
	max_scaling_factor: 0.2

	# Oscillation Prevention Parameters
	oscillation_reset_dist: 0.05

В подавляющем большинстве случаев навигация робота начинает работать при правильных настройках симуляции скоростей. Для расчёта скоростей робота при объезде препятствий мы используем следующие значения:

• acc_lim_x — максимальное ускорение робота по оси Х: 1,8 м/с².
• acc_lim_y— максимальное ускорение робота по оси Y: 0 м/с², наше шасси не голономно.
• acc_lim_theta — максимальное ускорение робота вокруг оси Z: 10 рад/с².
• max_vel_trans — максимальная скорость робота при движении по глобальному пути: 1 м/с.
• min_vel_trans— минимальная скорость робота при движении по глобальному пути: 0,2 м/с.
• max_vel_x — максимальная скорость робота по оси Х: 0,6 м/с.
• min_vel_x — минимальная скорость робота по оси Х: 0 м/с. Если установить отрицательное значение, робот также сможет ездить «задом».
• max_vel_y — максимальная скорость робота по оси Y: 0 м/с, наше шасси не голономно.
• min_vel_y — минимальная скорость робота по оси Y: 0 м/с, наше шасси не голономно.
• max_vel_theta — максимальная скорость робота вокруг оси Z: 1,57 рад/с.
• min_vel_theta — минимальная скорость робота вокруг оси Z: 0,01 рад/с.

Точность достижения цели на карте:

• xy_goal_tolerance — допускаем линейную ошибку в достижении роботом цели в 0,1 м.
• yaw_goal_tolerance — допускаем угловую ошибку в достижении роботом цели в 0,3 рад.

Добавляем новый файл запуска ноды move_base в общий файл запуска навигации робота abot_navigation.launch:

<launch>
	<arg name="map_file" default="$(find abot_slam)/maps/map.yaml" />
	<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)" />
	<include file="$(find abot_navigation)/launch/amcl.launch" />
	<include file="$(find abot_navigation)/launch/move_base.launch" />
</launch>

Тестируем автономную навигацию

Пришло время протестить автономную навигацию робота!

Убедимся, что на настольном компьютере запущено ядро roscore. На Raspberry Pi запустим главный файл запуска робота bringup.launch:

cd ~/ros
su root
source devel/setup.bash
roslaunch abot_description bringup.launch

На настольном компьютере запустим файл запуска визуализации навигации display_navigation.launch:

cd ~/ros
source devel/setup.bash
roslaunch abot_description display_navigation.launch

Сейчас граф наших ROS-нод и топиков выглядит так:

В rviz значение глобального кадра «Global Options → Fixed Frame» должно быть установлено в map. Ещё нужно добавить все визуализации, сгенерированные нодой /move_base. В rviz на панели Displays нажмём *«Add → By topic» и по очереди выберем визуализации:

• /move_base/global_costmap/costmap — глобальная карта затрат.
• /move_base/local_costmap/costmap — локальная карта затрат.
• /move_base/local_costmap/footprint — габаритный размер робота, который используется в расчёте навигации.
• /move_base/NavfnROS/plan — глобальная траектория движения робота, сгенерированная глобальным планировщиком.
• /move_base/DWAPlannerROS/global_plan — глобальная траектория движения робота, скорректированная локальным планировщиком.
• /move_base/DWAPlannerROS/local_plan — локальная траектория движения робота. Визуализация симуляции скоростей робота при непосредственном объезде препятствий.
• /move_base_simple/goal — конечная точка маршрута. Отображается в виде вектора.

Далее задаём начальную точку локализации робота на карте кнопкой 2D Pose Estimate на панели в rviz.

Целевую точку маршрута можно задать кнопкой 2D Nav Goal на панели rviz. Целевая точка указывается как вектор. Направление вектора показывает, какую ориентацию должен иметь робот в целевой точке, куда он должен «смотреть». Учтите, что необязательно каждый раз вручную задавать целевые точки на карте своему роботу. Он может работать по алгоритму, при котором он сам будет выбирать для себя целевые точки и ставить себе задачи навигации.

Мы можем смело указать целевую точку на карте вручную и робот начнет самостоятельное движение к этой точке обьезжая все препятствия.

Мы решили начать с двух тестов, в которые мы для эксперимента включили данные с сенсоров в глобальную карту затрат. Первый тест нашего робота — самостоятельно добраться от рабочего места в середине нашего помещения в коридор. Кроме того, мы будем ставить преграды на пути движения робота, мешать ему и смотреть, как меняется его маршрут. Второй тест нашего робота — добраться из коридора к рабочему месту, но в этот раз мешать ему мы уже не будем.

Посмотрим, как наш робот справляется с автономной навигацией!

Еще пара тестов! На этот раз глобальная карта затрат статическая а данные с лидара используются только в локальной карте затрат. В этот раз мы несколько раз последовательно поездим из одного конца помещения в другой и вернемся на стартовую позицию. Еще в этот раз мы установим значение параметра min_vel_x отличным от нуля, таким образом заставив нашего робота двигаться спиной и разворачиваться. На видео можно видеть как робот самостоятельно расчитывает и выполняет развороты в узких коридорах между стелажей!

Наш робот ездит самостоятельно! Да, он ещё не может похвастаться идеальными манёврами, но это отличное начало для более точной настройки и корректировки параметров навигации.

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

Перед заключением ответим на некоторые ключевые вопросы, которые могли у вас возникнуть.

• Почему все ROS-ноды на Raspberry Pi запускаются от root?

Прежде всего, для удобства все наши ноды на Raspberry Pi запускаются единым файлом *.launch. Любой *.launch-файл можно запустить от root, а можно и от другого пользователя. Но запустить одним *.launch-файлом несколько нод под разными пользователями одновременно нельзя. Почему под root? Дело в том, что мы управляем моторами, используя аппаратный ШИМ через устройство операционной системы /dev/gpiomem. Если к /dev/mem можно получить доступ, используя usermod и chmod, то получить доступ к /dev/gpiomem без root во время работающих аппаратных таймеров довольно тяжело. В официальной операционной системе Raspberry Pi OS доступ к /dev/gpiomem открыт для всех пользователей. В Ubuntu, которой мы воспользовались, нужно вмешательство в ядро. Мы решили этого не делать и просто запускать весь *.launch-файл от root. На самом же деле единственная нода, которая у нас действительно нуждается в root — это dc_motors.

• Зачем мы используем два компьютера и почему мы запускаем ядро roscore на настольном компьютере?

Ядро roscore можно запустить там, где вы хотите: на настольном компьютере или на Raspberry. Существенно производительность от этого не пострадает. А в некоторых случаях это будет даже полезно. Например, если настольный компьютер «отвалится» от сети, а ядро roscore будет запущено на Raspberry, то дистанционное управление с геймпада продолжит свою работу. Вторую машину с ROS мы используем исключительно из-за пакетов навигации gmapping, amcl, и move_base. Они требуют высокую вычислительную мощность компьютера. По факту эти пакеты можно запустить и на Raspberry Pi. Однако при этом нужно существенно увеличить значение параметра map_update_interval в SLAM и параметров типа transform_tolerance в AMCL, а также уменьшить частоту обновления update_frequency в планировщиках пути. Пакеты будут работать на Raspberry, но скорость и качество построения карт и навигаиции будут заметно ниже.

• Как собрать только опредлённые пакеты из проекта?

Что делать, если вы скачали весь исходный код нашего проекта abot c GitHub, но хотите повторить этот проект поэтапно или использовать другое железо? Сборка всего проекта с помощью catkin_make может вам помешать. Просто создайте новое рабочее пространство ROS и скопируйте в него нужные вам пакеты, а затем уже соберите их через catkin_make. Или же используйте команду catkin_make --only-pkg-with-deps для сборки лишь определённых пакетов из проекта.

Заключение

Данная статья подошла к концу, но она лишь первая в нашей серии гайдов по системе ROS и робототехнике.

Мы разобрались с основами инфраструктуры ROS, построили и запрограммировали нашего первого робота, который способен решать базовые задачи автономной навигации.

Мы обязательно продолжим работу над нашим ABot’ом, будем совершенствовать его и добавлять ему новые функции. В следующих статьях мы:

• Добавим роботу восемь инфракрасных дальномеров (бамперов). Введём новый слой сенсоров для нашего проекта и покажем, как работать с простыми датчиками расстояния в ROS.
• Добавим голосовое управление нашему роботу. Команды для навигации будут отдаваться не вручную, а вербально.
• Рассмотрим возможность создания зарядной докстанции.

До встречи!

Читать далее >

В начало проекта >