ROS

1.

Управление и моделирование
беспилотных систем и роботов в
Robot Operation System
Программа профессиональной переподготовки
Квалификация: «Программист систем управления
беспилотных систем и роботов»
УдГУ, 2024

2.

Преподаватели
1. Клековкин Антон Владимирович, к.т.н., доцент кафедры «Мехатронные
системы» ИжГТУ имени М.Т. Калашникова, научный сотрудник УдГУ,
руководитель программы.
1. Звонарев Иван Сергеевич, аспирант 4 года обучения кафедры «Мехатронные
системы» ИжГТУ имени М.Т. Калашникова.
1. Дерюшев Павел Николаевич, аспирант 1 года обучения кафедры
«Мехатронные системы» ИжГТУ имени М.Т. Калашникова , инженерконструктор 3-й категории АО «Аксион-Холдинг».
1. Банников Александр Андреевич, аспирант 1 года обучения кафедры
«Мехатронные системы» ИжГТУ имени М.Т. Калашникова, инженерконструктор 3-й категории АО «Аксион-Холдинг».

3.

План занятий
1. Установка Robot Operation System (ROS). Знакомство с основными понятиями
ROS: нода, мастер-нода, пакет, сообщения, топик, сервис, модель издательподписчик.
2. Создание пакета в ROS. Реализация передачи информации между нодами
используя топики и модель издатель-подписчик. Знакомство с launch-файлами,
создание своих launch-файлов. Работа с утилитой rqt.
3. Описание кинематики роботов в ROS в формате URDF. Основные элементы
URDF: звенья (links) и сочленения (joints). XACRO-файлы.
4. Создание URDF-модели мобильного робота с дифференциальным приводом.
Встроенный визуализатор rviz.
5. Знакомство с симулятором реального мира Gazebo в ROS. Основные ноды и
топики. Генерация модели мобильного робота в среде Gazebo. Создание
собственных пакетов для управления мобильным роботом в среде Gazebo.
Введение в cmake.
6. Разработка программы управления мобильным роботом в среде Gazebo для
задания движения робота с клавиатуры.
7. Знакомство с датчиками, представленными в среде Gazebo. Добавление
датчиков на мобильный робот. Получение данных с датчиков: лазерный
дальномер (лидар) и камера. Визуализация полученных данных с датчиков в
rviz. Визуализация полученных данных в виде графиков, используя утилиты
rqt_plot и rqt_multiplot.

4.

План занятий
8. Работа в ROS и Gazebo с БАС мультироторного типа.
8. Разработка программы управления мобильным роботом для движения в
заданную точку. Движение в заданную точку с объездом (облетом)
препятствий. Алгоритм тангенциального избегания.
8. Разработка программы управления мобильным роботом для движения вдоль
заданной траектории без обратной связи.
8. Разработка программы управления мобильным роботом для движения вдоль
заданной траектории с обратной связью. ПИД-регулятор. Регулятор,
полученный на основе метода функций Ляпунова.
8. Разработка программы управления мобильным роботом с дифференциальным
приводом для движения вдоль заданной траектории с обратной связью,
используя ускорения в качестве управляющих сигналов.
8. Работа с пакетами навигации gmapping, move_base, amcl.

5.

Программа итоговой аттестации
• Итоговая аттестация обучающихся проходит в формате защиты проекта, в ходе
которого обучающиеся используют и демонстрируют цифровые компетенции,
предусмотренные в процессе освоения настоящей программы.
• Защита проекта сопровождается презентацией и докладом об основных этапах
реализации проекта, а также демонстрацией проектного решения.
• Примеры тем заданий на итоговую аттестацию:
• Разработка пакета в ROS для управления мобильным роботом с
дифференциальным приводом в среде моделирования Gazebo.
• Разработка пакета в ROS для управления мобильным роботом с
омниколесами в среде моделирования Gazebo.
• Разработка пакета в ROS для управления беспилотным летательным
аппаратом в среде моделирования Gazebo.

6.

Введение
1. Robot Operating System (ROS) – это гибкая платформа (фреймворк) для
разработки программного обеспечения роботов. Это набор разнообразных
инструментов, библиотек и определенных правил, целью которых является
упрощение задач разработки ПО роботов.
1. Важные моменты для разработки под ROS: активность сообщества, наличие
различных библиотек, расширяемость и простота использования.
1. Важные моменты ROS:
• Повторное использование программных модулей. Разработанная
программа легко запускается и используется в любом другом приложении.
• Готовый протокол коммуникации. Любой программных модуль может
быть представлен как отдельный процесс, взаимодействующий с другими
процессами по сетевому протоколу.
• Распределенные процессы. Структура ROS создана в виде минимальных
единиц исполняемых процессов (нод), и каждый процесс выполняется
изолированно. Взаимодействие разных нод происходит только на уровне
обмена сообщениями.
• Развитость средств разработки и отладки. ROS предоставляет готовые
инструменты для отладки, инструмент 2D-визуализации (rqt), и
инструмент 3D-визуализации (RViz), инструмент 3D-симуляции (Gazebo).

