ROS行為樹實現（Python）

一、行為樹

　　行為樹是一種控制結構，在相關論文資料中通常會與有限狀態機進行比較，並認為其比有限狀態機更適合復雜條件下的控制，目前多用於游戲開發中（主要用於NPC行為），工業領域的應用研究正逐漸增多，主要面向移動機器人/AGV/無人駕駛等等。

　　相關論文資料可以參考：http://kth.diva-portal.org/smash/get/diva2:1078940/FULLTEXT01

 

二、程序實現

　　完整的ROS包地址：https://gitee.com/xjEzekiel/bt_tree

　　主要包括：

　　1、/visio/BT_Basic.vssx，用於繪制行為樹視圖的visio模具；

　　2、/src/bt_tree_lib/bt_tree_lib.py，參考pi_trees（https://github.com/pirobot/pi_trees）實現的行為樹類庫，bt_tree_ros.py用於ROS Action，bt_print.py則用於打印行為樹結構，使用方法均與pi_trees類似；

　　3、/templates/SubTree/SubTree_Temp.py，行為樹子樹模板，主要用於規范子樹變量維護和行為獨立，SubTree_Temp_Test.py用於子樹測試；

　　　　1）/templates/SubTree_Template_Generator.py用於快速生成所需子樹框架；

　　以下着重介紹SubTree_Temp.py：

#!/usr/bin/env python


_prefix = "Temp"


import rospy


Temp_SubTree_Dict = {}


class Temp_SubTreeParamStatus(object):
    PREPARING = 0
    EXECUTING = 1


class Temp_SubTreeParam(object):
    def __init__(self, *args, **kwargs):
        super(Temp_SubTreeParam, self).__init__(*args, **kwargs)
        self.status = Temp_SubTreeParamStatus.PREPARING
        return

    def get_status(self):
        return self.status

    def set_status(self, status):
        self.status = status
        return

    def reset(self):
        self.status = Temp_SubTreeParamStatus.PREPARING
        return

    def set(self, param):
        return


def Temp_init():
    return


def Temp_cancel():
    for key, value in Temp_SubTree_Dict.items():
        value['SubTreeParam'].reset()
        value['SubTree'].reset()
    return


def Temp_get_SubTree(name):
    # Input check.
    if not isinstance(name, str):
        return False, None, None, "Parameter passed is not a string."
    if Temp_SubTree_Dict.has_key(name):
        return False, None, None, "The Temp_SubTree %s has already existed." % name
    
    SubTreeParam = Temp_SubTreeParam()
    SubTree = _Temp_get_SubTree(SubTreeParam)
    
    # Save the SubTree, and return result.
    Temp_SubTree_Dict[name] = {'SubTreeParam': SubTreeParam, 'SubTree': SubTree}
    return True, SubTreeParam, SubTree, ""


def _Temp_get_SubTree(param):
    return LeafNode("Temp")


class TempNode_has_Temp_EXECUTING(LeafNode):
    def __init__(self, name, param, *args, **kwargs):
        super(TempNode_has_Temp_EXECUTING, self).__init__(name, *args, **kwargs)
        self.param = param
        return

    def run(self):
        status = self.param.get_status()
        if status == Temp_SubTreeParamStatus.EXECUTING:
            return NodeStatus.SUCCESS
        return NodeStatus.FAILURE


class TempNode_set_Temp_EXECUTING(LeafNode):
    def __init__(self, name, param, *args, **kwargs):
        super(TempNode_set_Temp_EXECUTING, self).__init__(name, *args, **kwargs)
        self.param = param
        return

    def run(self):
        self.param.set_status(Temp_SubTreeParamStatus.EXECUTING)
        return NodeStatus.SUCCESS


class TempNode_set_Temp_PREPARING(LeafNode):
    def __init__(self, name, param, *args, **kwargs):
        super(TempNode_set_Temp_PREPARING, self).__init__(name, *args, **kwargs)
        self.param = param
        return

    def run(self):
        self.param.set_status(Temp_SubTreeParamStatus.PREPARING)
        return NodeStatus.SUCCESS

 

　　分析：

　　1、Temp_init和Temp_cancel分別用於子樹初始化工作和取消/中斷時的收尾工作；

　　2、Temp_get_SubTree用於獲取子樹參數對象和子樹根節點，無需改動；

　　3、_Temp_get_SubTree用於實際生成子樹（根節點）；

　　4、Temp_SubTree_Dict變量用於保存全部已生成的該類子樹參數對象和子樹根節點，以提供一種全局范圍的變量控制；

　　5、Temp_SubTreeParam作為子樹參數類，統籌控制用於子樹的全部參數並提供子樹的運行狀態；

　　6、TempNode_set_Temp_EXECUTING和TempNode_set_Temp_PREPARING是用於子樹中設置子樹運行狀態的節點類，TempNode_has_Temp_EXECUTING是用於子樹中判斷子樹運行狀態的節點類；

 

下一篇將給出使用模板的例子。