# Взаимодействие Подписчика и Издателя в рамках одной ноды. Часть 1 {% embed url="" %} ### Введение На этом уроке начнем рассматривать то, как Подписчик и Издатель могут работать в рамках одной программы-ноды. Сейчас мы напишем полнофункциональную ноду-регулятор, которая принимает, какие-то входящие сигналы при помощи *Подписчика*, анализирует их по правилам, которые мы с вами определим, и отправляет выходные сигналы с командами для робота, при помощи *Издателя*. Итак, программа которую мы будем с вами писать, будет делать так, чтобы наш робот проезжал ровно 1 метр вперед. Давайте для начала сформулируем задачу и ее границы: Во-первых, для упрощения написания программы, мы будем задавать движение **по прямой только вдоль оси Х робота** и считать пройденное расстояние тоже - только вдоль этой оси. Понятно, что если робот поедет под углом к оси Х, то реальное пройденное расстояние будет больше измеренного по оси Х, на ***единицу делить на косинус угла отклонения***. Но так как мы не будем поворачивать робота, а будем ехать вдоль оси Х, то никакие боковые перемещения учитываться не будут. Во-вторых, давайте сформулируем условие для программы регулятора. Мы будем ехать вперед, до тех пор, пока разница между **текущим значением** и **нулевым значением**, не будет **больше 1 метра**, и как только она будет **больше или равна 1 метру**, мы будем останавливать нашего робота. {% hint style="info" %} Перед тем как перейти непосредственно к программированию, сделаем небольшую ремарку по поводу регулятора. Принцип регулирования, который мы сформулировали для управления поведением нашего робота, т.е. останавливаться по превышению одного метра, а до этого держать постоянную скорость, называется ***релейным регулятором***. Это самый простой из всех возможных регуляторов. Как только наступает интересующее нас событие мы просто щелкаем кнопку и робот останавливается. Тот же принцип реализован в чайниках, датчиках дождя и освещенности, ёмкостных охранных комплексах да и много где еще. Получаем нужный нам сигнал, включаем или выключаем реле управления. Для управления движением робота это не самый правильный способ, т.к. роботы обладают инерцией и даже если мы выключим их двигатели, они все равно могут проехать еще какое-то расстояние. {% endhint %} ### Пишем код Назовем нашу программу: *pub-sub1.py* и сохраним ее в папку *ros-cource* в папке на роботе. Начнем с получения сигнала о положении нашего робота. Данные о положении публикуются в топик */odom*. Давайте посмотрим как они выглядят. Сначала найдем название типа данных: ```python rostopic info /odom -------------------- Type: nav_msgs/Odometry Publishers: * /stm_serial_node (http://192.168.1.73:45261/) Subscribers: * /simple_odom (http://192.168.1.73:40251/) * /web_telemetry_node (http://192.168.1.73:45799/) ``` Как мы видим структура данных называется Odometry и расположена она в одном из стандартных пакетов сообщений ROS - **nav\_msgs.** Теперь давайте посмотрим из чего она состоит: ```python rosmsg info nav_msgs/Odometry ----------------------------- std_msgs/Header header uint32 seq time stamp string frame_id string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[36] covariance geometry_msgs/TwistWithCovariance twist geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z float64[36] covariance ``` Как вы видите, это достаточно громоздкая штука, но из всего этого набора данных нам понадобится только одна переменная: `pose.pose.position.x` именно по её изменению мы и будем судить о пройденном пути. Управлять движением робота мы будем уже знакомым нам способом - заполнять структуру данных *Twist* нужными нам значениями скоростей и публиковать их в топик **/cmd\_vel** Для начала, нам надо подключить библиотеку **rospy**. ```python #! /usr/bin/env python3 # -*- coding: utf-8 -*- import rospy ``` Теперь нам надо импортировать все структуры данных, которыми мы будем пользоваться: ```python from nav_msgs.msg import Odometry from geometry_msgs.msg import Twist ``` Теперь создадим и зарегистрируем в ROS нашу ноду. Назовем ее *one\_meter\_node*: ```python rospy.init_node("one_meter_node") ``` Далее создадим объект *Издатель*. Так как мы управляем роботом, через публикацию в топик **/cmd\_vel**, заполненной структуры данных *Twist*, то и *Издатель* у нас выглядит следующим образом: ```python pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) ``` Теперь давайте создадим объект *Подписчик*. Пусть он подпишется на топик **/odom** и как только в топике появится структура данных *Odometry*, вызовет функцию **cb\_regulator** и передаст в нее эту структуру данных: ```python rospy.Subscriber("/odom", Odometry, cb_regulator) ``` Теперь напишем функцию обратного вызова (регулятор), который и будет реализовывать нашу логику работы: ```python def cb_regulator(msg): ``` Создадим в этой функции переменную **vel** типа *Twist()*. Именно ее мы будем заполнять и публиковать при помощи *Издателя:* ```python def cb_regulator(msg): vel = Twist() ``` Дальше, как вы помните, в функцию обратного вызова передается в качестве аргумента, вся структура данных, которая публикуется в топик, на которую подписан *Подписчик*. В нашем случае это структура *Odometry*. По традиции, мы назовем аргумент нашей функции **msg**, но разумеется вы можете заменить его на любое другое название. Итак, в **msg** у нас лежит вся структура *Odometry*, но из нее нам нужна только одна переменная **x**. Получим к ней доступ, указав ее через точки, от родительских структур и сравним с нулём: ```python def cb_regulator(msg): vel = Twist() if msg.pose.pose.position.x >= 1: vel.linear.x = 0 else: vel.linear.x = 0.1 pub.publish(vel) ``` Таким образом, каждый раз *Подписчик* вызывает функцию обратного вызова и передает ей всю структуру *Odometry*, а функция регулятор сравнивает текущее значение **координаты х** с целевым значением **1 метр** и если оно меньше, то заполняет **линейную скорость по х** значением 0.1 метр в секунду, а если оно больше или равно 1, то нулем и после этого публикует этот заполненный *Twist* в топик **/cmd\_vel** при помощи *Издателя* **pub**. {% hint style="info" %} Небольшая ремарка. В связи с тем что программа в Python выполняется последовательно, то и определить нашу функцию обратного вызова мы должны до того, как укажем ее в Подписчике. Давайте расположим ее перед вызовом Подписчика. {% endhint %} В основном теле программы, используем метод библиотеки **rospy** - **spin()** чтобы наша нода не закрывалась: ```python rospy.spin() ``` В самом теле программы может ничего не происходить, и это потому что основная функция нашей программы - регулятор, вызывается не из нашего тела программы, а созданным нами *Подписчиком* и именно в тот момент, когда в топик приходит сообщение. Фактически наша функция-регулятор вызывается по команде из ROS. {% hint style="info" %} И еще один момент о котором надо сказать - функция-регулятор вызывается ***асинхронно***, то есть, что-бы не делала наша программа в процессе, как только в топик - *Подписчика* публикуется сообщение, программа останавливается и вызывается функция-регулятор и только после того, как функция-регулятор отработает, продолжается основной цикл нашей программы. {% endhint %} Мы с вами написали программу, в которой есть Подписчик - входные сигналы, Издатель - выходные сигналы и сам регулятор - наша логика! ```python #! /usr/bin/env python3 # -*- coding: utf-8 -*- import rospy from nav_msgs.msg import Odometry from geometry_msgs.msg import Twist rospy.init_node("one_meter_node") pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) def cb_regulator(msg): vel = Twist() if msg.pose.pose.position.x >= 1: vel.linear.x = 0 else: vel.linear.x = 0.1 pub.publish(vel) rospy.Subscriber("/odom", Odometry, cb_regulator) rospy.spin() ``` Вот и все, сохраним нашу программу и запустим это все на роботе, чтобы посмотреть как оно будет работать. {% hint style="info" %} Перед тем, как запускать робота на проезд, рекомендуется сбрасывать значения одометрии. Сделать это можно двумя путями: программно и физически.\ \&#xNAN;*Физически*: поставьте робота на точку из которой он поедет и нажмите кнопку ***restart***, расположенную рядом с индикатором батареи; \ \&#xNAN;*Программно*: поставьте робота на точку из которой он поедет и в терминале вызовете сервис ***/reset:***\ `rosservice call /reset` *P.S. О сервисах мы поговорим в Продвинутом курсе по ROS* Для того, чтобы проверить, что одометрия сбросилась вызовите топик /odom и убедитесь, что координаты X и Y равны нулю: `rostopic echo /odom -n 1` {% endhint %} ### Запускаем и тестируем Зайдем на самого робота по ssh и запустим файл: ```python ssh pi@turtlebro37.local python3 ros_course/pub-sub1.py ``` Робот действительно проехал какое-то расстояние. Теперь давайте проверим, что он проехал действительно 1 метр: ```python rostopic echo /odom -n 1 ------------------------- header: seq: 71818 stamp: secs: 1656942862 nsecs: 269105052 frame_id: "odom" child_frame_id: "base_footprint" pose: pose: position: x: 1.0096851587295532 y: 0.01325586810708046 z: 0.0 orientation: x: 0.0 y: 0.0 z: -0.008814848959445953 w: -0.9999611377716064 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] twist: twist: linear: x: 0.0 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: -0.002222222276031971 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] --- ``` Как вы видите значение Х чуть больше 1 и не увеличивается, так как робот остановился, т.е. все работает именно так, как мы хотели.