7.

ROS и ROS2

8.

ROS и ROS2
• ROS1 – версия Noetic.
Инструкция по установке и документация:
http://wiki.ros.org/noetic/Installation/Ubuntu
• ROS2 – версия Jazzy.
Инструкция по установке и документация:
http://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html

9.

Языки программирования в ROS
Основные языки программирования:
• C++;
• Python;
• Lisp;
Неосновные языки программирования:
• Go;
• .Net/C#;
• Haskell;
• Java;
• Javascript / Node.js;
• Julia;
• Pharo;
• R;
• Ruby;
• Rust;
• Matlab/Simulink;

10.

Языки программирования в ROS
Базовые знания в программировании:
• Типы данных;
• Операции над числами;
• Операторы сравнения чисел;
• Массивы, динамические массивы;
• Ветвление программы. if – else if – else;
• Логические операторы;
• Циклы. For и while;
• Функции. Определение, вызов, возврат, аргументы функции.
• Локальные и глобальные переменные.
• Математические операции.
Продвинутые знания в программировании:
• ООП.
• Классы. Объект класса. Конструктор класса.
• Поля и методы класса. Модификаторы доступа.

11.

Основные термины
1. Нода (Node)
Понятие ноды, относится к наименьшей "рабочей" единицы используемой в ROS.
Можно провести аналогию с одной исполняемой программой. При запуске нода
регистрирует информацию о себе на мастере. Зарегистрированная нода может
взаимодействовать с другими нодами (получать и отправлять запросы).
2. Мастер (Master), Мастер-Нода.
Мастер выполняет роль сервера имен для возможности подключения между
собой различных нод. Команда roscore запускает сервер мастера. Обмен
сообщениями между нодами невозможна без запущенного мастера.
3. Пакет (Package)
Пакет является основной единицей ROS. Любое приложение ROS оформляется в
пакет, в котором определяются: конфигурация пакета, ноды необходимые для
работы пакета, зависимости от других пакетов ROS.
Слабуха Николай, Введение в ROS, компания "Братья Вольт“
http://docs.voltbro.ru/starting-ros/

12.

Основные термины
4. Сообщение (Message)
Ноды отправляют и принимают данные между собой, согласно заданного
формата. Эти данные называют Сообщения, а описание Типом Сообщения.
Сообщения могут быть как простых типов (integer, float, boolean), так и могут
состоять из сложных структур, содержащих вложенные сообщения и массивы
сообщений).
Например, для сообщения с координатами обьекта (XYZ) можно использовать
тип сообщения geometry_msgs/Vector3.msg который описывается:
float64 x
float64 y
float64 z
Еще пример – geometry_msgs/Twist.msg
Vector3 linear
Vector3 angular

13.

Основные термины
5. Топик (Topic)
Топик – это один из видов обмена сообщениями. Нода издателя (publisher)
сначала регистрирует свою тему на мастере, а затем начинает публикацию
сообщений в эту тему (топик). Узлы подписчиков, которые хотят получать
информацию из этой темы (топика) при помощи мастера получают адрес этой
темы и далее получают сообщения из этого топика.
• Издатель (Publisher)
Издателем называется процесс, который рассылает сообщения в рамках
созданного топика для других нод. Одна нода может содержать
несколько издателей, публикующих данные в разные топики.
• Подписчик (Subscriber)
Подписчиком называется процесс, который получает сообщения из
определенного топика. Подписчик регистрируется на Мастере, указывая
какие топики хочет получать. После этого Издатель начинает отправлять
сообщения подписчику. Связь с топиком для подписчика является
асинхронной (издатель публикует сообщения в не зависимости от статуса
подписчиков).
Этот тип взаимодействия удобно применять для работы с датчиками,
которые непрерывно передают полученные значения.

14.

