3.10.5 Робот для очистки дома с использованием деревьев поведения.

Ранее в этой главе мы использовали SMACH для имитации робота-уборщика дома. Давайте теперь сделаем то же самое, используя деревья поведения. Наш новый сценарий называется clean_house_tree.py и находится в подкаталоге rbx2_tasks/nodes. Программа аналогична той, что была разработана в сценарии patrol_tree.py, но на этот раз мы добавим несколько задач, которые имитируют уборку пылесосом, чистку и мытье пола так же, как мы сделали это с примером SMACH. Помимо этого, мы добавим проверку батареи и поведение подзарядки.

Прежде чем описывать код, давайте попробуем его использовать. Если у вас еще не запущен файл fake_turtlebot.launch, запустите его прямо сейчас:

$ roslaunch rbx2_tasks fake_turtlebot.launch

Напомним, что этот файл запуска также запускает узел move_base, картографический сервер с пустой картой и имитацию аккумуляторного узла со временем выполнения по умолчанию 60 секунд.

Далее, активируйте RViz с конфигурационным файлом nav_tasks.rviz:

$ rosrun rviz rviz -d `rospack find rbx2_tasks`/nav_tasks.rviz

Наконец, запустите сценарий clean_house_tree.py:

$ rosrun rbx2_tasks clean_house_tree.py

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

Общее дерево поведения, реализованное с помощью сценария clean_house_tree.py выглядит примерно так:

В дополнение к основным задачам, показанным выше, нам также требуется несколько узлов условий, таких как «является ли комната уже чистой» и «находимся ли мы в нужном месте?». Необходимость в этих проверках состояния возникает из-за поведения подзарядки робота. Например, предположим, что робот находится в середине мытья кухонного пола, когда его уровень заряда батареи падает ниже порога. Робот выйдет из кухни и направится к док-станции. Как только робот будет заряжен, управление вернется к последней запущенной задаче -мытью кухонного пола - но робота больше нет на кухне. Если мы не проверим это, робот начнет чистить док-станцию! Поэтому, чтобы вернуться на кухню, мы включаем задачу, которая проверяет текущее местоположение робота и сравнивает его с тем, где он должен быть. Если местоположение не совпадает, то робот возвращается в исходную точку.

Хороший способ понять дерево поведения, которое мы создадим — это представить, что вас просят самостоятельно убрать комнату. Если бы вас попросили почистить ванную комнату, вы могли бы использовать следующую стратегию:

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

• Если ванная комната действительно нуждается в уборке, вы должны быть в этой комнате, чтобы очистить ее. Если вы уже находитесь в ванной комнате, то можете приступать к уборке. Если нет, то вам придется прокладывать свой путь через весь дом в ванную комнату.

• Как только вы окажетесь в ванной комнате, проверьте список задач, которые нужно выполнить. После завершения каждой задачи поставьте галочку рядом с ней в списке.

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

• Уборка (селектор);

• Комната чистая (условие);

• Убрать комнату (последовательность);

• Направиться в комнату (селектор);

• Проверить местоположение (условие);

• Переместиться на начальную точку (действие);

• Список дел (итератор);

• Сделать задание 1 (последовательность);

• Проверить местоположение (условие);

• Выполнить задание (действие);

• Обновить список дел (действие);

• Сделать задание 2 (последовательность);

• Проверить местоположение (условие);

• Выполнить задание (действие);

• Обновить список дел (действие);

• И так далее.

Вот как выглядело бы дерево, если бы единственной задачей было пропылесосить гостиную, и мы бы пренебрегли поддеревом проверки батареи:

Мы интерпретируем это дерево следующим образом. Узел верхнего уровня («УБОРКА») является селектором, поэтому если узел условия «КОМНАТА ЧИСТАЯ» возвращает «УСПЕХ», то мы закончили. В противном случае мы переходим к следующей дочерней задаче селектора, «УБРАТЬ КОМНАТУ».

Задача «УБРАТЬ КОМНАТУ» — это последовательность, первой подзадачей которой является задача «НАПРАВИТЬСЯ В КОМНАТУ», которая, в свою очередь, является селектором. Первая подзадача в селекторе «НАПРАВИТЬСЯ В КОМНАТУ» — это условие «ПРОВЕРИТЬ МЕСТОПОЛОЖЕНИЕ». Если эта проверка возвращает «УСПЕХ», то «НАПРАВИТЬСЯ В КОМНАТУ» также возвращает «УСПЕХ», и последовательность «УБРАТЬ КОМНАТУ» может перейти к следующему поведению в своей последовательности, которое является итератором СПИСКА ЗАДАЧ. Если задача «ПРОВЕРИТЬ МЕСТОПОЛОЖЕНИЕ» возвращает «ОШИБКУ», то мы выполняем поведение «ПЕРЕМЕСТИТЬСЯ В НАЧАЛЬНУЮ ТОЧКУ». Это продолжается до тех пор, пока задача «ПРОВЕРИТЬ МЕСТОПОЛОЖЕНИЕ» не вернет «УСПЕХ».

