Урок 18

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

Подготовка: Данную задачу можно решать в любой комбинации полигона, подойдет и "Офис" и "Дорога". Попросите учащихся непосредственно перед выполнением написанной программы, при помощи управления через веб-интерфейс, отвести робота от зарядки примерно на 20-30 см, развернуть его на 180 градусов (спиной к зарядке) и сбросить одометрию вызвав rosservice call /reset. Попросите техника на полигоне поставить желтый шарик в дальний угол полигона. Если программа написана некорректно, и робот делает что-то несогласованное, попросите учащихся прервать выполнение программы, остановить робота при при помощи управления через веб-интерфейс и вернуть его в первоначальную позицию.

Убедимся что настройки не сбились: Для реализации того, алгоритма который разобран в уроке, необходимо убедиться, что видео-поток с камеры перенаправлен в ROS (По умолчанию он направлен в web-интерфейс). В связи с тем, что два потока не могут работать с камерой одновременно, поэтому необходимо переконфигурировать файл запуска /etc/ros/turtlebro.d/turtlebro.launch таким образом, чтобы по умолчанию видео-поток шел в ROS:

<arg name="run_turtlebro_web" default="false"/>
<arg name="run_ros_camera" default="true"/>

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

Общий алгоритм решения:

• Передать желаемые координаты примерного места расположения шарика в систему автономной навигации
• После приезда робота в точку цели передать управление модулю поиска шарика на изображении
• Получить изображение с камеры робота
• Преобразовать его в HSV
• Наложить цветовую маску - желтого цвета (взять алгоритм из предыдущего урока)
• Применить размытие к полученной маске, для того, чтобы убрать мелкие пиксели совпадающего цвета.
• Применить к полученной маске функцию поиска контуров на изображении
• Описать вокруг найденного контура окружность, для оценки координат центра и радиуса шарика
• Передать найденные параметры окружности в функцию, которая будет управлять движением робота
• Написать функцию управляющую движением робота, которая будет анализировать где на изображении находится шарик и поворачивать робота таким образом, чтобы камера робота смотрела строго на робота. Кроме того робот будет ехать вперед до тех пор пока радиус описанной окружности не будет больше заданного, т.е. робот подъедет к шарику на требуемое расстояние.
• После подъезда на нужное расстояние, остановить модуль поиска шарика и передать координаты возвращения в исходную позицию в систему автономной навигации.
• После достижения исходной позиции, остановить робота и завершить выполнение программы.

Решение: Основные подходы к решению данной задачи мы уже изучили на предыдущих уроках, так что сейчас мы будем применять полученные знания и комплексировать их в единое решение.

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

Начнем с основной программы, именно ее мы будем вызывать из консоли. Создадим новую папку, а в ней файл с расширением py, с тем именем которое вы хотите, к примеру, robot.py

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

Начнем с импорта rospy

import rospy

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

r = RobotMover()
b = BallSearcher()

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

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

Оценку координат, в которые должен приехать робот можно предоставить учащимся самостоятельно. По камере полигона, руководствуясь размерами полигона примерно (1,76м на 2.20 м), определите месторасположения шарика

Далее нам нужна функция search_the_ball(), которая будет осуществлять поиск и подъезд к шарику.

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

Таким образом у нас получилась вот такая программа:

r.move_to(1,-1.8)
b.search_the_ball()
r.move_to(0,0)

Теперь давайте создадим файлы модулей и объявим в них классы, которые мы перечислили.

Сделаем файл mover.py и файл ballsearcher.py, объявим в них соответствующие классы и функции, а затем импортируем их в файл robot.py

class RobotMover():

    def __init__(self):
        pass

    def move_to(self, x, y):
        print("Moving to: ", x, " ", y)

class BallSearcher():

    def __init__(self):
        pass

    def search_the_ball(self):
        print("Searching the ball")
        print("Ball has found")

Теперь файл robot.py будет выглядеть следующим образом.

import rospy
from ballsearcher import BallSearcher
from mover import RobotMover

r = RobotMover()
b = BallSearcher()

r.move_to(1,-1.8)
b.search_the_ball()
r.move_to(0,0)