Установка "Guest Additions" (Дополнения гостевой системы)
Эти дополнения обеспечивают улучшенную интеграцию между
хостовой и гостевой операционными системами.
1. Запустите вашу виртуальную машину (гостевую ОС).
2. В верхнем меню VirtualBox выберите "Устройства" (Devices) →
"Подключить образ диска с дополнениями гостевой ОС" (Insert
Guest Additions CD image).
3. Внутри гостевой системы откроется окно с предложением
установить дополнения. Если нет, откройте CD-диск вручную и
выполните установочный файл:
○ Для Windows — запустите VBoxWindowsAdditions.exe.
○ Для Linux — выполните команду для монтирования диска и
установки дополнений (обычно sudo sh
/media/cdrom/VBoxLinuxAdditions.run).
4. После установки перезагрузите гостевую ОС.

15.

Командная строка
Основные команды командной строки:
• Ctrl + Alt + T – открытие терминала / открытие новой вкладки.
• Tab – автодополнение команды, если доступен один вариант
продолжения.
• Tab-Tab (двойное нажатие) – показ всех вариантов продолжения команды,
если их несколько.
• Стрелка вверх/вниз – предыдущая/следующая команда в истории.
• Ctrl + C – остановить выполнение программы (команды).
• Ctrl + Shift + C – копировать из консоли.
• Ctrl + Shift + V – вставить в консоль.
• help – вывод краткой информации о команде.
• ls – вывод списка файлов в текущей директории (от list) .
• cd – сменить директорию (change directory).
• cd .. – перейти на уровень вверх
• mkdir – создать директорию (make directory).
• nano – консольный текстовый редактор.
• gedit – текстовый редактор.
• apt (или apt-get) – менеджер пакетов.
• sudo apt install имя_пакета – установить пакет через менеджер apt.
• sudo – запуск команд и приложений от имени администратора.
• chmod - изменение прав доступа к файлам и папкам (change mode)

16.

Установка Visual Studio Code
1. Обновите список пакетов:
2.
3.
sudo apt update
Установите необходимые зависимости:
sudo apt install -y software-properties-common apt-transport-https wget
Добавьте ключ GPG и репозиторий Microsoft для VS Code:
wget -qO- https://packages.microsoft.com/keys/microsoft.asc | gpg -dearmor > packages.microsoft.gpg
sudo install -o root -g root -m 644 packages.microsoft.gpg
/usr/share/keyrings/
sudo sh -c 'echo "deb [arch=amd64 signedby=/usr/share/keyrings/packages.microsoft.gpg]
https://packages.microsoft.com/repos/code stable main" >
/etc/apt/sources.list.d/vscode.list'
1. Установите Visual Studio Code:
sudo apt update
sudo apt install code

17.

Установка Python
1. Проверьте, установлен ли Python:
python3 --version
1. Если Python не установлен, установите его:
sudo apt install python3
1. Установите пакет pip для Python:
sudo apt install python3-pip

18.

Настройка VS Code для работы с Python
1. Запустите Visual Studio Code:
code
2. Установите расширение "Python" от Microsoft:
Перейдите в панель расширений (значок квадратов слева или Ctrl+Shift+X).
○ В строке поиска введите "Python" и установите расширение от Microsoft.
3. Настройка интерпретатора Python:
○
Нажмите Ctrl+Shift+P, чтобы открыть командную палитру.
○
Введите Python: Select Interpreter и выберите установленный
интерпретатор Python (например, /usr/bin/python3).
4. Установка необходимых пакетов для Python-проектов:
○
В терминале VS Code можно использовать pip для установки библиотек,
например:
○
pip3 install numpy

19.

Начало работы
Настройка рабочего пространства
1.
Создайте рабочую директорию для ROS: Обычно создают директорию
catkin_ws в домашней папке:
mkdir -p ~/catkin_ws/src
1.
Перейдите в созданную директорию и инициализируйте рабочую
область (workspace):
cd ~/catkin_ws
catkin_make
Это создаст структуру файлов для вашего рабочего пространства ROS.

20.

Начало работы
Автоматическое применение окружения при открытии консоли:
1. Откройте ваш .bashrc файл в текстовом редакторе:
nano ~/.bashrc
1. Добавьте строку с командой source в конец файла. Например:
source /opt/ros/noetic/setup.bash
source ~/catkin_ws/devel/setup.bash
1. Сохраните изменения и закройте редактор. В nano, например, для
сохранения нажмите Ctrl + O, затем Enter, для выхода нажмите
Ctrl + X.
Примените изменения:
source ~/.bashrc

21.

