3.10.4 Пример патрульного бота с использованием деревьев поведения

Мы уже видели, как с помощью SMACH можно запрограммировать робота на патрулирование ряда путевых точек, одновременно контролируя уровень заряда его батареи и подзаряжая ее при необходимости. Давайте теперь посмотрим, как мы можем сделать то же самое с помощью пакета pi_trees.

Наша тестовая программа называется patrol_tree.py и находится в подкаталоге rbx2_tasks/nodes. Прежде чем взглянуть на код, давайте попробуем его использовать.

Начните с того, что возьмите имитацию робота TurtleBot, пустую карту и имитацию симулятора батареи:

$ roslaunch rbx2_tasks fake_turtlebot.launch

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

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

Наконец, запустите сценарий программы patrol_tree.py:

$ rosrun rbx2_tasks patrol_tree.py

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

Ссылка на источник: patrol_tree.py:

#!/usr/bin/env python 

import rospy 
from std_msgs.msg import Float32 
from geometry_msgs.msg import Twist 
from rbx2_msgs.srv import * 
from pi_trees_ros.pi_trees_ros import * 
from rbx2_tasks.task_setup import * 

class Patrol(): 
    def __init__(self): 
        rospy.init_node("patrol_tree") 
 
        # Устанавливаем функцию выключения (остановить робота)
        rospy.on_shutdown(self.shutdown) 
         
        # Инициализируем ряд параметров и переменных
        setup_task_environment(self) 
 
        # Создаем список, который будет содержать задачи move_base 
        MOVE_BASE_TASKS = list() 
         
        n_waypoints = len(self.waypoints) 
         
        # Создаем простую навигационную задачу действий для каждой путевой точки
        for i in range(n_waypoints + 1): 
            goal = MoveBaseGoal() 
            goal.target_pose.header.frame_id = 'map' 
            goal.target_pose.header.stamp = rospy.Time.now() 
            goal.target_pose.pose = self.waypoints[i % n_waypoints] 
             
            move_base_task = SimpleActionTask("MOVE_BASE_TASK_" + str(i), "move_base", MoveBaseAction, goal) 
             
            MOVE_BASE_TASKS.append(move_base_task) 
         
        # Устанавливаем позицию док-станции
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = 'map'
        goal.target_pose.header.stamp = rospy.Time.now() 
        goal.target_pose.pose = self.docking_station_pose 
         
        # Назначаем позицию док-станции задаче действия move_base
        NAV_DOCK_TASK = SimpleActionTask("NAV_DOC_TASK", "move_base", MoveBaseAction, goal, reset_after=True) 
 
        # Создаем корневой узел
        BEHAVE = Sequence("BEHAVE")
         
        # Создаем селектор "стоять спокойно"
        STAY_HEALTHY = Selector("STAY_HEALTHY") 
         
        # Создаем декоратор цикла патрулирования
        LOOP_PATROL = Loop("LOOP_PATROL", announce=True, iterations=self.n_patrols) 
         
        # Добавляем два поддерева в корневой узел в порядке приоритета
        BEHAVE.add_child(STAY_HEALTHY) 
        BEHAVE.add_child(LOOP_PATROL) 
         
        # Создаем итератор патруля
        PATROL = Iterator("PATROL") 
         
        # Добавляем задачи move_base в задачу патрулирования
        for task in MOVE_BASE_TASKS: 
            PATROL.add_child(task)
   
        # Добавляем патруль в цикл патрулирования
        LOOP_PATROL.add_child(PATROL) 
         
        # Добавляем задачи проверки и подзарядки аккумулятора в задачу "стоять спокойно"
        with STAY_HEALTHY: 
            # Проверка состояния батареи (использование MonitorTask)
            CHECK_BATTERY = MonitorTask("CHECK_BATTERY", "battery_level", Float32, self.check_battery) 
            
            # Задача зарядить робота (использование ServiceTask)
            CHARGE_ROBOT = ServiceTask("CHARGE_ROBOT", "battery_simulator/set_battery_level", SetBatteryLevel, 100, result_cb=self.recharge_cb) 
       
            # Построение последовательности подзарядки с помощью встроенной конструкции
            RECHARGE = Sequence("RECHARGE", [NAV_DOCK_TASK, CHARGE_ROBOT]) 
                 
            # Добавление задачи проверки заряда батареи и подзарядки в селектор «стоять спокойно»
            STAY_HEALTHY.add_child(CHECK_BATTERY) 
            STAY_HEALTHY.add_child(RECHARGE) 
                 
        # Отображаем дерево перед началом выполнения
        print "Patrol Behavior Tree" 
        print_tree(BEHAVE)
             
        # Запускаем дерево
        while not rospy.is_shutdown(): 
            BEHAVE.run() 
            rospy.sleep(0.1) 
             
    def check_battery(self, msg): 
        if msg.data is None: 
            return TaskStatus.RUNNING 
        else: 
            if msg.data < self.low_battery_threshold: 
                rospy.loginfo("LOW BATTERY - level: " + str(int(msg.data))) 
                return TaskStatus.FAILURE 
            else: 
                return TaskStatus.SUCCESS 
     
    def recharge_cb(self, result): 
        rospy.loginfo("BATTERY CHARGED!") 
             
    def shutdown(self): 
        rospy.loginfo("Stopping the robot...") 
        self.move_base.cancel_all_goals() 
        self.cmd_vel_pub.publish(Twist()) 
        rospy.sleep(1) 
 
 if __name__ == '__main__': 
     tree = Patrol()

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

