Подписчик на данные робота

Введение

Превратим нашу программу (subscriber.py), из прошлого урока, из абстрактной читалки сообщений в нечто более осмысленное. Но для сохранения преемственности мы также как и с программой publisher_for_robor.py используем нашу прошлую программу в качестве заготовки.

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

Возьмем поток данных с лидара. Лидар на нашем роботе вращается с частотой около 5 герц и на выходе создает массив из 360 точек (по одной на каждый градус), отображающий расстояния до объектов, лежащих в плоскости его вращения.

Давайте на базе этих входных сигналов от лидара сделаем из робота лазерный дальномер. И будем при помощи него измерять расстояния до объектов.

Лидар публикует свои данные в топик, который называется /scan. Давайте посмотрим какая структура данных в этом топике и как нам импортировать ее в Python. Для этого выполним команду:

rostopic info /scan

---------------
Type: sensor_msgs/LaserScan

Publishers: 
 * /rplidarNode (http://192.168.1.73:37571/)

Subscribers: None

Как мы видим, структура данных топика это LaserScan, а описание ее мы можем найти в файле sensor_msgs.msg.

Давайте подробнее посмотрим из чего состоит структура LaserScan, зайдем на страницу официальной документации по ссылке:

https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/LaserScan.html

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

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

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

Итак, давайте перейдем непосредственно к программированию и сформулируем нашу задачу. Мы напишем программу Подписчик, которая подпишется на структуру данных LaserScan топика /scan и будет выводить нулевой элемент массива ranges этой структуры.

Пишем код

Давайте возьмем программу Подписчик (subscriber.py) и немного ее модифицируем.

Назовем нашу программу: subscriber_to_lidar.py и перейдем непосредственно к программированию.

Импорт rospy остается:

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

import rospy 

Вместо String, который мы использовали, чтобы читать “Hello ROS”, мы будем использовать структуру данных лидара - LaserScan:

from sensor_msgs.msg import LaserScan

Для создания ноды мы также используем стандартный метод init_node(), но давайте назовем ее, например - scan_node:

rospy.init_node("scan_node")

Раз мы читаем структуру LaserScan из топика /scan, то давайте пока пропустим определение функции обратного вызова и сначала сконфигурируем Подписчик на этот топик и эту структуру данных. Вот так:

pub = rospy.Subscriber("scan", LaserScan, scan_cb)

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

Теперь вернемся к функции обратного вызова и перепишем ее.

Во первых, поменяем ей имя на scan_cb. Во вторых, заменим имя переменной, которая принимает в себя объект LaserScan, на что-то более осмысленное. Например, на scan. Далее из всей структуры LaserScan, нам нужен только массив ranges, а в нем только нулевой элемент. Именно его мы и будем выводить на экран. Как вы помните, доступ до элементов структуры данных в Python осуществляется через точку:

def scan_cb(scan):
    print(scan.ranges[0])

Так как вся логика работы - выведение на экран нулевого элемента массива, заложена в функции обратного вызова нашего Подписчика, в основном теле программы мы оставим только rospy.spin(), который не будет давать закрываться нашей ноде:

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

import rospy 
from sensor_msgs.msg import LaserScan

rospy.init_node("scan_node")

def scan_cb(scan):
    print(scan.ranges[0])

pub = rospy.Subscriber("scan", LaserScan, scan_cb)

rospy.spin()

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

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

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

ssh [e-mail]

python3 ros_course/subscriber_to_lidar.py

Как вы видите в терминале появляются данные расстояния до объектов, которые расположены строго перед нашим роботом. И если мы будем направлять его на интересующий нас объект, он будет показывать расстояние до него в метрах.

Всё работает так как мы хотели!

Обратите внимание, что иногда среди этих данных проскакивает строка inf. Это связано с тем, что лидар теряет отраженный луч, из-за засветки или переотражений и считает, что луч не вернулся, а значит ушел на бесконечность - infinity. Так и пишет в значение расстояния - inf. Сейчас нас не очень волнуют эти inf’ы.