Взаимодействие Подписчика и Издателя в рамках одной ноды. Часть 1
Last updated
Last updated
На этом уроке начнем рассматривать то, как Подписчик и Издатель могут работать в рамках одной программы-ноды.
Сейчас мы напишем полнофункциональную ноду-регулятор, которая принимает, какие-то входящие сигналы при помощи Подписчика, анализирует их по правилам, которые мы с вами определим, и отправляет выходные сигналы с командами для робота, при помощи Издателя.
Итак, программа которую мы будем с вами писать, будет делать так, чтобы наш робот проезжал ровно 1 метр вперед.
Давайте для начала сформулируем задачу и ее границы:
Во-первых, для упрощения написания программы, мы будем задавать движение по прямой только вдоль оси Х робота и считать пройденное расстояние тоже - только вдоль этой оси. Понятно, что если робот поедет под углом к оси Х, то реальное пройденное расстояние будет больше измеренного по оси Х, на единицу делить на косинус угла отклонения. Но так как мы не будем поворачивать робота, а будем ехать вдоль оси Х, то никакие боковые перемещения учитываться не будут.
Во-вторых, давайте сформулируем условие для программы регулятора. Мы будем ехать вперед, до тех пор, пока разница между текущим значением и нулевым значением, не будет больше 1 метра, и как только она будет больше или равна 1 метру, мы будем останавливать нашего робота.
Перед тем как перейти непосредственно к программированию, сделаем небольшую ремарку по поводу регулятора.
Принцип регулирования, который мы сформулировали для управления поведением нашего робота, т.е. останавливаться по превышению одного метра, а до этого держать постоянную скорость, называется релейным регулятором. Это самый простой из всех возможных регуляторов. Как только наступает интересующее нас событие мы просто щелкаем кнопку и робот останавливается. Тот же принцип реализован в чайниках, датчиках дождя и освещенности, ёмкостных охранных комплексах да и много где еще.
Получаем нужный нам сигнал, включаем или выключаем реле управления. Для управления движением робота это не самый правильный способ, т.к. роботы обладают инерцией и даже если мы выключим их двигатели, они все равно могут проехать еще какое-то расстояние.
Назовем нашу программу: pub-sub1.py и сохраним ее в папку ros-cource в папке на роботе.
Начнем с получения сигнала о положении нашего робота. Данные о положении публикуются в топик /odom. Давайте посмотрим как они выглядят. Сначала найдем название типа данных:
Как мы видим структура данных называется Odometry и расположена она в одном из стандартных пакетов сообщений ROS - nav_msgs. Теперь давайте посмотрим из чего она состоит:
Как вы видите, это достаточно громоздкая штука, но из всего этого набора данных нам понадобится только одна переменная: pose.pose.position.x именно по её изменению мы и будем судить о пройденном пути.
Управлять движением робота мы будем уже знакомым нам способом - заполнять структуру данных Twist нужными нам значениями скоростей и публиковать их в топик /cmd_vel
Для начала, нам надо подключить библиотеку rospy.
Теперь нам надо импортировать все структуры данных, которыми мы будем пользоваться:
Теперь создадим и зарегистрируем в ROS нашу ноду. Назовем ее one_meter_node:
Далее создадим объект Издатель.
Так как мы управляем роботом, через публикацию в топик /cmd_vel, заполненной структуры данных Twist, то и Издатель у нас выглядит следующим образом:
Теперь давайте создадим объект Подписчик. Пусть он подпишется на топик /odom и как только в топике появится структура данных Odometry, вызовет функцию cb_regulator и передаст в нее эту структуру данных:
Теперь напишем функцию обратного вызова (регулятор), который и будет реализовывать нашу логику работы:
Создадим в этой функции переменную vel типа Twist(). Именно ее мы будем заполнять и публиковать при помощи Издателя:
Дальше, как вы помните, в функцию обратного вызова передается в качестве аргумента, вся структура данных, которая публикуется в топик, на которую подписан Подписчик. В нашем случае это структура Odometry. По традиции, мы назовем аргумент нашей функции msg, но разумеется вы можете заменить его на любое другое название.
Итак, в msg у нас лежит вся структура Odometry, но из нее нам нужна только одна переменная x. Получим к ней доступ, указав ее через точки, от родительских структур и сравним с нулём:
Таким образом, каждый раз Подписчик вызывает функцию обратного вызова и передает ей всю структуру Odometry, а функция регулятор сравнивает текущее значение координаты х с целевым значением 1 метр и если оно меньше, то заполняет линейную скорость по х значением 0.1 метр в секунду, а если оно больше или равно 1, то нулем и после этого публикует этот заполненный Twist в топик /cmd_vel при помощи Издателя pub.
Небольшая ремарка. В связи с тем что программа в Python выполняется последовательно, то и определить нашу функцию обратного вызова мы должны до того, как укажем ее в Подписчике. Давайте расположим ее перед вызовом Подписчика.
В основном теле программы, используем метод библиотеки rospy - spin() чтобы наша нода не закрывалась:
В самом теле программы может ничего не происходить, и это потому что основная функция нашей программы - регулятор, вызывается не из нашего тела программы, а созданным нами Подписчиком и именно в тот момент, когда в топик приходит сообщение. Фактически наша функция-регулятор вызывается по команде из ROS.
И еще один момент о котором надо сказать - функция-регулятор вызывается асинхронно, то есть, что-бы не делала наша программа в процессе, как только в топик - Подписчика публикуется сообщение, программа останавливается и вызывается функция-регулятор и только после того, как функция-регулятор отработает, продолжается основной цикл нашей программы.
Мы с вами написали программу, в которой есть Подписчик - входные сигналы, Издатель - выходные сигналы и сам регулятор - наша логика!
Вот и все, сохраним нашу программу и запустим это все на роботе, чтобы посмотреть как оно будет работать.
Перед тем, как запускать робота на проезд, рекомендуется сбрасывать значения одометрии. Сделать это можно двумя путями: программно и физически. Физически: поставьте робота на точку из которой он поедет и нажмите кнопку restart, расположенную рядом с индикатором батареи;
Программно: поставьте робота на точку из которой он поедет и в терминале вызовете сервис /reset:
rosservice call /reset
P.S. О сервисах мы поговорим в Продвинутом курсе по ROS
Для того, чтобы проверить, что одометрия сбросилась вызовите топик /odom и убедитесь, что координаты X и Y равны нулю:
rostopic echo /odom -n 1
Зайдем на самого робота по ssh и запустим файл:
Робот действительно проехал какое-то расстояние. Теперь давайте проверим, что он проехал действительно 1 метр:
Как вы видите значение Х чуть больше 1 и не увеличивается, так как робот остановился, т.е. все работает именно так, как мы хотели.