from pi_trees_ros.pi_trees_ros import *

Мы начинаем с импорта библиотеки pi_trees_ros, которая, в свою очередь, импортирует основные классы pi_trees из библиотеки pi_trees_lib. Первый ключевой блок кода включает в себя создание навигационных задач, показанных ниже:

for i in range(n_waypoints + 1): 
    goal = MoveBaseGoal() 
    goal.target_pose.header.frame_id = 'map' 
    goal.target_pose.header.stamp = rospy.Time.now() 
    goal.target_pose.pose = self.waypoints[i % n_waypoints] 
     
    move_base_task = SimpleActionTask("MOVE_BASE_TASK_" + str(i), "move_base", MoveBaseAction, goal) 
     
    MOVE_BASE_TASKS.append(move_base_task) 
 
# Устанавливаем позицию док-станции
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now() 
goal.target_pose.pose = self.docking_station_pose 
 
# Назначаем позицию док-станции задаче действия move_base
NAV_DOCK_TASK = SimpleActionTask("NAV_DOC_TASK", "move_base", MoveBaseAction, goal, reset_after=True) 

Здесь мы видим почти ту же процедуру, что и при использовании с SMACH, хотя теперь мы используем SimpleActionTask из библиотеки pi_trees вместо SimpleActionState из библиотеки SMACH.

Обратите внимание на параметр, называемый reset_after в аргументах функции SimpleActionTask. Мы устанавливаем этот параметр в значение «ЛОЖЬ» для задач move_base, присвоенных путевым точкам, но для задачи стыковки move_base мы устанавливаем его в значение «ИСТИНА». Напомним, что, когда поведение или задача в дереве поведения завершается успехом или неудачей, она сохраняет этот статус бесконечно долго, пока не будет сброшена. Это свойство "памяти" очень важно, потому что в каждом цикле выполнения мы считываем статус каждого узла в дереве, что позволяет нам постоянно проверять узлы состояния, статус которых мог измениться с момента последнего цикла. Однако, если робот только что успешно достиг путевой точки, мы хотим, чтобы этот статус был сохранен на следующем проходе через дерево, чтобы родительский узел продвинул последовательность к следующей путевой точке. С другой стороны, когда дело доходит до подзарядки, нам нужно сбросить навигационную задачу, как только робот состыкуется с док-станцией так, чтобы можно было выполнить этот процесс снова в следующий раз, когда батарея разрядится.

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

# Создаем корневой узел
BEHAVE = Sequence("BEHAVE")
 
# Создаем селектор "стоять спокойно"
STAY_HEALTHY = Selector("STAY_HEALTHY") 
 
# Создаем декоратор цикла патрулирования
LOOP_PATROL = Loop("LOOP_PATROL", announce=True, iterations=self.n_patrols) 
 
# Добавляем два поддерева в корневой узел в порядке приоритета
BEHAVE.add_child(STAY_HEALTHY) 
BEHAVE.add_child(LOOP_PATROL) 

Корневое поведение — это Последовательность, обозначенная надписью «ПОВЕДЕНИЕ», которая будет иметь две дочерние ветви; одна из них начинается с Селектора с надписью «СТОЯТЬ СПОКОЙНО», вторая ветвь с надписью «ЦИКЛ ПАТРУЛИРОВАНИЯ», которая использует декоратор циклов для циклического выполнения задачи патрулирования. Затем мы добавим две дочерние ветви к корневому узлу в порядке, определяющем их приоритет. В этом случае ветвь «СТОЯТЬ СПОКОЙНО» имеет более высокий приоритет, чем «ЦИКЛ ПАТРУЛИРОВАНИЯ».

# Создаем итератор патруля
PATROL = Iterator("PATROL") 
 
# Добавляем задачи move_base в задачу патрулирования
for task in MOVE_BASE_TASKS: 
    PATROL.add_child(task)

# Добавляем патруль в цикл патрулирования
LOOP_PATROL.add_child(PATROL) 

Далее мы займемся остальными патрульными узлами. Сама последовательность патрулирования строится как итератор, называемый патрулем. Затем мы добавляем каждую задачу move_base в итератор. Наконец, мы добавляем весь патруль к задаче «ЦИКЛ ПАТРУЛИРОВАНИЯ».

