> For the complete documentation index, see [llms.txt](https://voltbro.gitbook.io/new_bazovyi-kurs-po-ros/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://voltbro.gitbook.io/new_bazovyi-kurs-po-ros/lesson5/pub-sub-1.md). # Взаимодействие Подписчика и Издателя в рамках одной ноды. Часть 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 и не увеличивается, так как робот остановился, т.е. все работает именно так, как мы хотели. --- # Agent Instructions This documentation is published with GitBook. GitBook is the documentation platform designed so that both humans and AI agents can read, navigate, and reason over technical content effectively. Learn more at gitbook.com. ## Querying This Documentation If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question. Perform an HTTP GET request on the current page URL with the `ask` query parameter: ``` GET https://voltbro.gitbook.io/new_bazovyi-kurs-po-ros/lesson5/pub-sub-1.md?ask= ``` The question should be specific, self-contained, and written in natural language. The response will contain a direct answer to the question and relevant excerpts and sources from the documentation. Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.