Давайте перенесем эти файлы в какую-нибудь отдельную папку на роботе и запустим файл robot.py

В его консоли мы должны будем увидеть следующий вывод:

Если у вас все заработало - отлично, скелет программы готов. Давайте наращивать функциональность.

Для начала возьмем программу из урока про передачу целей в экшн-сервер автономной навигации вставим ее в файл mover.py и обернем ее в класс. В итоге вы должны будете получить, что-то такое:

import rospy 
import actionlib 
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

class RobotMover():
    def __init__(self):
        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)

    def move_to(self, x, y):
        print("Moving to: ", x, " ", y)
        self.client.wait_for_server()

        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = x
        goal.target_pose.pose.position.y = y
        goal.target_pose.pose.orientation.w = 1.0

        self.client.send_goal(goal)
        wait = self.client.wait_for_result()
        if not wait:
            rospy.logerr("Action server not available!")
            rospy.signal_shutdown("Action server not available!")
        else:
            return self.client.get_result()

Сохраним все на роботе. Затем откроем еще один терминал на робота через ssh, и в нем запустим навигацию

roslaunch turtlebro_navigation turtlebro_slam_navigation.launch

В первом окне терминала запустим нашу программу robot.py и по камере полигона, убедимся, что робот приехал в точку с координатами x=1, y=2, а затем вернулся в исходную точку 0,0.

Теперь напишем модуль распознавания желтого шарика. Также возьмем уже написанную программу и немного ее модифицируем. Для начала обернем ее в класс BallSearcher и получим примерно следующее:

import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CompressedImage
from geometry_msgs.msg import Twist

class BallSearcher():

    def __init__(self):
        rospy.Subscriber("/front_camera/compressed", CompressedImage, self.cb_video_capture)
        rospy.sleep(1)
        rospy.loginfo("init done")

    yellowLower = (14, 180, 80) # dark
    yellowUpper = (34, 255, 255) # light
    ball_reached = False
    vel = Twist()
    ANGULAR_SPEED = 1.
    LIN_SPEED = 0.10
    DES_BALL_SIZE = 50
    MIN_BALL_SIZE = 10
    pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)


    def cb_video_capture(self, image_msg):
        np_arr = np.frombuffer(image_msg.data, np.uint8)
        self.image_from_ros_camera = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

    def process(self):
        frame = np.copy(self.image_from_ros_camera)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, self.yellowLower, self.yellowUpper)
        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)
        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE)[-2]

        current_data = [0,0,0]
        if len(cnts) > 0:
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            if radius > self.MIN_BALL_SIZE:
                current_data = [x,y,radius]

        return current_data

    def go_to_ball(self, data):
        self.vel.angular.z = -self.ANGULAR_SPEED*((data[0]-320)/320)
        if data[2] < self.DES_BALL_SIZE:
            self.vel.linear.x = self.LIN_SPEED
        else:
            self.vel.linear.x = 0
            self.ball_reached = True
        self.pub.publish(self.vel)

    def search_the_ball(self):
        while not rospy.is_shutdown():
            rospy.sleep(0.1)
            data = self.process()
            if data == [0,0,0] or not self.ball_reached:
                self.go_to_ball(data)
            else:
                self.pub.publish(Twist())
                print("Ball found")
                break

Здесь все аналогично программе по поиску желтого шарика из предыдущего урока, но цикл вызывающий программу анализа перемещен в функцию search_the_ball().

Вот собственно и все, проверим как справились учащиеся.

Проверка решения:

Попросите учащихся сохранить написанную программу на роботе. Включить ноду навигации в соседнем окне терминала.

roslaunch turtlebro_navigation turtlebro_slam_navigation.launch

Перед запуском программы по камере полигона, попросите учащихся убедиться, что желтый шарик находится где-то рядом с точкой в которую должен приехать робот (х=1, y=-1.8). И запускайте! Робот должен приехать в точку (х=1, y=-1.8), там начать вращаться и искать шарик, после нахождения шарика, робот должен подъехать к нему поближе вывести в консоль "Ball found", и вернуться на исходную позицию.

С следующих уроках, мы продолжим тему работы на удаленном полигоне.