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

Введение

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

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

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

Давайте для начала сформулируем задачу и ее границы:

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

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

Перед тем как перейти непосредственно к программированию, сделаем небольшую ремарку по поводу регулятора.

Принцип регулирования, который мы сформулировали для управления поведением нашего робота, т.е. останавливаться по превышению одного метра, а до этого держать постоянную скорость, называется релейным регулятором. Это самый простой из всех возможных регуляторов. Как только наступает интересующее нас событие мы просто щелкаем кнопку и робот останавливается. Тот же принцип реализован в чайниках, датчиках дождя и освещенности, ёмкостных охранных комплексах да и много где еще.

Получаем нужный нам сигнал, включаем или выключаем реле управления. Для управления движением робота это не самый правильный способ, т.к. роботы обладают инерцией и даже если мы выключим их двигатели, они все равно могут проехать еще какое-то расстояние.

Пишем код

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

Начнем с получения сигнала о положении нашего робота. Данные о положении публикуются в топик /odom. Давайте посмотрим как они выглядят. Сначала найдем название типа данных:

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. Теперь давайте посмотрим из чего она состоит:

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.

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

import rospy

Теперь нам надо импортировать все структуры данных, которыми мы будем пользоваться:

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

Теперь создадим и зарегистрируем в ROS нашу ноду. Назовем ее one_meter_node:

rospy.init_node("one_meter_node")

Далее создадим объект Издатель.

Так как мы управляем роботом, через публикацию в топик /cmd_vel, заполненной структуры данных Twist, то и Издатель у нас выглядит следующим образом:

pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

Теперь давайте создадим объект Подписчик. Пусть он подпишется на топик /odom и как только в топике появится структура данных Odometry, вызовет функцию cb_regulator и передаст в нее эту структуру данных:

rospy.Subscriber("/odom", Odometry, cb_regulator)

Теперь напишем функцию обратного вызова (регулятор), который и будет реализовывать нашу логику работы:

def cb_regulator(msg):  

Создадим в этой функции переменную vel типа Twist(). Именно ее мы будем заполнять и публиковать при помощи Издателя:

def cb_regulator(msg):  
    vel = Twist() 

Дальше, как вы помните, в функцию обратного вызова передается в качестве аргумента, вся структура данных, которая публикуется в топик, на которую подписан Подписчик. В нашем случае это структура Odometry. По традиции, мы назовем аргумент нашей функции msg, но разумеется вы можете заменить его на любое другое название.

Итак, в msg у нас лежит вся структура Odometry, но из нее нам нужна только одна переменная x. Получим к ней доступ, указав ее через точки, от родительских структур и сравним с нулём:

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.

Небольшая ремарка. В связи с тем что программа в Python выполняется последовательно, то и определить нашу функцию обратного вызова мы должны до того, как укажем ее в Подписчике. Давайте расположим ее перед вызовом Подписчика.

В основном теле программы, используем метод библиотеки rospy - spin() чтобы наша нода не закрывалась:

rospy.spin()

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

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

Мы с вами написали программу, в которой есть Подписчик - входные сигналы, Издатель - выходные сигналы и сам регулятор - наша логика!

#! /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()

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

Перед тем, как запускать робота на проезд, рекомендуется сбрасывать значения одометрии. Сделать это можно двумя путями: программно и физически. Физически: поставьте робота на точку из которой он поедет и нажмите кнопку restart, расположенную рядом с индикатором батареи;

Программно: поставьте робота на точку из которой он поедет и в терминале вызовете сервис /reset: rosservice call /reset

P.S. О сервисах мы поговорим в Продвинутом курсе по ROS

Для того, чтобы проверить, что одометрия сбросилась вызовите топик /odom и убедитесь, что координаты X и Y равны нулю:

rostopic echo /odom -n 1

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

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

ssh [e-mail]

python3 ros_course/pub-sub1.py

Робот действительно проехал какое-то расстояние. Теперь давайте проверим, что он проехал действительно 1 метр:

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 и не увеличивается, так как робот остановился, т.е. все работает именно так, как мы хотели.