Как только мы оказываемся в целевой комнате, начинается итератор СПИСКА ЗАДАЧ. Сначала мы проверяем, что все еще находимся в правильном месте, затем выполняем каждую задачу в итераторе и обновляем список задач.

Чтобы сделать наш скрипт более читабельным, смоделированные задачи уборки можно найти в файле cleaning_tasks_tree.py в папке rbx2_tasks/src. Затем мы импортируем этот файл в верхней части сценария clean_house_tree.py. Давайте рассмотрим определение одной из этих смоделированных задач:

class VacuumFloor(Task):     
    
    def __init__(self, name, room, timer, *args):         
        super(VacuumFloor, self).__init__(self, name, *args)
        self.name = name         
        self.room = room         
        self.counter = timer         
        self.finished = False
        self.cmd_vel_pub = rospy.Publisher('cmd_vel',Twist)         
        self.cmd_vel_msg = Twist()         
        self.cmd_vel_msg.linear.x = 0.05 

    def run(self):        
        if self.finished:             
            return TaskStatus.SUCCESS         
        else:             
            rospy.loginfo('Vacuuming the floor in the ' + str(self.room))                   
            while self.counter > 0:                 
                self.cmd_vel_pub.publish(self.cmd_vel_msg)
                self.cmd_vel_msg.linear.x *= -1                 
                rospy.loginfo(self.counter)
                self.counter -= 1                 
                rospy.sleep(1)
            return TaskStatus.RUNNING

          self.finished = True
          self.cmd_vel_pub.publish(Twist())
          message = "Finished vacuuming the " + str(self.room) + "!"               
          rospy.loginfo(message)

Класс VacuumFloor расширяет базовый класс задач Task. Поскольку мы хотим перемещать робота взад и вперед, имитируя движение пылесоса, мы создаем ROS издателя, который публикует сообщения о ротации (Twist) в тему cmd_vel. Затем мы переопределяем функцию запуска run(), которая создает желаемое движение. Поскольку функция запуска run() посещается при каждом проходе через дерево поведения, мы возвращаем состояние «ВЫПОЛНЕНИЕ» до тех пор, пока движение не будет завершено, и тогда мы возвращаем состояние «УСПЕХ».

Сценарий clean_house_tree.py похож на программу patrol_tree.py, которую мы уже подробно описывали ранее. Поэтому давайте сосредоточимся только на ключевых различиях.

class BlackBoard():  
   
    def __init__(self):
        # Список складских помещений и задач
        self.task_list = list()

        # Текущее положение робота на карте
        self.robot_position = Point()

Напомним, что некоторые поведенческие деревья используют объект под названием глобальная "черная доска" для отслеживания определенных свойств дерева и окружающего мира. В Python черная доска может быть простым классом с несколькими переменными для хранения данных. В верхней части сценария clean_house_tree.py мы определяем класс BlackBoard(), показанный выше, с переменной list для хранения списка задач и переменной ROS Point для отслеживания текущих координат робота на карте.

black_board = BlackBoard() 

# Создание списка задач, сопоставление комнат с задачами 
black_board.task_list = OrderedDict([
      ('living_room', [Vacuum(room="living_room", timer=5)]),
      ('kitchen', [Mop(room="kitchen", timer=7)]),
      ('bathroom', [Scrub(room="bathroom", timer=9), Mop(room="bathroom",  timer=5)]),
      ('hallway', [Vacuum(room="hallway", timer=5)])
      ])

Далее мы создаем экземпляр класса BlackBoard и делаем упорядоченный список задач уборки с использованием определений задач из файла clean_house_tasks_tree.py в подкаталоге src/rbx2_tasks. Этот список задач будет удобен для перебора всех задач, поставленных перед роботом. Это также означает, что мы можем добавлять или удалять задачи, просто редактируя список здесь, в верхней части сценария.

Суть сценария заключается в создании желаемого дерева поведения из этого списка задач. Вот соответствующий блок в полном объеме.