# Добавляем задачи проверки и подзарядки аккумулятора в задачу "стоять спокойно"
with STAY_HEALTHY: 
    # Проверка состояния батареи (использование MonitorTask)
    CHECK_BATTERY = MonitorTask("CHECK_BATTERY", "battery_level", Float32, self.check_battery) 
    
    # Задача зарядить робота (использование ServiceTask)
    CHARGE_ROBOT = ServiceTask("CHARGE_ROBOT", "battery_simulator/set_battery_level", SetBatteryLevel, 100, result_cb=self.recharge_cb) 

    # Построение последовательности подзарядки с помощью встроенной конструкции
    RECHARGE = Sequence("RECHARGE", [NAV_DOCK_TASK, CHARGE_ROBOT]) 

Теперь перейдем к детализации ветви дерева «СТОЯТЬ СПОКОЙНО». Сначала мы определяем задачу «ПРОВЕРИТЬ БАТАРЕЮ» как MonitorTask в ROS теме battery_level, используя функцию обратного вызова self.check_battery (описано ниже). Далее мы определяем поведение «ЗАРЯДКА РОБОТА» как ServiceTask, который подключается к ROS сервису battery_simulator/set_battery_level и отправляет значение 100 для подзарядки имитационной батареи.

Затем мы строим задачу «ЗАРЯДИТЬСЯ» как Последовательность, дочерними задачами которой являются задачи «ОТПРАВИТЬСЯ К ДОК-СТАНЦИИ» и «ЗАРЯДКА РОБОТА». Обратите внимание на то, как мы использовали встроенный синтаксис, чтобы проиллюстрировать, как мы можем добавлять дочерние задачи в то время, когда мы создаем родительские. Аналогично, мы могли бы использовать три строчки кода:

RECHARGE=Sequence("RECHARGE")             
RECHARGE.add_child(NAV_DOCK_TASK)           
RECHARGE.add_child(CHARGE_ROBOT)

Вы можете использовать любой синтаксис, который предпочитаете.

STAY_HEALTHY.add_child(CHECK_BATTERY) 
STAY_HEALTHY.add_child(RECHARGE) 

Мы завершаем ветвь дерева «СТОЯТЬ СПОКОЙНО», добавляя задачи «ПРОВЕРИТЬ БАТАРЕЮ» и «ЗАРЯДИТЬСЯ». Еще раз обратите внимание, что порядок очень важен, так как мы хотим сначала проверить батарею, чтобы понять, нужно ли нам ее заряжать.

# Отображаем дерево перед началом выполнения
print "Patrol Behavior Tree" 
print_tree(BEHAVE)
             
# Запускаем дерево
while not rospy.is_shutdown(): 
    BEHAVE.run()
    rospy.sleep(0.1) 

Перед началом выполнения мы используем функцию print_tree() из библиотеки pi_trees для отображения представления дерева поведения на экране. Само дерево выполняется вызовом функции run() на корневом узле. Функция run() делает один проход через узлы дерева, поэтому нам нужно поместить ее в цикл.

В конце, мы имеем функцию обратного вызова для проверки батареи - check_battery()

def check_battery(self, msg): 
    if msg.data is None: 
        return TaskStatus.RUNNING 
    else: 
        if msg.data < self.low_battery_threshold: 
            rospy.loginfo("LOW BATTERY - level: " + str(int(msg.data))) 
            return TaskStatus.FAILURE 
        else: 
            return TaskStatus.SUCCESS 

Напомним, что эта функция была назначена задаче «ПРОВЕРИТЬ БАТАРЕЮ» в MonitorTask, которая отслеживает тему с уровнем батареи. Поэтому мы проверяем уровень заряда батареи по параметру low_battery_threshold. Если уровень ниже порогового значения, мы возвращаем статус задачи как «ОШИБКА». В противном случае мы возвращаем «УСПЕХ». Поскольку задача «ПРОВЕРИТЬ БАТАРЕЮ» является самой приоритетной задачей в селекторе «СТОЯТЬ СПОКОЙНО», то если она возвращает ошибку, селектор переходит к своей следующей подзадаче, которой является задача «ЗАРЯДИТЬСЯ».

Сценарий patrol_tree.py иллюстрирует важное свойство деревьев поведения, которое помогает отличить их от обычных иерархических машин состояний, таких как SMACH. Вы заметите, что после подзарядки робот продолжает свое патрулирование там, где он остановился, хотя нигде в сценарии мы явно не сохранили последнюю достигнутую точку пути. Помните, что в примере SMACH (patrol_smach_concurrence.py), мы должны были сохранить последнее состояние непосредственно перед подзарядкой, чтобы робот знал, где продолжить работу после зарядки. Деревья поведения по своей сути хранят свое состояние в силу свойства состояния каждого узла. В частности, если робот находится в движении к путевой точке, то навигационная задача, выполняющая работу по перемещению робота, имеет статус запущенной. Если после этого робот будет перенаправлен на док-станцию для подзарядки, то статус ранее активного навигационного статуса все еще будет работать. Это означает, что, когда робот полностью заряжен и задача «ПРОВЕРИТЬ БАТАРЕЮ» вернет «УСПЕХ», управление автоматически возвращается к запущенному навигационному узлу.