Начало работы
Создание пакета
• Любой написанный и запущенный код в рамках ROS должен принадлежать
конкретному пакету.
• Для создания пакета:
• Переходим в папку ~/catkin_ws/src:
$ cd ~/catkin_ws/src
• Команда для создания пакета - catkin_create_pkg
catkin_create_pkg my_package std_msgs rospy roscpp
где my_package – имя пакета (придумываем сами);
std_msgs, rospy, roscpp – зависимости от других пакетов;

22.

Начало работы
Создание издателя на python:
• Создаем файл my_publisher.py в папке src нашего пакета
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
pub = rospy.Publisher('hello', String, queue_size=10)
rospy.init_node('hello_topic_publisher')
rospy.loginfo("Hello from PUB node")
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pub.publish("Hello World")
r.sleep()
• Делаем файл исполняемым:
$ chmod a+x src/my_publisher.py
• Запускаем (не забываем запускать мастер-ноду – roscore):
$ python3 /src/my_publisher.py

23.

Начало работы
Создание подписчика на python:
• Создаем файл my_subscriber.py в папке src нашего пакета
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
def msg_callback(msg):
rospy.loginfo("received msg = %s", msg.data)
rospy.init_node('topic_subscriber_node')
rospy.loginfo("Hello from SUB node")
rospy.Subscriber('hello', String, msg_callback)
rospy.spin()
• Делаем файл исполняемым:
$ chmod a+x src/my_subscriber.py
• Запускаем (не забываем запускать мастер-ноду – roscore):
$ python3 /src/my_subscriber.py

24.

Начало работы

25.

Начало работы
Команда rqt_graph.

26.

Начало работы
Launch файлы.
• Для одновременного запуска файлов и конфигурации служит утилита roslaunch.
Используя roslaunch возможно дополнительно настраивать исполняемые
файлы в момент их запуска (передавать параметры, изменять имена и тп).
• Создаем файл ./launch/demo.launch
<launch>
<node pkg="test_package" type="my_publisher.py"
name="topic_publisher1"/>
<node pkg="test_package" type="my_subscriber.py"
name="topic_subscriber1"/>
</launch>
pkg – имя пакета, type – имя запускаемого файла, name – имя ноды (игнорируется
имя в файле); Можно запустить и файл и лаунч-файл, будет много нод.
Запускаем:
$ roslaunch test_package demo.launch --screen

27.

Начало работы
Launch файлы:
• Можно использовать параметры, которые передаются в ноду.
• Правим файл ./launch/demo.launch
<launch>
<param name="slp" value="2", type="int“/>
<node pkg="test_package" type="topic_publisher.py"
name="topic_publisher1"/>
<node pkg="test_package" type="topic_subscriber"
name="topic_subscriber1"/>
</launch>
• Для более подробного изучения формата launch-файлов есть статья:
Roslaunch tips for large projects
http://wiki.ros.org/ROS/Tutorials/Roslaunch%20tips%20for%20larger%20projects

28.

