Взаимодействие Подписчика и Издателя в рамках одной ноды. Часть 2
Last updated
Last updated
На прошлом уроке мы рассмотрели как Подписчик и Издатель могут работать в рамках одной программы-ноды, при этом запуск издателя зависит от момента срабатывания функции вызываемой Подписчиком.
В этом уроке мы перепишем нашу прошлую программу так, чтобы оба эти события - вызов функции Подписчиком и публикация Издателем были независимы друг от друга.
Для удобной организации доступа к переменным нашей программы мы будем использовать классы. Для тех из вас, кто пока чувствует себя неуверенно с принципами объектно-ориентированного программирования, наша программа будет хорошим примером использования классов.
Программа которую мы будем с вами писать, будет делать ровно то, что и предыдущая - будет следить, чтобы наш робот проезжал ровно 1 метр.
Условие для регулятора не изменится - мы будем ехать вперед, до тех пор, пока разница между текущим значением и нулевым значением, не будет больше 1 метра, и как только она будет больше или равна 1 метру, мы остановим нашего робота.
Итак, начнем: возьмем программу из предыдущего видео и добавим в нее немного объекто-ориентированной магии.
Назовем нашу программу: pub-sub2.py и сохраним ее в папку ros-cource в папке на роботе.
Оставим импорты rospy и типов данных на своем месте:
А вот дальше сделаем класс который и будем описывать управление роботом.
Как вы знаете из Базового курса про ROS (раздел про ООП) - класс описывается при помощи ключевого слова class, и в целом описание класса, очень похоже на определение функции.
Давайте назовем наш класс RobotMover:
И первая функция, которую обязательно надо определить в классе - это функция с названием __init__(). Именно она вызывается, когда создается экземпляр нашего класса:
В неё удобно запихнуть все наши определения Подписчиков, Издателей, используемых переменных и т.д.
Смотрите мы объявляем в ней Издателей и Подписчиков из нашей старой программы, а также переменную vel типа Twist(), которую мы и будем публиковать. Я хочу сделать это здесь в функции __init_(), чтобы не тратить ресурсы компьютера каждый раз на ее инициализацию в каком-нибудь другом месте:
Обратите внимание, в рамках класса появляется ключевое слово self. Оно указывает на то, что функция или переменная относится именно к этому экземпляру класса. Собственно, это сильно облегчает использование переменных внутри класса. Вы можете заполнить self переменную в одной функции класса, а прочитать ее в другой. Разумеется надо отчетливо понимать, что вы делаете и зачем, и не удивляться, когда переменная изменила свое значение, после того как несколько функций использовали её для своих целей. В целом, подход к использованию переменных класса такой же, как и к использованию глобальных переменных. Это полезная и удобная вещь, если вы пользуетесь ей по назначению. И опасная и непредсказуемая, если вы используете глобальные переменные или переменные класса для того, для чего они не предназначены. В нашей программе в переменных класса мы будем хранить актуальное состояние объекта. И каждый раз изменяя или считывая это состояние, мы будем понимать, что в каждый следующий момент времени это состояние будет другим. И в данном случае использовать переменные класса, для хранения состояния объекта - и удобно и оправдано.
Вернемся к написанию кода. По правилам хорошего тона, мы будем писать функции таким образом, чтобы каждая из них делала какое-то очень простое, но при этом осмысленное действие. В дальнейшем, такой подход позволит нам с легкостью модифицировать уже написанный код. Давайте опишем функцию обратного вызова. Сделаем ее максимально лаконичной. Все что будет делать эта функция, это брать значение пройденного пути по оси Х из структуры Odometry и присваивать его в переменную класса self.distance_passed. Так как никаких других действий эта функция выполнять не будет, переименуем ее из cb_regulator в cb_handler:
Обратите внимание на ключевое слово self. Оно требуется для передачи в качестве аргумента, во все функции класса, использующие переменные класса.
Не забудьте переименовать функцию обратного вызова в инициализации Подписчика !
Теперь напишем функцию которая будет управлять публикацией значений скорости в топик /cmd_vel. Назовем ее vel_publisher():
Здесь кроме ключевого слова self, мы укажем еще один аргумент, который мы будем передавать в эту функцию, для того, чтобы она именно его и публиковала:
Итак, функция берет передаваемый ей аргумент желаемой скорости, заполняет им значение линейной скорости по оси X переменной vel и публикует используя объект Издателя, который мы инициализируем при создании объекта класса. Так как мы уже определили Издателя в функции __init__(), нашего класса, здесь мы можем просто пользоваться этим объектом и не определять его повторно.
Теперь напишем еще одну функцию, которая будет сравнивать значение переменной класса self.distance_passed с расстоянием в 1 метр. И если оно меньше 1 метра, то будет вызывать другую функцию vel_publisher(), передавая ей в качестве аргумента желаемое значение линейной скорости, а если оно больше или равно одному метру, то будет вызываться все та же функция vel_publisher(), но с нулевой скоростью в качестве аргумента. Эта функция и будет нашей функцией регулятором, поэтому так ее и назовем regulator():
Теперь давайте напишем основной цикл нашей программы. Во-первых, создадим экземпляр нашего класса и передадим ссылку на него в какую-нибудь переменную, чтобы в дальнейшем мы могли пользоваться методами нашего класса обращаясь к ним, через созданный экземпляр:
Дальше используем стандартный ROS цикл, который позволит нам корректно выйти из нашей программы по нажатию Ctrl+C:
А в самом цикле, мы будем вызывать функцию-регулятор с той частотой, которая нам будет нужна. Например 2 герца:
В отличие от прошлой программы - где регулятор, вызывался не из нашего основного цикла, а созданным нами Подписчиком, в данном случае именно МЫ определяем частоту, с который будет вызываться регулятор, то есть, фактически делаем это с той частотой, которая нам наиболее удобна, и не зависим от того, с какой частотой приходят данные из ROS.
Например, можно применить такой аналог - в случае программы из прошлого урока, мы неотрывно смотрим на одометр и как только там появляется 1 метр, сразу останавливаемся. А в текущей реализации, мы поглядываем на одометр с удобной нам частотой, а в остальное время можем заниматься тем, чем хотим.
Вот и все, сохраним нашу программу и запустим это все на роботе, чтобы посмотреть как оно будет работать.
Зайдем на самого робота по ssh и запустим файл:
Наш робот проехал метр и остановился.
Теперь давайте проверим, насколько больше ошибка точности остановки по сравнению с прошлым значением. Запустите предыдущую программу и запишите конечное значение положения по оси X, сбросьте одометрую и запустите программу из этого урока.
Разница не большая, и я бы не сказал, что дело здесь только в частоте принятия решения и публикации команд. На самом деле для улучшения точности остановки робота, нам надо применить качественно другой регулятор, не релейного типа.