Взаимодействие Подписчика и Издателя в рамках одной ноды. Часть 2

Введение

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

В этом уроке мы перепишем нашу прошлую программу так, чтобы оба эти события - вызов функции Подписчиком и публикация Издателем были независимы друг от друга.

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

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

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

Пишем код

Итак, начнем: возьмем программу из предыдущего видео и добавим в нее немного объекто-ориентированной магии.

Назовем нашу программу: pub-sub2.py и сохраним ее в папку ros-cource в папке на роботе.

Оставим импорты rospy и типов данных на своем месте:

#! /usr/bin/env python3
# -*- coding: utf-8 -*-

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

А вот дальше сделаем класс который и будем описывать управление роботом.

Как вы знаете из Базового курса про ROS (раздел про ООП) - класс описывается при помощи ключевого слова class, и в целом описание класса, очень похоже на определение функции.

Давайте назовем наш класс RobotMover:

class RobotMover():

И первая функция, которую обязательно надо определить в классе - это функция с названием __init__(). Именно она вызывается, когда создается экземпляр нашего класса:

class RobotMover():
    def __init__(self):

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

Смотрите мы объявляем в ней Издателей и Подписчиков из нашей старой программы, а также переменную vel типа Twist(), которую мы и будем публиковать. Я хочу сделать это здесь в функции __init_(), чтобы не тратить ресурсы компьютера каждый раз на ее инициализацию в каком-нибудь другом месте:

class RobotMover():
    def __init__(self):
        rospy.init_node('one_meter_node')
        self.vel = Twist()
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        rospy.Subscriber('/odom', Odometry, self.cb_regulator)

Обратите внимание, в рамках класса появляется ключевое слово self. Оно указывает на то, что функция или переменная относится именно к этому экземпляру класса. Собственно, это сильно облегчает использование переменных внутри класса. Вы можете заполнить self переменную в одной функции класса, а прочитать ее в другой. Разумеется надо отчетливо понимать, что вы делаете и зачем, и не удивляться, когда переменная изменила свое значение, после того как несколько функций использовали её для своих целей. В целом, подход к использованию переменных класса такой же, как и к использованию глобальных переменных. Это полезная и удобная вещь, если вы пользуетесь ей по назначению. И опасная и непредсказуемая, если вы используете глобальные переменные или переменные класса для того, для чего они не предназначены. В нашей программе в переменных класса мы будем хранить актуальное состояние объекта. И каждый раз изменяя или считывая это состояние, мы будем понимать, что в каждый следующий момент времени это состояние будет другим. И в данном случае использовать переменные класса, для хранения состояния объекта - и удобно и оправдано.

Вернемся к написанию кода. По правилам хорошего тона, мы будем писать функции таким образом, чтобы каждая из них делала какое-то очень простое, но при этом осмысленное действие. В дальнейшем, такой подход позволит нам с легкостью модифицировать уже написанный код. Давайте опишем функцию обратного вызова. Сделаем ее максимально лаконичной. Все что будет делать эта функция, это брать значение пройденного пути по оси Х из структуры Odometry и присваивать его в переменную класса self.distance_passed. Так как никаких других действий эта функция выполнять не будет, переименуем ее из cb_regulator в cb_handler:

    def cb_handler(self, msg):
        self.distance_passed = msg.pose.pose.position.x

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

Не забудьте переименовать функцию обратного вызова в инициализации Подписчика !

Теперь напишем функцию которая будет управлять публикацией значений скорости в топик /cmd_vel. Назовем ее vel_publisher():

def vel_publisher(self, vel_value):

Здесь кроме ключевого слова self, мы укажем еще один аргумент, который мы будем передавать в эту функцию, для того, чтобы она именно его и публиковала:

    def vel_publisher(self, vel_value):
        self.vel.linear.x = vel_value 
        self.pub.publish(self.vel)

Итак, функция берет передаваемый ей аргумент желаемой скорости, заполняет им значение линейной скорости по оси X переменной vel и публикует используя объект Издателя, который мы инициализируем при создании объекта класса. Так как мы уже определили Издателя в функции __init__(), нашего класса, здесь мы можем просто пользоваться этим объектом и не определять его повторно.

Теперь напишем еще одну функцию, которая будет сравнивать значение переменной класса self.distance_passed с расстоянием в 1 метр. И если оно меньше 1 метра, то будет вызывать другую функцию vel_publisher(), передавая ей в качестве аргумента желаемое значение линейной скорости, а если оно больше или равно одному метру, то будет вызываться все та же функция vel_publisher(), но с нулевой скоростью в качестве аргумента. Эта функция и будет нашей функцией регулятором, поэтому так ее и назовем regulator():

    def regulator(self):
        print(self.distance_passed)
        if self.distance_passed >= 1:
            self.vel_publisher(0)
        else:
            self.vel_publisher(0.1)

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

r = RobotMover()

Дальше используем стандартный ROS цикл, который позволит нам корректно выйти из нашей программы по нажатию Ctrl+C:

while not rospy.is_shutdown():

А в самом цикле, мы будем вызывать функцию-регулятор с той частотой, которая нам будет нужна. Например 2 герца:

while not rospy.is_shutdown(): 
    rospy.sleep(0.5) 
    r.regulator()

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

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

#! /usr/bin/env python3
# -*- coding: utf-8 -*-

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

class RobotMover():
    def __init__(self):
        rospy.init_node('one_meter_node')
        self.vel = Twist()
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        rospy.Subscriber('/odom', Odometry, self.cb_handler)

    def cb_handler(self, msg):
        self.distance_passed = msg.pose.pose.position.x

    def vel_publisher(self, vel_value):
        self.vel.linear.x = vel_value 
        self.pub.publish(self.vel)

    def regulator(self):
        print(self.distance_passed)
        if self.distance_passed >= 1:
            self.vel_publisher(0)
        else:
            self.vel_publisher(0.1)

r = RobotMover()

while not rospy.is_shutdown(): 
    rospy.sleep(0.5) 
    r.regulator()

Вот и все, сохраним нашу программу и запустим это все на роботе, чтобы посмотреть как оно будет работать.

Запускаем и тестируем

Зайдем на самого робота по ssh и запустим файл:

ssh [e-mail]

python3 ros_course/pub-sub2.py

Наш робот проехал метр и остановился.

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

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