Издатель на с++.
#include "ros/ros.h
#include "std_msgs/String.h"
int main(int argc, char **argv){
std::vector<std::string> vec;
int par = 1, count = 1;
ros::init(argc, argv, "cpp_publisher_node");
ros::NodeHandle nh;
ros::Publisher pub1 = nh.advertise<std_msgs::String>(“topic1", 1000);
ros::Rate loop_rate(1);
nh.getParamNames(vec);
nh.getParam("slp", par);
while(ros::ok()) {
std_msgs::String msg1;
std::stringstream ss;
ss << count;
msg1.data = ss.str();
ROS_INFO("I'm publisher: %s", msg1.data.c_str());
pub1.publish(msg1);
ros::spinOnce();
ros::Duration(par).sleep();
count++;
}
return 0;
}

29.

Издатель на python.
Эквивалентный код для ROS на Python:
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
def main():
vec = []
par = 1
count = 1
rospy.init_node('python_publisher_node')
pub1 = rospy.Publisher('topic1', String, queue_size=1000)
loop_rate = rospy.Rate(1)
rospy.get_param_names()
# Здесь мы просто вызываем метод для получения имен параметров
if rospy.has_param('slp'):
par = rospy.get_param('slp')
while not rospy.is_shutdown():
msg1 = String()
msg1.data = str(count)
rospy.loginfo("I'm publisher: %s", msg1.data)
pub1.publish(msg1)
rospy.sleep(par)
count += 1
if __name__ == '__main__':
main()

30.

Пример работы с log.

31.

Начало работы
Работа с имеющимися нодами:
• Запускаем ноду симуляции turtlesim:
$ rosrun turtlesim turtlesim_node
• $ rqt_graph
• Выполнить команды:
$ rosnode list
$ rostopic list
$ rostopic pub /turtle1/cmd_vel <tab> <tab>
(rostopic pub [topic] [msg_type])
$ rostopic pub [topic] –r 1 [msg_type]

32.

Начало работы
Работа с имеющимися нодами:
• Запускаем ноду управления черепашкой:
$ rosrun turtlesim turtle_teleop_key
• $ rqt_graph
• $ rqt_plot

33.

Мобильный робот. URDF-модель
• В ROS существует собственная спецификация описания кинематики и динамики
робота называемый URDF (Unified Robotic Description Format) и реализуется за счёт
файлов с расширением .urdf. Данный файл пишется с использованием формата
XML.
• В данной работе будет использована продвинутая версия URDF спецификации под
названием XACRO (XML macro), который обладает следующими основными
преимуществами:
• Встраивание одного файла в другой
• Инициализация параметров
• Математический вычисления
• Описание макросов

34.

Мобильный робот. URDF-модель
• Робот представляет из себя кинематическую цепочку состоящий из сочленений
(joint) и звеньев (link).
• Каждый из представленных компонентов кинематической цепи описывается
собственным тэгом.
Произвольная кинематическая цепь робота
В URDF могут быть описаны не только
звенья робота, но другие сущности
рабочей среды

35.

Мобильный робот. URDF-модель. Link
• Звено (Link) описывает физические или виртуальные объекты:
• Физические: звено робота, часть «мира», рабочий орган (end-effector) и др.
• Виртуальные: центральная точка инструмента (TCP), инерциалная система
отсчета (robot base frame), и др.
• Каждое звено имеет свою систему координат
• Каждое звено может иметь 3 блока: visual, collision, inertial.

36.

Мобильный робот. URDF-модель. Link. Состав.
• Visual – визуальная часть звена, служит только для отображения и не участвует
в просчете физики.
• Collision – часть звена, отвечающая за физику столкновений; представляет из
себя описание геометрии звена для определения границ столкновений.
• Зачастую визуальная составляющая и коллизия равны, но для оптимизации
вычислений, если 3D модель звена имеет избыточно плотную сетку, то её
сокращают и загружают как коллизию

37.

Мобильный робот. URDF-модель. Link. Состав.
• Inertial – задаётся масса звена, центр тяжести и тензор инерции для звена, без
данных характеристик невозможно запустить симуляцию с роботом, поэтому
они должны быть обязательно для каждого звена.

38.

Мобильный робот. URDF-модель. Joint
• Сочленение (Joint) соединяет 2 звена
• Определяет преобразование системы координат родительского звена к
системе координат дочернего звена.
• Задает возможные движения звеньев относительно друг друга: неподвижные,
свободные, линейное перемещение, вращение.
• Указывает подвижные оси
• Содержит ограничения на координаты и скорости (голономные и
неголономные связи)

39.

Мобильный робот. URDF-модель + движение
• Создаем новый пакет:
$ catkin_create_pkg m2wr_description urdf xacro
Создаем новую папку с именем urdf, копируем нужные файлы
Создаем новую папку с именем launch , копируем нужные файлы
Устанавливаем расширение URDF от smilerobotics (не обязательно)
Запускаем rviz: roslaunch m2wr_description rviz.launch
Запускаем gazebo:
• roslaunch gazebo_ros empty_world.launch
• roslaunch m2wr_description spawn.launch
• запуск ноды с движением
• Для проверки движения можно использовать стандартный пакет управления с
клавиатуры:
• Установка: sudo apt-get install ros-noetic-teleop-twist-keyboard
• Запуск: rosrun teleop_twist_keyboard teleop_twist_keyboard.py

40.

Мобильный робот. Запуск Gazebo. Lifehack
3
1
2
1. Заходим в настройки виртуальной машины
2. Выбираем вкладку Display
3. Снимаем галочку у пункта Accelerate 3D graphics

41.

Мобильный робот. RVIZ
• При работе с rviz в панели Displays внизу нажимаем кнопку Add, далее выбираем
RobotModel.
• В поле Fixed Frame выбираем base_link
• Чтобы каждый раз не добавлять модель робота можно сохранить конфигурацию:
File->Save Config As. Создаем папку config в пакете и сохраняем туда файл.
• При следующих запусках можно открывать этот файл через File->Open Config.
• Либо прописать файл конфига в launch файл rviz’а (путь до файла и имя файла свои):
• <node name="m2wr_rviz" pkg="rviz" type="rviz" args="-d
$(find m2wr_description)/config/m2wr.rviz"/>

42.

Мобильный робот. Движение
• При запуске симуляции можно выполнить команду: rostopic list. В списке
вывода данной команды должен присутствовать топик /cmd_vel. Топик /cmd_vel
служит для задания линейных и угловых скоростей движения мобильного робота
используя сообщение geometry_msgs/Twist.
• Для ручного запуска робота выполнить команду:
$ rostopic pub /cmd_vel <Tab> <Tab>
• По нажатию на Tab пропишется тип сообщения и далее параметры.
• Можно задать x-компоненту линейной скорости или z-компоненту угловой.
• Для создания исполняемых файлов, в которых будем задавать движение создаем
новый пакет:
$ cd ~/catkin_ws/src
$ catkin_create_pkg m2wr_twist roscpp rospy geometry_msgs
std_msgs visualization_msgs
• Для C++ - В пакете в папке src создаем .cpp файл с именем m2wr_twist.cpp (имя
может быть любым).
• Для python - В пакете в папке src создаем .py файл с именем m2wr_twist.py (имя
может быть любым). Делаем файл исполняемым:
cd ~/catkin_ws
$ chmod a+x src/m2wr_twist.py

43.

Мобильный робот. URDF-модель + движение
• Текст файла m2wr_twist.cpp:
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char **argv)
{
// Инициализация ноды с определённым именем для отладки
ros::init(argc, argv, "m2wr_twist");
ros::NodeHandle nh;
// Инициализация издателя в определённый топик cmd_vel
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 100);
// Задаём линейную и/или угловую скорость
geometry_msgs::Twist vel;
vel.angular.z = 1.0;
while (ros::ok())
{
pub.publish(vel);
ROS_INFO(“Vel: %f”, vel.angular.z);
ros::spinOnce();
ros::Duration(1).sleep();
}
}