for room in black_board.task_list.keys(): 
    #Преобразуем название комнаты в верхний регистр для обеспечения согласованности
    ROOM = room.upper() 
     
    #Инициализируем селектор «УБОРКА» для этой комнаты
    CLEANING_ROUTINE[room] = Selector("CLEANING_ROUTINE_" + ROOM) 
         
    #Инициализируем состояние «ПРОВЕРИТЬ ЧИСТОТУ КОМНАТЫ»
    CHECK_ROOM_CLEAN[room] = CheckRoomCleaned(room) 
         
    #Добавляем условие «ПРОВЕРИТЬ ЧИСТОТУ КОМНАТЫ» в селектор «УБОРКА»
    CLEANING_ROUTINE[room].add_child(CHECK_ROOM_CLEAN[room]) 
         
    #Инициализируем последовательность «УБРАТЬ КОМНАТУ» для этой комнаты
    CLEAN_ROOM[room] = Sequence("CLEAN_" + ROOM) 
 
    # Инициализируем селектор «НАПРАВИТЬСЯ В КОМНАТУ» для этой комнаты 
    NAV_ROOM[room] = Selector("NAV_ROOM_" + ROOM) 
          
    #Инициализируем условие «ПРОВЕРИТЬ МЕСТОПОЛОЖЕНИЕ» для этой комнаты.
    CHECK_LOCATION[room] = CheckLocation(room, self.room_locations) 
      
    # Добавляем условие «ПРОВЕРИТЬ МЕСТОПОЛОЖЕНИЕ» в селектор «НАПРАВИТЬСЯ В КОМНАТУ»
    NAV_ROOM[room].add_child(CHECK_LOCATION[room]) 
          
    # Добавляем задачу MOVE_BASE для этой комнаты в селектор «НАПРАВИТЬСЯ В КОМНАТУ» 
    NAV_ROOM[room].add_child(MOVE_BASE[room]) 
          
    # Добавляем селектор «ПРОВЕРИТЬ МЕСТОПОЛОЖЕНИЕ» в последовательность «УБРАТЬ КОМНАТУ»
    CLEAN_ROOM[room].add_child(NAV_ROOM[room]) 
         
    # Инициализируем итератор СПИСКА ДЕЛ для этой комнаты 
    TASK_LIST[room] = Iterator("TASK_LIST_" + ROOM) 
 
    #Добавляем задачи, назначенные этой комнате 
    for task in black_board.task_list[room]: 
        #Инициализируем последовательность «СДЕЛАТЬ ЗАДАНИЕ» для этой комнаты и задачи
        DO_TASK = Sequence("DO_TASK_" + ROOM + "_" + task.name) 
             
        #Добавляем условие «ПРОВЕРИТЬ МЕСТОПОЛОЖЕНИЕ» в последовательность «СДЕЛАТЬ ЗАДАНИЕ» 
        DO_TASK.add_child(CHECK_LOCATION[room]) 
             
        #Добавляем саму задачу в последовательность «СДЕЛАТЬ ЗАДАНИЕ» 
        DO_TASK.add_child(task) 
             
        #Создаем задачу «ОБНОВИТЬ СПИСОК ДЕЛ» для этой комнаты и задачи
        UPDATE_TASK_LIST[room + "_" + task.name] = UpdateTaskList(room, task) 
             
        #Добавляем задачу «ОБНОВИТЬ СПИСОК ДЕЛ» в последовательность «СДЕЛАТЬ ЗАДАНИЕ» 
        DO_TASK.add_child(UPDATE_TASK_LIST[room + "_" + task.name]) 
             
        #Добавляем последовательность «СДЕЛАТЬ ЗАДАНИЕ» в итератор СПИСКА ДЕЛ
        TASK_LIST[room].add_child(DO_TASK) 
             
    #Добавляем итератор СПИСКА ДЕЛ помещения в последовательность «УБРАТЬ КОМНАТУ» 
    CLEAN_ROOM[room].add_child(TASK_LIST[room]) 
             
    #Добавляем последовательность «УБРАТЬ КОМНАТУ» в селектор «УБОРКА» 
    CLEANING_ROUTINE[room].add_child(CLEAN_ROOM[room]) 
     
    #Добавляем функцию «УБОРКА» для этой комнаты в последовательность «УБРАТЬ ДОМ»
    CLEAN_HOUSE.add_child(CLEANING_ROUTINE[room])

Как вы можете видеть, дерево поведения строится путем циклического перебора всех задач в списке дел, который хранится на черной доске. Встроенные комментарии должны прояснить, как мы строим поддерево для каждой комнаты и ее задач. Затем мы добавляем каждое поддерево в общую задачу УБРАТЬ ДОМ.