Урок 3

Введение: Данная задача является продолжением задач про считывание данных с датчиков робота и управление движением робота на основании этих данных. В данном уроке мы будем читать данные одометрии и подавать команды на движение роботу, таким образом, чтобы он проехал заданное нами расстояние 0.4м.

Подготовка: Попросите учащихся непосредственно перед выполнением написанной программы, при помощи управления через веб-интерфейс, поставить робота на небольшом расстоянии от любой из стенок полигона. По внешней камере полигона убедитесь, что перед роботом достаточно свободного пространства, чтобы он мог свободно проехать 0.4м. После того как робот занял стартовое положение попросите учащихся "сбросить" одометрию, тем самым фиксируется нулевое положение робота. Сброс одометрии надо делать перед каждым стартом программы. Для сброса одометрии используется вызов сервиса rosservice call /reset

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

Общий алгоритм решения:

1. Создаем Издателя для топика команд на движение /cmd_vel

2. Создаем Подписчика на данные одометрии.

3. В функции обратного вызова Подписчика считываем данные одометрии о пройденном расстоянии по оси Х.

4. Сравниваем пройденное роботом расстояние с заданным, и если оно меньше, то публикуем в топик команд на движение /cmd_vel какое-то положительное значение линейной скорости. Если же расстояние пройденное роботом больше или равно заданному, публикуем нулевую линейную скорость, чтобы робот остановился.

Структура данных Odometry

Структуру данных Odometry описывает сообщение nav_msgs. Из каких элементов состоит структура можно прочитать в документации.

Список структур данных, описываемых nav_msgs представлен здесь

Как и для чего используется Odometry рассказано в нашем базовом курсе по ROS

Решение: Для начала импортируем библиотеку rospy, структуру данных Odometry для данных одометрии и структуру Twist для топика команд на движение. Затем инициализируем ноду, создадим экземпляр издателя в топик команд на движение /cmd_vel и подписчика на топик /odom, в котором публикуются данные одометрии. Кроме того создадим переменную target в которую запишем то расстояние, которое мы хотим, чтобы робот проехал вперед и переменную vel типа Twist, которую будем сначала заполнять нужным нам значением линейной скорости, а затем публиковать при помощи экземпляра подписчика.

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

rospy.init_node('lineyka')

pub = rospy.Publisher("cmd_vel", Twist, queue_size = 1)
target = 0.3
vel = Twist()

rospy.Subscriber("/odom", Odometry, callback)

Далее создадим функцию обратного вызова для нашего подписчика. Функция будет брать структуру данных Odometry, которую передает ей подписчик, и выделять из всей этой сложной структуры только данные по пройденному расстоянию по оси X. Для того чтобы определить где в структуре данных Odometry находится значение пройденного расстояния воспользуемся официальной документацией ROS: http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html

Пройдите по ссылкам вглубь структуры и найдите значение position.x - http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Point.html именно эта переменная нас интересует. Обратите внимание, что она расположена достаточно глубоко от верхнего уровня структуры и чтобы до нее добраться, нам надо указать полный путь до нее через точку. Получится - odom.pose.pose.position.x именно с этим значением мы и будем дальше работать.

Далее функция будет сравнивать пройденное расстояние с переменной target, и если оно меньше заданного, то устанавливать значение линейной скорости по Х = 0.1, а если больше или равно заданному, то значение линейной скорости по Х будет равно нулю. После чего в функция будет вызывать метод publish нашего издателя и передавать ему "собранную" структуру данных скоростей.

def callback(odom):
    if odom.pose.pose.position.x < target:
        vel.linear.x = 0.1
    else:
        vel.linear.x = 0
    pub.publish(vel)

Расположим нашу функцию обратного вызова таким образом, чтобы она находилась перед объявлением Подписчика. И в конце добавим rospy.spin(), чтобы программа не закрывалась сразу. Всё! Наша программа готова. Вот так она выглядит.

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

rospy.init_node('lineyka')
pub = rospy.Publisher("cmd_vel", Twist, queue_size = 1)
target = 0.4
vel = Twist()

def callback(odom):
    if odom.pose.pose.position.x < target:
        vel.linear.x = 0.1
    else:
        vel.linear.x = 0
    pub.publish(vel)

rospy.Subscriber("/scan", Odometry, callback)

rospy.spin()

Проверка решения:

Попросите учащихся перенести написанные программы на своих роботов и запустить. После запуска программы робот должен проехать ровно 0.4 м. Попросите учащихся проконтролировать это по камере полигона, принимая во внимание, что размеры робота 20х20см. А также проверить что расстояние правильное, прочитав значения одометрии в топике odom при помощи команды rostopic echo /odom

При помощи веб-интерфейса попросите учащихся вернуть робота на исходную позицию, снова сбросить одометрию при помощи rosservice call /reset, задать новое значение переменной target снова запустить программу и проверьте что робот снова все выполняет корректно. Проконтролируйте работу учащихся в режиме трансляции их экрана через Discord.