44.

Мобильный робот. URDF-модель + движение
• Текст файла m2wr_twist.py:
#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
def main():
# Инициализация ноды с определённым именем для отладки
rospy.init_node('m2wr_twist')
# Инициализация издателя в определённый топик cmd_vel
pub = rospy.Publisher('cmd_vel', Twist, queue_size=100)
# Задаём линейную и/или угловую скорость
vel = Twist()
vel.angular.z = 1.0
# Цикл публикации сообщений
rate = rospy.Rate(1) # Частота 1 Hz
while not rospy.is_shutdown():
pub.publish(vel)
rospy.loginfo("Vel: %f", vel.angular.z)
rate.sleep()
if __name__ == '__main__':
main()
)

45.

Мобильный робот. URDF-модель + движение
• Редактируем файл CMakeLists.txt данного пакета для компиляции, добавляем
следующие строки (можно взять шаблон из тестового пакета):
add_executable(m2wr_twist src/m2wr_twist.cpp)
add_dependencies(m2wr_twist ${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS})
target_link_libraries(m2wr_twist ${catkin_LIBRARIES})
• Сохраняем, компилируем командой $ catkin_make.
• После каждого изменения необходимо сохранить файл и откомпилировать.
• Запускаем газебо, добавляем туда модель, запускаем нашу программу – робот
движется.
• Если завершить выполнение программы (сочетание Ctrl+c), то робот не
останавливается, а продолжает выполнять последнюю полученную команду. В ROS
можно подписаться на событие перед завершением программы и выполнять там
какие-либо действия, например, давать роботу команду остановиться.

46.

Мобильный робот. Движение. Завершение программы.
• Если завершить выполнение программы (сочетание Ctrl+c), то робот не
останавливается, а продолжает выполнять последнюю полученную команду. В ROS
можно подписаться на событие перед завершением программы и выполнять там
какие-либо действия, например, давать роботу команду остановиться.
• Для этого нужно подключить библиотеку signal:
#include <signal.h>
• В функции main до бесконечного цикла подписаться на событие:
signal(SIGINT, mySigintHandler);
здесь SIGINT – имя события (его не трогаем),
mySigintHandler – имя функции, в которую будет заходить программа перед
завершением.
В программе определить функцию-обработчик события и прописать в ней необходимые
действия, например, остановку робота:
void mySigintHandler(int sig)
{
StopRobot();
// эту функцию нужно прописать
ROS_INFO(«Sigint Event");
ros::shutdown();
// это обязательно
}
Возможно после цикла while нужно вставить следующую строку (без нее тоже должно
работать):
mySigintHandler(1);

47.

Мобильный робот. Движение. Завершение программы.
Получении сигнала о завершении работы ноды на Python:
● Регистрируем вызов (callback), отвечающий за действия при завершении работы
def shutdown_callback():
rospy.loginfo("Shutting down the node. Stopping the robot.")
# Остановить робота при завершении работы ноды
stop_robot() # функцию stop_robot() необходимо реализовать
● В основном теле программы регистрируем callback:
def main():
rospy.init_node('m2wr_twist')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=100)
# Регистрация обратного вызова при завершении работы ноды
rospy.on_shutdown(shutdown_callback)

