Урок 2
Введение: Данная задача является одной из самых базовых задач комплексирования данных датчиков робота, принятия решений на основании этих данных и передачи команд роботу на изменение его состояния в зависимости от изменяющихся внешних условий. По логике организации взаимодействия с роботом, данная задача является развитием первой задачи про робота-лазерную линейку. Здесь и далее мы начнем применять базовые термины и понятия теории автоматизированного управления (ТАУ) и для того, чтобы вы чувствовали себя более уверенно мы рекомендуем прочесть несколько первых глав любой книги по введению в ТАУ, где разобраны основные термины и определения. Однако мы в любом случае будем стараться объяснять базовые понятия ТАУ в наших уроках.
В терминах теории управления данная задача пример простейшего цикла управления автоматической системой с обратной связью. Цикл обратной связи это такое построение управление системой, при котором для принятия решения о том, какое будет управляющее воздействие на систему, берутся данные о состоянии системы в текущий момент.
На примере автомобиля едущего по прямой - если мы хотим чтобы прямолинейное движение сохранялось, то мы анализируем текущее состояние машины и если она смещается вправо, то крутим руль влево, если машину тянет влево, то рулим вправо. Такой принцип управления и называется управлением по обратной связи. В данном случае управлением по отрицательной обратной связи т.к. смещение машины и направление поворота руля противоположны. При этом данные положения машины на дороге мы называем обратной связью, наши усилия по повороту руля в нужном направлении - управляющим воздействием, а правила по которым мы вращаем рулем - регулятором.
В основе регулятора данной системы применен релейный регулятор, т.е. регулятор управляющее воздействие которого имеет два положения вкл. и выкл. именно он отвечает за управляющее воздействие на основании обратной связи от лидара, и в нашей программе у нас будет всего два значения линейной скорости "едем" и "стоим".
Подготовка: Попросите учащихся непосредственно перед выполнением написанной программы, при помощи управления через веб-интерфейс, поставить робота перед любой из стенок полигона. По внешней камере полигона убедитесь, что расстояние между лидаром робота и впередилежащей стеной больше 50 см. Если программа написана некорректно, и робот делает что-то несогласованное, попросите учащихся прервать выполнение программы, остановить робота при при помощи управления через веб-интерфейс и вернуть его в первоначальную позицию в 50-60 см от стены.
Видео:
Задача: После запуска программы, основываясь на данных получаемых от лидара, робот должен начать двигаться прямо к впередилежащей стене полигона, и при подъезде к стене на заданное в программе расстояние робот должен самостоятельно остановиться.
Общий алгоритм решения:
1. Робот движется вперед с заданной скоростью.
2. В цикле проверяется больше ли целевое расстояние до стены, если расстояние до стены больше, то скорость не меняется. Если расстояние достигнуто, или меньше целевого, то дается команда на остановку робота.
3. Программа завершается
Решение: Для решения задачи мы будем использовать структуру данных Twist, которую содержит файл сообщения geometry_msgs.msg. В документации можно прочитать из каких элементов состоит эта структура.
Список всех структур данных, описываемых geometry_msgs представлен здесь
Как и для чего используется Twist рассказано в нашем базовом курсе по ROS
Далее импортируем библиотеку rospy, все структуры данных используемые в данной задаче, а именно Twist - для управления движением и LaserScan для данных получаемых с лидара. Кроме того объявим экземпляры издателя на топик /cmd_vel управляющий движением робота и подписчика на топик /scan, в котором публикуются данные лидара. В принципе, мы можем взять за основу программу из прошлого урока (лазерный дальномер), однако для практики можно написать и все заново. Выбор варианта остается за преподавателем.
import rospy
from geometry_msgs.msg import Twist
from sensors_msgs.msg import LaserScan
rospy.init_node('laser_mover')
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
rospy.Subscriber("/scan", LaserScan, lasercb)
Теперь, напишем функцию обратного вызова для подписчика на данные лидара. Функция будет брать структуру данных LaserScan, которую передает ей подписчик, "вырезать" из нее данные только по нулевому элементу массива ranges - именно там лежат данные расстояния до ближайшего препятствия "по носу" робота, и записывать их в глобальную переменную L. Эту переменную мы будем использовать в дальнейшем, для оценки расстояния до стены, но уже в другой функции. И именно для того, чтобы в другой функции нам было доступно ее значение заполняемое в этой функции мы и сделаем ее глобальной.
def lasercb(msg):
global L
L = msg.ranges[0]
Теперь напишем функцию которая будет преобразовывать скорость которую мы в нее будем передавать в структуру данных понятную нашему роботу и публиковать ее в топик управляющий движением. В ней мы создаем переменную pub_vel, типа Twist, потом присваиваем переданное в качестве аргумента vel в функцию значение желаемой скорости в элемент linear.x этой переменной pub_vel и публикуем эту уже заполненную переменную при помощи экземпляра издателя pub, которого мы создали в начале нашей программы.
def mover(vel):
pub_vel = Twist()
pub_vel.linear.x = vel
pub.publush(pub_vel)
Нам осталось написать функцию, которая будет всем управлять и запустить ее в цикле нашей программы.
Напишем функцию controller(), она будет проверять какое расстояние до стены и останавливать робота если это расстояние меньше целевого. В качестве целевого мы выбрали значение 0.3, но вы можете поставить, то которое вам нужно. В этой функции мы используем глобальную переменную L, которую как вы помните заполняет функция обратного вызова lasercb. Мы будем просто сравнивать значение этой глобальной переменной с 0.3, и в зависимости от результата сравнения передаем в написанную нами ранее функцию mover() или 0 или 0.1 м/с желаемой скорости.
def controller():
if L > 0.3:
mover(0.1)
else:
mover(0)
И финальный момент, мы должны написать цикл, в котором будем вызывать функцию controller(). Используем стандартный цикл rospy, с небольшой задержкой внутри цикла, чтобы не перегружать наш топик /cmd_vel сообщениями.
while not rospy.is_shutdown():
controller()
rospy.sleep(0.2)
Код решения:
import rospy
from geometry_msgs.msg import Twist
from sensors_msgs.msg import LaserScan
rospy.init_node('laser_mover')
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
L = 1
print("Init done")
def lasercb(msg):
print(msg.ranges[0])
if msg.ranges[0] == float('inf'):
pass
else:
global L
L = msg.ranges[0]
rospy.Subscriber("/scan", LaserScan, lasercb)
def mover(vel):
print(vel)
pub_vel = Twist()
pub_vel.linear.x = vel
pub.publush(pub_vel)
def controller():
if L > 0.3:
mover(0.1)
else:
mover(0)
while not rospy.is_shutdown():
controller()
rospy.sleep(0.2)
Примечания: Данный код является учебным и не является эталонным или единственно возможным. К примеру, можно избавиться от цикла и глобальной переменной, и передавать значения расстояния до стены напрямую из функции обратного вызова в функцию controller(). Данный подход возможен, однако он ведет к тому, что потенциально вызов функции обратного вызова будет перенаправлен далее в функцию контроллер, и в случае если контроллер будет выполняться какое-то значимое время, это приведет к утечкам ресурсов, т.к. каждая следующая функция обратного вызова будет вызваться в новом потоке. Или не писать отдельную функцию mover(), а сразу вызывать метод publish из controller(), но мы рекомендуем разделять функции в соответствии с принципом единой ответственности. Кроме того, имеет смысл вынести определение целевого расстояния до стены в отдельную константную переменную и объявить ее в одном месте (например в начале программы), чтобы потом, в случае необходимости мы могли бы ее поменять в этом же одном месте, а не искать ее по всему коду. В целом имеет смысл объяснить ученикам, что так код становится гораздо проще для восприятия человеком. И хотя он не полностью оптимален для робота, но с точки зрения требований к быстродействию он вполне приемлем.
Проверка решения:
Попросите учащихся перенести написанные программы на своих роботов и запустить. Если программа написана правильно, робот должен подъехать к стене и остановиться в 0.4м от нее. Попросите учащихся проконтролировать правильность расстояния "на глаз" по внешней камере полигона, принимая во внимание, что размеры робота 20х20см.
Проконтролируйте работу учащихся в режиме трансляции их экрана через Discord.