ROS行为树实现（Python）

时间：2018-11-06 15:14:37 阅读：537 评论：0 收藏：0 [点我收藏+]

标签：als asi turn 取消初始 imp test 全局绘制

一、行为树

　　行为树是一种控制结构，在相关论文资料中通常会与有限状态机进行比较，并认为其比有限状态机更适合复杂条件下的控制，目前多用于游戏开发中（主要用于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