48.

Мобильный робот. Движение. Управление с клавиатуры.
• Для управление с клавиатуры можно использовать консоль. Для получения данных,
введенных в консоль, можно использовать функцию ch = input().strip(). Данная
функция возвращает очередной символ, введенный в консоль.
• Пример:
def main():
global pub
rospy.init_node('m2wr_twist')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=100)
while not rospy.is_shutdown():
ch = input("Enter command (w/s/a/d/x): ").strip()
if ch == 'w':
MoveRobot(1, 0)
#необходимо реализовать функцию MoveRobot
elif ch == 's':
StopRobot()
#необходимо реализовать функцию StopRobot
elif ch == 'a':
MoveRobot(0, 1) # Поворот влево
elif ch == 'd':
MoveRobot(0, -1) # Поворот вправо
elif ch == 'x':
MoveRobot(-1, 0) # Движение назад
else:
print("Invalid command! Use w/s/a/d/x to control the robot.")
rospy.spinOnce()

49.

Автономное управление. Основные концепции
Траектория
Система управления
Система оценки
положения

50.

Автономное управление. Траектория
Аналитическое представление
1)
2)
Массив точек

51.

Примеры траекторий

52.

Системы оценки положения
-
-
Одометрия (измерение и
интегрирование скорости
колес)
Лидар
GPS
Радио/УЗ маяки(InDoor GPS)
Камеры (Система тех.
зрения)
Интегрирование
показателей
инерционных датчиков
(Гироскоп + Акселерометр)

53.

Схема робота. Кинематические соотношения.

54.

55.

Движение к заданной точке.

56.

Мобильный робот. Обратная связь.
• При работе Gazebo создается топик /odom, в который публикуются данные о
положении и ориентации мобильного робота.
• Тип сообщения - nav_msgs/Odometry
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
• Тип сообщения geometry_msgs/PoseWithCovariance
geometry_msgs/Pose pose
float64[36] covariance
• Тип сообщения geometry_msgs/Pose
geometry_msgs/Point position //(x,y,z)
geometry_msgs/Quaternion orientation //(x,y,z,w)
• Тип сообщения geometry_msgs/TwistWithCovariance
Twist twist
float64[36] covariance

57.

Мобильный робот. Обратная связь. С++
• Подписываемся на топик /odom:
ros::Subscriber subOdom = nh.subscribe("odom", 100, msgCallbackOdom);
где msgCallbackOdom – имя функции, в которую заходит программа при получении
новых данных из топика.
• Вид функции msgCallbackOdom:
void msgCallbackOdom(const nav_msgs::Odometry::ConstPtr &msg)
{
xReal = msg->pose.pose.position.x;
yReal = msg->pose.pose.position.y;
linVelReal = msg->twist.twist.linear.x;
angVelReal = msg->twist.twist.angular.z;
}
• Кватернион
[w, x, y, z] = [cos(alpha/2), sin(alpha/2)*vx, sin(alpha/2)*vy, sin(alpha/2)*vz]
Где alpha — это угол вращения, а vx, vy, vz — вектор оси вращения.
• Тогда угол поворота равен:
angle = asin(msg->pose.pose.orientation.z)*2*180/PI;
angle = acos(msg->pose.pose.orientation.w)*2*180/PI; // либо так
//но нужно разобраться со знаками (можно использовать atan2(z,w) ).

58.

Мобильный робот. Обратная связь. Python
• Импортируем тип данных Odometry:
from nav_msgs.msg import Odometry
• Подписываемся на топик /odom:
rospy.Subscriber('/odom', Odometry, odom_callback)
где odom_callback – имя функции, в которую заходит программа при получении
новых данных из топика.
• Вид функции odom_callback :
def odom_callback(msg):
robot_pose = msg
robot_X = msg.pose.pose.position.x
robot_Y = msg.pose.pose.position.y
robot_yaw,robot_pitch,robot_roll=
euler_from_quaternion(msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
[w, x, y, z] = [cos(alpha/2), sin(alpha/2)*vx, sin(alpha/2)*vy, sin(alpha/2)*vz]
robot_obj.alpha = robot_yaw
alpha — это угол вращения, а vx, vy, vz — вектор оси вращения.
•Где
Кватернион

59.

Преобразование кватерниона в углы эйлера
def euler_from_quaternion(x, y, z, w):
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return yaw_z, pitch_y, roll_x # in radians

60.

Мобильный робот. Добавляем лидар.
• В пакете m2wr_description в файле m2wr.urdf.xacro уже прописаны «линки» и
«джойнты» для лидара. Но там есть ссылка на 3Д-модель. Добавим файл модели
hokuyo.dae в пакет в папку meshes (ее нужно предварительно создать).
• В файле m2wr.gazebo.xacro также уже прописан контроллер лидара для газебо, его
нужно раскомментировать. Можно изменить некоторые настройки, например,
samples – количество точек, минимальный и максимальный угол обзора, диапазон
видимости, имя топика.
• В Rviz добавляем лидар Add->LaserScan. Выбираем нужный топик – отображаются
точки, там где препятствия. Препятствия предварительно добавить в газебо.
• LifeHack: чтобы точки корректно отображались необходимо Rviz открывать раньше,
чем газебо. А также в Rviz нужно выбрать в строке Fixed Frame – base_link.
• Тип собщения от лидара - sensor_msgs/LaserScan
• Можно выполнить команду $ rostopic echo /m2wr/laser/scan, чтобы посмотреть,
что приходит от лидара.

61.

Мобильный робот. Добавляем лидар.
• sensor_msgs/LaserScan
Header header
float32 angle_min
float32 angle_max
float32 angle_increment
# start angle of the scan [rad]
# end angle of the scan [rad]
# angular distance between measurements [rad]
float32 time_increment
float32 scan_time
# time between measurements [seconds] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
# time between scans [seconds]
float32 range_min
float32 range_max
# minimum range value [m]
# maximum range value [m]
float32[] ranges
x should be discarded)
# range data [m] (Note: values < range_min or > range_ma
float32[] intensities
# intensity data [device-specific units]. If your
# device does not provide intensities, please leave
# the array empty.

62.

Мобильный робот. Добавляем лидар. C++
• Для получения данных с лидара в программе:
• Создаем новый файл .cpp
• Подписываемся на топик лидара (rostopic list).
• В поле msg->ranges – массив со значениями расстояний с лидара.
• Также можно использовать angle_increment
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import LaserScan
import math
# Константа для вычисления угла в градусах
PI = 3.1415926
# Глобальные переменные для хранения данных
dist_arr = []
angle_increment = 0.0
def msg_callback(msg):
global dist_arr, angle_increment
# Сохраняем шаг угла
angle_increment = msg.angle_increment
# Копируем данные о расстояниях
dist_arr = msg.ranges
# Выводим в консоль значения углов и расстояний
for i in range(len(dist_arr)):
angle = i * angle_increment * 180.0 / PI # Угол в градусах
dist = dist_arr[i] # Расстояние
rospy.loginfo(f"angle: {angle:.2f}, dist: {dist:.2f}")

63.

Мобильный робот. Вывод графиков.
• Для вывода графиков можно использовать стандартные утилиты: rqt_plot и
rqt_multiplot.
• В rqt_plot мы можем выводить данные из топика относительно времени (по оси Х
всегда время).
• Примеры: 1) Вывод значения переменной, которая 1 раз в секунду
инкрементируется, а при достижении значения 10 – сбрасывается в 0. 2) Вторая
переменная квадратично зависит от первой.
1
2

64.

Мобильный робот. Вывод графиков.
• Если хотим вывести, например, траекторию движения робота, то есть, зависимость
Х от Y – используем rqt_multiplot.
• Устанавливаем: $ sudo apt-get install ros-noetic-rqt-multiplot
• Запускаем командой $ rqt_multiplot (возможно в первый раз нужно
прописать $ rqt_multiplot --force-discover).
• Нажимаем шестеренку в правом верхнем углу окна графика – выбираем топики и
значения из них для каждой из осей, а также остальные настройки.
• Пример вывода из предыдущего примера, где вторая переменная квадратично
зависит от первой, а первая инкрементируется 1 раз в секунду. Первая переменная
– ось Х, вторая – ось Y.
• Для запуска – кнопка Play.

65.

Мобильный робот. Вывод графиков.
• Пример вывода теоретической траектории движения робота (код программы
смотри — https://github.com/AntonKlekovkin/MobileRobotsROS/).
• Чтобы каждый раз не выбирать топики и настройки осей, можно сохранить конфигфайл.