diff --git a/dataset/input.txt b/dataset/input.txt deleted file mode 100644 index 536fa42..0000000 --- a/dataset/input.txt +++ /dev/null @@ -1 +0,0 @@ -{pre: 1, 2, 3}{action: moveTo(s)}{add: 4}{del: 2, 3} \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index 7711d13..85bbf47 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,3 +6,4 @@ gym==0.21.0 grpcio==1.53.0 requests urllib3 +tabulate \ No newline at end of file diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py new file mode 100644 index 0000000..1c6433a --- /dev/null +++ b/robowaiter/behavior_lib/_base/Act.py @@ -0,0 +1,11 @@ +from robowaiter.behavior_lib._base.Behavior import Bahavior + +class Act(Bahavior): + def __init__(self,*args): + super().__init__(*args) + + def get_conds(self): + pre = set() + add = set() + de = set() + return pre, add, de \ No newline at end of file diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py new file mode 100644 index 0000000..b5ac2db --- /dev/null +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -0,0 +1,54 @@ +import py_trees as ptree +from typing import Any +import enum + +class Status(enum.Enum): + """An enumerator representing the status of a behavior.""" + + SUCCESS = ptree.common.Status.SUCCESS + """Behaviour check has passed, or execution of its action has finished with a successful result.""" + FAILURE = ptree.common.Status.FAILURE + """Behaviour check has failed, or execution of its action finished with a failed result.""" + RUNNING = ptree.common.Status.RUNNING + """Behaviour is in the middle of executing some action, result still pending.""" + INVALID = ptree.common.Status.INVALID + """Behaviour is uninitialised and/or in an inactive state, i.e. not currently being ticked.""" + + +# _base Behavior +class Bahavior(ptree.behaviour.Behaviour): + num_params = 0 + valid_params=''' + None + ''' + scene = None + + def __init__(self,*args): + name = self.__class__.__name__ + if len(args)>0: + name = f'{name}({",".join(list(args))})' + self.name = name + self.args = args + super().__init__(self.name) + + def _update(self) -> ptree.common.Status: + print("this is just a _base behavior node.") + return Status.INVALID + + # let behavior node interact with the scene + def set_scene(self, scene): + self.scene = scene + + def setup(self, **kwargs: Any) -> None: + return super().setup(**kwargs) + + def initialise(self) -> None: + return super().initialise() + + def update(self) -> Status: + re = self._update() + print(f"{self.__class__.__name__}: {re.value}") + return re + + def terminate(self, new_status: Status) -> None: + return super().terminate(new_status) diff --git a/robowaiter/behavior_lib/_base/Cond.py b/robowaiter/behavior_lib/_base/Cond.py new file mode 100644 index 0000000..15147f6 --- /dev/null +++ b/robowaiter/behavior_lib/_base/Cond.py @@ -0,0 +1,15 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Behavior import Bahavior, Status + +class Cond(Bahavior): + def __init__(self): + super().__init__() + + + def _update(self) -> ptree.common.Status: + # if self.scene.status? + if self.scene.state['chat_list'] == []: + return Status.FAILURE + else: + return Status.SUCCESS diff --git a/robowaiter/behavior_lib/_base/__init__.py b/robowaiter/behavior_lib/_base/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robowaiter/behavior_lib/act/DealChat.py b/robowaiter/behavior_lib/act/DealChat.py new file mode 100644 index 0000000..ba6ed8b --- /dev/null +++ b/robowaiter/behavior_lib/act/DealChat.py @@ -0,0 +1,17 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Behavior import Bahavior +from robowaiter.llm_client.ask_llm import ask_llm + +class DealChat(Bahavior): + def __init__(self): + super().__init__() + + def _update(self) -> ptree.common.Status: + # if self.scene.status? + chat = self.scene.state['chat_list'].pop() + answer = ask_llm(chat) + print(f"机器人回答:{answer}") + self.scene.chat_bubble(f"机器人回答:{answer}") + + return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py similarity index 100% rename from robowaiter/behavior_lib/MoveTo.py rename to robowaiter/behavior_lib/act/MoveTo.py diff --git a/robowaiter/behavior_lib/act/__init__.py b/robowaiter/behavior_lib/act/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robowaiter/behavior_lib/cond/At.py b/robowaiter/behavior_lib/cond/At.py new file mode 100644 index 0000000..ec97bb2 --- /dev/null +++ b/robowaiter/behavior_lib/cond/At.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Act import Act + +class At(Act): + num_params = 2 + valid_params = ''' + Coffee, Table + ''' + + def __init__(self,*args): + super().__init__(*args) + + + + def _update(self) -> ptree.common.Status: + # if self.scene.status? + if self.scene.state['chat_list'] == []: + return ptree.common.Status.FAILURE + else: + return ptree.common.Status.SUCCESS diff --git a/robowaiter/behavior_lib/cond/Chatting.py b/robowaiter/behavior_lib/cond/Chatting.py new file mode 100644 index 0000000..63bab10 --- /dev/null +++ b/robowaiter/behavior_lib/cond/Chatting.py @@ -0,0 +1,15 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Behavior import Bahavior + +class Chatting(Bahavior): + def __init__(self): + super().__init__() + + + def _update(self) -> ptree.common.Status: + # if self.scene.status? + if self.scene.state['chat_list'] == []: + return ptree.common.Status.FAILURE + else: + return ptree.common.Status.SUCCESS diff --git a/robowaiter/behavior_lib/cond/__init__.py b/robowaiter/behavior_lib/cond/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/opt_bt_expansion/BehaviorTree.py b/robowaiter/behavior_tree/obtea/BehaviorTree.py similarity index 100% rename from opt_bt_expansion/BehaviorTree.py rename to robowaiter/behavior_tree/obtea/BehaviorTree.py diff --git a/robowaiter/behavior_tree/obtea/MakeCoffee.ptml b/robowaiter/behavior_tree/obtea/MakeCoffee.ptml new file mode 100644 index 0000000..d2f0b28 --- /dev/null +++ b/robowaiter/behavior_tree/obtea/MakeCoffee.ptml @@ -0,0 +1,59 @@ +selector{ +cond At(Table,Coffee) +selector{ +cond Holding(Coffee), At(Robot,Table) +act PutDown(Table,Coffee) +} +selector{ +cond NotHolding, At(Robot,Coffee), At(Robot,Table) +act PickUp(Coffee) +} +selector{ +cond Holding(Coffee), Available(Table) +act MoveTo(Table) +} +selector{ +cond Holding(VacuumCup), At(Robot,Coffee), At(Robot,Table) +act PutDown(Table,VacuumCup) +} +selector{ +cond NotHolding, At(Robot,CoffeeMachine), At(Robot,Table) +act OpCoffeeMachine +} +selector{ +cond NotHolding, Available(Table), At(Robot,Coffee) +act PickUp(Coffee) +} +selector{ +cond Holding(VacuumCup), At(Robot,Table), At(Robot,CoffeeMachine) +act PutDown(Table,VacuumCup) +} +selector{ +cond NotHolding, Available(Coffee), Available(Table) +act MoveTo(Coffee) +} +selector{ +cond NotHolding, Available(Table), At(Robot,CoffeeMachine) +act OpCoffeeMachine +} +selector{ +cond Holding(VacuumCup), Available(Coffee), Available(Table), At(Robot,Table) +act PutDown(Table,VacuumCup) +} +selector{ +cond Available(CoffeeMachine), NotHolding, Available(Table) +act MoveTo(CoffeeMachine) +} +selector{ +cond Holding(VacuumCup), Available(Coffee), Available(Table) +act MoveTo(Table) +} +selector{ +cond Available(CoffeeMachine), Holding(VacuumCup), Available(Table), At(Robot,Table) +act PutDown(Table,VacuumCup) +} +selector{ +cond Available(CoffeeMachine), Holding(VacuumCup), Available(Table) +act MoveTo(Table) +} +} diff --git a/opt_bt_expansion/OptimalBTExpansionAlgorithm.py b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py similarity index 100% rename from opt_bt_expansion/OptimalBTExpansionAlgorithm.py rename to robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py diff --git a/opt_bt_expansion/README.assets/image-20231103191141047.png b/robowaiter/behavior_tree/obtea/README.assets/image-20231103191141047.png similarity index 100% rename from opt_bt_expansion/README.assets/image-20231103191141047.png rename to robowaiter/behavior_tree/obtea/README.assets/image-20231103191141047.png diff --git a/opt_bt_expansion/README.md b/robowaiter/behavior_tree/obtea/README.md similarity index 100% rename from opt_bt_expansion/README.md rename to robowaiter/behavior_tree/obtea/README.md diff --git a/robowaiter/behavior_tree/obtea/__init__.py b/robowaiter/behavior_tree/obtea/__init__.py new file mode 100644 index 0000000..fe4de55 --- /dev/null +++ b/robowaiter/behavior_tree/obtea/__init__.py @@ -0,0 +1,3 @@ + +from robowaiter.robot.robot import Robot +from robowaiter.scene import task_map \ No newline at end of file diff --git a/opt_bt_expansion/examples.py b/robowaiter/behavior_tree/obtea/examples.py similarity index 98% rename from opt_bt_expansion/examples.py rename to robowaiter/behavior_tree/obtea/examples.py index 743f554..019af9b 100644 --- a/opt_bt_expansion/examples.py +++ b/robowaiter/behavior_tree/obtea/examples.py @@ -1,5 +1,5 @@ -from opt_bt_expansion.OptimalBTExpansionAlgorithm import Action +from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action diff --git a/robowaiter/behavior_tree/obtea/opt_bt_exp_main.py b/robowaiter/behavior_tree/obtea/opt_bt_exp_main.py new file mode 100644 index 0000000..b8ef90a --- /dev/null +++ b/robowaiter/behavior_tree/obtea/opt_bt_exp_main.py @@ -0,0 +1,121 @@ +from robowaiter.behavior_tree.obtea.BehaviorTree import Leaf,ControlBT # 行为结点类:叶子结点和非叶子节点 +from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm,state_transition # 调用最优行为树扩展算法 +from robowaiter.behavior_tree.obtea.tools import print_action_data_table,BTTest +from robowaiter.behavior_tree.obtea.examples import MoveBtoB_num,MoveBtoB,Cond2BelongsToCond3 # 导入三个例子 +from robowaiter.behavior_tree.obtea.examples import * + + +# 封装好的主接口 +class BTOptExpInterface: + def __init__(self, action_list): + """ + Initialize the BTOptExpansion with a list of actions. + :param action_list: A list of actions to be used in the behavior tree. + """ + # self.actions = [] + # for act in action_list: + # a = Action(name=act.name) + # a.pre=act['pre'] + # a.add=act['add'] + # a.del_set= act['del_set'] + # a.cost = 1 + # self.actions.append(a) + self.actions = action_list + self.has_processed = False + + def process(self, goal): + """ + Process the input sets and return a string result. + :param input_set: The set of goal states and the set of initial states. + :return: A PTML string representing the outcome of the behavior tree. + """ + self.goal = goal + self.algo = OptBTExpAlgorithm(verbose=False) + self.algo.clear() + self.algo.run_algorithm(self.goal, self.actions) # 调用算法得到行为树保存至 algo.bt + self.ptml_string = self.algo.get_ptml() + self.has_processed = True + # algo.print_solution() # print behavior tree + + return self.ptml_string + + # 方法一:查找所有初始状态是否包含当前状态 + def find_all_leaf_states_contain_start(self,start): + if not self.has_processed: + raise RuntimeError("The process method must be called before find_all_leaf_states_contain_start!") + # 返回所有能到达目标状态的初始状态 + state_leafs = self.algo.get_all_state_leafs() + for state in state_leafs: + if start >= state: + return True + return False + + # 方法二:模拟跑一遍行为树,看 start 能够通过执行一系列动作到达 goal + def run_bt_from_start(self,goal,start): + if not self.has_processed: + raise RuntimeError("The process method must be called before run_bt_from_start!") + # 检查是否能到达目标 + right_bt = True + state = start + steps = 0 + val, obj = self.algo.bt.tick(state) + while val != 'success' and val != 'failure': + state = state_transition(state, obj) + val, obj = self.algo.bt.tick(state) + if (val == 'failure'): + # print("bt fails at step", steps) + right_bt = False + steps += 1 + if not goal <= state: + # print("wrong solution", steps) + right_bt = False + else: + pass + # print("right solution", steps) + return right_bt + + + + +if __name__ == '__main__' : + + # todo: Example Cafe + # todo: Define goal, start, actions + actions=[ + Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), + Action(name='PutDown(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Robot,Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), + + Action(name='PickUp(Coffee)', pre={'NotHolding','At(Robot,Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), + + Action(name='MoveTo(Table)', pre={'Available(Table)'}, add={'At(Robot,Table)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,CoffeeMachine)'}, cost=1), + Action(name='MoveTo(Coffee)', pre={'Available(Coffee)'}, add={'At(Robot,Coffee)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Table)','At(Robot,CoffeeMachine)'}, cost=1), + Action(name='MoveTo(CoffeeMachine)', pre={'Available(CoffeeMachine)'}, add={'At(Robot,CoffeeMachine)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,Table)'}, cost=1), + + Action(name='OpCoffeeMachine', pre={'At(Robot,CoffeeMachine)','NotHolding'}, add={'Available(Coffee)','At(Robot,Coffee)'}, del_set=set(), cost=1), + ] + algo = BTOptExpInterface(actions) + + + goal = {'At(Table,Coffee)'} + ptml_string = algo.process(goal) + print(ptml_string) + + file_name = "MakeCoffee" + with open(f'./{file_name}.ptml', 'w') as file: + file.write(ptml_string) + + + # 判断初始状态能否到达目标状态 + start = {'At(Robot,Bar)', 'Holding(VacuumCup)', 'Available(Table)', 'Available(CoffeeMachine)','Available(FrontDesk)'} + # 方法一:算法返回所有可能的初始状态,在里面看看有没有对应的初始状态 + right_bt = algo.find_all_leaf_states_contain_start(start) + if not right_bt: + print("ERROR1: The current state cannot reach the goal state!") + else: + print("Right1: The current state can reach the goal state!") + # 方法二:预先跑一边行为树,看能否到达目标状态 + right_bt2 = algo.run_bt_from_start(goal,start) + if not right_bt2: + print("ERROR2: The current state cannot reach the goal state!") + else: + print("Right2: The current state can reach the goal state!") diff --git a/opt_bt_expansion/tools.py b/robowaiter/behavior_tree/obtea/tools.py similarity index 98% rename from opt_bt_expansion/tools.py rename to robowaiter/behavior_tree/obtea/tools.py index cf84229..1af057d 100644 --- a/opt_bt_expansion/tools.py +++ b/robowaiter/behavior_tree/obtea/tools.py @@ -3,7 +3,7 @@ from tabulate import tabulate import numpy as np import random -from opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm +from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm import time diff --git a/robowaiter/behavior_tree/ptml/compile.sh b/robowaiter/behavior_tree/ptml/compile.sh new file mode 100644 index 0000000..9b84246 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/compile.sh @@ -0,0 +1,2 @@ +cd ./robowaiter/behavior_tree/ptml +antlr4 -Dlanguage=Python3 ptml.g4 \ No newline at end of file diff --git a/robowaiter/behavior_tree/ptml/ptml.g4 b/robowaiter/behavior_tree/ptml/ptml.g4 index 0a545a9..14a2857 100644 --- a/robowaiter/behavior_tree/ptml/ptml.g4 +++ b/robowaiter/behavior_tree/ptml/ptml.g4 @@ -4,13 +4,13 @@ root : tree+ EOF; tree : internal_node '{' (action_sign|tree)+ '}' ; internal_node : 'sequence' | 'selector' | 'parallel' Integer ; -action_sign : ('task'|'cond') Names '(' action_parm? ')'; -action_parm : (Integer|Float|boolean) (',' (Integer|Float|boolean))* ; +action_sign : ('act'|'cond') String '(' action_parm? ')'; +action_parm : (Integer|Float|boolean|String) (',' (Integer|Float|boolean|String))* ; // var_decls : var_type Names ; // var_type : 'int' | 'float' | 'bool' | 'string' ; boolean : 'True' | 'False' ; -Names : [a-zA-Z_][a-zA-Z_0-9]* ; +String : [a-zA-Z_][a-zA-Z_0-9]* ; Integer : '-'?[1-9][0-9]* | '0' ; Float : [0-9]+'.'[0-9]* | '.'[0-9]+ ; diff --git a/robowaiter/behavior_tree/ptml/ptml.interp b/robowaiter/behavior_tree/ptml/ptml.interp index a96ef7d..b0e3b01 100644 --- a/robowaiter/behavior_tree/ptml/ptml.interp +++ b/robowaiter/behavior_tree/ptml/ptml.interp @@ -5,7 +5,7 @@ null 'sequence' 'selector' 'parallel' -'task' +'act' 'cond' '(' ')' @@ -32,7 +32,7 @@ null null null null -Names +String Integer Float LINE_COMMENT @@ -48,4 +48,4 @@ boolean atn: -[4, 1, 17, 62, 2, 0, 7, 0, 2, 1, 7, 1, 2, 2, 7, 2, 2, 3, 7, 3, 2, 4, 7, 4, 2, 5, 7, 5, 1, 0, 4, 0, 14, 8, 0, 11, 0, 12, 0, 15, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 4, 1, 24, 8, 1, 11, 1, 12, 1, 25, 1, 1, 1, 1, 1, 2, 1, 2, 1, 2, 1, 2, 3, 2, 34, 8, 2, 1, 3, 1, 3, 1, 3, 1, 3, 3, 3, 40, 8, 3, 1, 3, 1, 3, 1, 4, 1, 4, 1, 4, 3, 4, 47, 8, 4, 1, 4, 1, 4, 1, 4, 1, 4, 3, 4, 53, 8, 4, 5, 4, 55, 8, 4, 10, 4, 12, 4, 58, 9, 4, 1, 5, 1, 5, 1, 5, 0, 0, 6, 0, 2, 4, 6, 8, 10, 0, 2, 1, 0, 6, 7, 1, 0, 11, 12, 66, 0, 13, 1, 0, 0, 0, 2, 19, 1, 0, 0, 0, 4, 33, 1, 0, 0, 0, 6, 35, 1, 0, 0, 0, 8, 46, 1, 0, 0, 0, 10, 59, 1, 0, 0, 0, 12, 14, 3, 2, 1, 0, 13, 12, 1, 0, 0, 0, 14, 15, 1, 0, 0, 0, 15, 13, 1, 0, 0, 0, 15, 16, 1, 0, 0, 0, 16, 17, 1, 0, 0, 0, 17, 18, 5, 0, 0, 1, 18, 1, 1, 0, 0, 0, 19, 20, 3, 4, 2, 0, 20, 23, 5, 1, 0, 0, 21, 24, 3, 6, 3, 0, 22, 24, 3, 2, 1, 0, 23, 21, 1, 0, 0, 0, 23, 22, 1, 0, 0, 0, 24, 25, 1, 0, 0, 0, 25, 23, 1, 0, 0, 0, 25, 26, 1, 0, 0, 0, 26, 27, 1, 0, 0, 0, 27, 28, 5, 2, 0, 0, 28, 3, 1, 0, 0, 0, 29, 34, 5, 3, 0, 0, 30, 34, 5, 4, 0, 0, 31, 32, 5, 5, 0, 0, 32, 34, 5, 14, 0, 0, 33, 29, 1, 0, 0, 0, 33, 30, 1, 0, 0, 0, 33, 31, 1, 0, 0, 0, 34, 5, 1, 0, 0, 0, 35, 36, 7, 0, 0, 0, 36, 37, 5, 13, 0, 0, 37, 39, 5, 8, 0, 0, 38, 40, 3, 8, 4, 0, 39, 38, 1, 0, 0, 0, 39, 40, 1, 0, 0, 0, 40, 41, 1, 0, 0, 0, 41, 42, 5, 9, 0, 0, 42, 7, 1, 0, 0, 0, 43, 47, 5, 14, 0, 0, 44, 47, 5, 15, 0, 0, 45, 47, 3, 10, 5, 0, 46, 43, 1, 0, 0, 0, 46, 44, 1, 0, 0, 0, 46, 45, 1, 0, 0, 0, 47, 56, 1, 0, 0, 0, 48, 52, 5, 10, 0, 0, 49, 53, 5, 14, 0, 0, 50, 53, 5, 15, 0, 0, 51, 53, 3, 10, 5, 0, 52, 49, 1, 0, 0, 0, 52, 50, 1, 0, 0, 0, 52, 51, 1, 0, 0, 0, 53, 55, 1, 0, 0, 0, 54, 48, 1, 0, 0, 0, 55, 58, 1, 0, 0, 0, 56, 54, 1, 0, 0, 0, 56, 57, 1, 0, 0, 0, 57, 9, 1, 0, 0, 0, 58, 56, 1, 0, 0, 0, 59, 60, 7, 1, 0, 0, 60, 11, 1, 0, 0, 0, 8, 15, 23, 25, 33, 39, 46, 52, 56] \ No newline at end of file +[4, 1, 17, 64, 2, 0, 7, 0, 2, 1, 7, 1, 2, 2, 7, 2, 2, 3, 7, 3, 2, 4, 7, 4, 2, 5, 7, 5, 1, 0, 4, 0, 14, 8, 0, 11, 0, 12, 0, 15, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 4, 1, 24, 8, 1, 11, 1, 12, 1, 25, 1, 1, 1, 1, 1, 2, 1, 2, 1, 2, 1, 2, 3, 2, 34, 8, 2, 1, 3, 1, 3, 1, 3, 1, 3, 3, 3, 40, 8, 3, 1, 3, 1, 3, 1, 4, 1, 4, 1, 4, 1, 4, 3, 4, 48, 8, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 3, 4, 55, 8, 4, 5, 4, 57, 8, 4, 10, 4, 12, 4, 60, 9, 4, 1, 5, 1, 5, 1, 5, 0, 0, 6, 0, 2, 4, 6, 8, 10, 0, 2, 1, 0, 6, 7, 1, 0, 11, 12, 70, 0, 13, 1, 0, 0, 0, 2, 19, 1, 0, 0, 0, 4, 33, 1, 0, 0, 0, 6, 35, 1, 0, 0, 0, 8, 47, 1, 0, 0, 0, 10, 61, 1, 0, 0, 0, 12, 14, 3, 2, 1, 0, 13, 12, 1, 0, 0, 0, 14, 15, 1, 0, 0, 0, 15, 13, 1, 0, 0, 0, 15, 16, 1, 0, 0, 0, 16, 17, 1, 0, 0, 0, 17, 18, 5, 0, 0, 1, 18, 1, 1, 0, 0, 0, 19, 20, 3, 4, 2, 0, 20, 23, 5, 1, 0, 0, 21, 24, 3, 6, 3, 0, 22, 24, 3, 2, 1, 0, 23, 21, 1, 0, 0, 0, 23, 22, 1, 0, 0, 0, 24, 25, 1, 0, 0, 0, 25, 23, 1, 0, 0, 0, 25, 26, 1, 0, 0, 0, 26, 27, 1, 0, 0, 0, 27, 28, 5, 2, 0, 0, 28, 3, 1, 0, 0, 0, 29, 34, 5, 3, 0, 0, 30, 34, 5, 4, 0, 0, 31, 32, 5, 5, 0, 0, 32, 34, 5, 14, 0, 0, 33, 29, 1, 0, 0, 0, 33, 30, 1, 0, 0, 0, 33, 31, 1, 0, 0, 0, 34, 5, 1, 0, 0, 0, 35, 36, 7, 0, 0, 0, 36, 37, 5, 13, 0, 0, 37, 39, 5, 8, 0, 0, 38, 40, 3, 8, 4, 0, 39, 38, 1, 0, 0, 0, 39, 40, 1, 0, 0, 0, 40, 41, 1, 0, 0, 0, 41, 42, 5, 9, 0, 0, 42, 7, 1, 0, 0, 0, 43, 48, 5, 14, 0, 0, 44, 48, 5, 15, 0, 0, 45, 48, 3, 10, 5, 0, 46, 48, 5, 13, 0, 0, 47, 43, 1, 0, 0, 0, 47, 44, 1, 0, 0, 0, 47, 45, 1, 0, 0, 0, 47, 46, 1, 0, 0, 0, 48, 58, 1, 0, 0, 0, 49, 54, 5, 10, 0, 0, 50, 55, 5, 14, 0, 0, 51, 55, 5, 15, 0, 0, 52, 55, 3, 10, 5, 0, 53, 55, 5, 13, 0, 0, 54, 50, 1, 0, 0, 0, 54, 51, 1, 0, 0, 0, 54, 52, 1, 0, 0, 0, 54, 53, 1, 0, 0, 0, 55, 57, 1, 0, 0, 0, 56, 49, 1, 0, 0, 0, 57, 60, 1, 0, 0, 0, 58, 56, 1, 0, 0, 0, 58, 59, 1, 0, 0, 0, 59, 9, 1, 0, 0, 0, 60, 58, 1, 0, 0, 0, 61, 62, 7, 1, 0, 0, 62, 11, 1, 0, 0, 0, 8, 15, 23, 25, 33, 39, 47, 54, 58] \ No newline at end of file diff --git a/robowaiter/behavior_tree/ptml/ptml.tokens b/robowaiter/behavior_tree/ptml/ptml.tokens index 1f7a813..6f604be 100644 --- a/robowaiter/behavior_tree/ptml/ptml.tokens +++ b/robowaiter/behavior_tree/ptml/ptml.tokens @@ -10,7 +10,7 @@ T__8=9 T__9=10 T__10=11 T__11=12 -Names=13 +String=13 Integer=14 Float=15 LINE_COMMENT=16 @@ -20,7 +20,7 @@ WS=17 'sequence'=3 'selector'=4 'parallel'=5 -'task'=6 +'act'=6 'cond'=7 '('=8 ')'=9 diff --git a/robowaiter/behavior_tree/ptml/ptmlCompiler.py b/robowaiter/behavior_tree/ptml/ptmlCompiler.py index 548369d..47a68f7 100644 --- a/robowaiter/behavior_tree/ptml/ptmlCompiler.py +++ b/robowaiter/behavior_tree/ptml/ptmlCompiler.py @@ -1,4 +1,5 @@ import os +import sys from antlr4 import * if "." in __name__: @@ -40,6 +41,11 @@ def load(scene, ptml_path: str, behaviour_lib_path: str): tree = parser.root() walker = ParseTreeWalker() + + + sys.path.append(os.path.join(behaviour_lib_path,"cond")) + sys.path.append(os.path.join(behaviour_lib_path,"act")) + ptml = ptmlTranslator(scene, behaviour_lib_path) # listener mode walker.walk(ptml, tree) diff --git a/robowaiter/behavior_tree/ptml/ptmlLexer.interp b/robowaiter/behavior_tree/ptml/ptmlLexer.interp index 823b753..a624c96 100644 --- a/robowaiter/behavior_tree/ptml/ptmlLexer.interp +++ b/robowaiter/behavior_tree/ptml/ptmlLexer.interp @@ -5,7 +5,7 @@ null 'sequence' 'selector' 'parallel' -'task' +'act' 'cond' '(' ')' @@ -32,7 +32,7 @@ null null null null -Names +String Integer Float LINE_COMMENT @@ -51,7 +51,7 @@ T__8 T__9 T__10 T__11 -Names +String Integer Float LINE_COMMENT @@ -65,4 +65,4 @@ mode names: DEFAULT_MODE atn: -[4, 0, 17, 156, 6, -1, 2, 0, 7, 0, 2, 1, 7, 1, 2, 2, 7, 2, 2, 3, 7, 3, 2, 4, 7, 4, 2, 5, 7, 5, 2, 6, 7, 6, 2, 7, 7, 7, 2, 8, 7, 8, 2, 9, 7, 9, 2, 10, 7, 10, 2, 11, 7, 11, 2, 12, 7, 12, 2, 13, 7, 13, 2, 14, 7, 14, 2, 15, 7, 15, 2, 16, 7, 16, 1, 0, 1, 0, 1, 1, 1, 1, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 5, 1, 5, 1, 5, 1, 5, 1, 5, 1, 6, 1, 6, 1, 6, 1, 6, 1, 6, 1, 7, 1, 7, 1, 8, 1, 8, 1, 9, 1, 9, 1, 10, 1, 10, 1, 10, 1, 10, 1, 10, 1, 11, 1, 11, 1, 11, 1, 11, 1, 11, 1, 11, 1, 12, 1, 12, 5, 12, 96, 8, 12, 10, 12, 12, 12, 99, 9, 12, 1, 13, 3, 13, 102, 8, 13, 1, 13, 1, 13, 5, 13, 106, 8, 13, 10, 13, 12, 13, 109, 9, 13, 1, 13, 3, 13, 112, 8, 13, 1, 14, 4, 14, 115, 8, 14, 11, 14, 12, 14, 116, 1, 14, 1, 14, 5, 14, 121, 8, 14, 10, 14, 12, 14, 124, 9, 14, 1, 14, 1, 14, 4, 14, 128, 8, 14, 11, 14, 12, 14, 129, 3, 14, 132, 8, 14, 1, 15, 1, 15, 1, 15, 1, 15, 5, 15, 138, 8, 15, 10, 15, 12, 15, 141, 9, 15, 1, 15, 3, 15, 144, 8, 15, 1, 15, 1, 15, 1, 15, 1, 15, 1, 16, 4, 16, 151, 8, 16, 11, 16, 12, 16, 152, 1, 16, 1, 16, 1, 139, 0, 17, 1, 1, 3, 2, 5, 3, 7, 4, 9, 5, 11, 6, 13, 7, 15, 8, 17, 9, 19, 10, 21, 11, 23, 12, 25, 13, 27, 14, 29, 15, 31, 16, 33, 17, 1, 0, 5, 3, 0, 65, 90, 95, 95, 97, 122, 4, 0, 48, 57, 65, 90, 95, 95, 97, 122, 1, 0, 49, 57, 1, 0, 48, 57, 3, 0, 9, 10, 12, 13, 32, 32, 166, 0, 1, 1, 0, 0, 0, 0, 3, 1, 0, 0, 0, 0, 5, 1, 0, 0, 0, 0, 7, 1, 0, 0, 0, 0, 9, 1, 0, 0, 0, 0, 11, 1, 0, 0, 0, 0, 13, 1, 0, 0, 0, 0, 15, 1, 0, 0, 0, 0, 17, 1, 0, 0, 0, 0, 19, 1, 0, 0, 0, 0, 21, 1, 0, 0, 0, 0, 23, 1, 0, 0, 0, 0, 25, 1, 0, 0, 0, 0, 27, 1, 0, 0, 0, 0, 29, 1, 0, 0, 0, 0, 31, 1, 0, 0, 0, 0, 33, 1, 0, 0, 0, 1, 35, 1, 0, 0, 0, 3, 37, 1, 0, 0, 0, 5, 39, 1, 0, 0, 0, 7, 48, 1, 0, 0, 0, 9, 57, 1, 0, 0, 0, 11, 66, 1, 0, 0, 0, 13, 71, 1, 0, 0, 0, 15, 76, 1, 0, 0, 0, 17, 78, 1, 0, 0, 0, 19, 80, 1, 0, 0, 0, 21, 82, 1, 0, 0, 0, 23, 87, 1, 0, 0, 0, 25, 93, 1, 0, 0, 0, 27, 111, 1, 0, 0, 0, 29, 131, 1, 0, 0, 0, 31, 133, 1, 0, 0, 0, 33, 150, 1, 0, 0, 0, 35, 36, 5, 123, 0, 0, 36, 2, 1, 0, 0, 0, 37, 38, 5, 125, 0, 0, 38, 4, 1, 0, 0, 0, 39, 40, 5, 115, 0, 0, 40, 41, 5, 101, 0, 0, 41, 42, 5, 113, 0, 0, 42, 43, 5, 117, 0, 0, 43, 44, 5, 101, 0, 0, 44, 45, 5, 110, 0, 0, 45, 46, 5, 99, 0, 0, 46, 47, 5, 101, 0, 0, 47, 6, 1, 0, 0, 0, 48, 49, 5, 115, 0, 0, 49, 50, 5, 101, 0, 0, 50, 51, 5, 108, 0, 0, 51, 52, 5, 101, 0, 0, 52, 53, 5, 99, 0, 0, 53, 54, 5, 116, 0, 0, 54, 55, 5, 111, 0, 0, 55, 56, 5, 114, 0, 0, 56, 8, 1, 0, 0, 0, 57, 58, 5, 112, 0, 0, 58, 59, 5, 97, 0, 0, 59, 60, 5, 114, 0, 0, 60, 61, 5, 97, 0, 0, 61, 62, 5, 108, 0, 0, 62, 63, 5, 108, 0, 0, 63, 64, 5, 101, 0, 0, 64, 65, 5, 108, 0, 0, 65, 10, 1, 0, 0, 0, 66, 67, 5, 116, 0, 0, 67, 68, 5, 97, 0, 0, 68, 69, 5, 115, 0, 0, 69, 70, 5, 107, 0, 0, 70, 12, 1, 0, 0, 0, 71, 72, 5, 99, 0, 0, 72, 73, 5, 111, 0, 0, 73, 74, 5, 110, 0, 0, 74, 75, 5, 100, 0, 0, 75, 14, 1, 0, 0, 0, 76, 77, 5, 40, 0, 0, 77, 16, 1, 0, 0, 0, 78, 79, 5, 41, 0, 0, 79, 18, 1, 0, 0, 0, 80, 81, 5, 44, 0, 0, 81, 20, 1, 0, 0, 0, 82, 83, 5, 84, 0, 0, 83, 84, 5, 114, 0, 0, 84, 85, 5, 117, 0, 0, 85, 86, 5, 101, 0, 0, 86, 22, 1, 0, 0, 0, 87, 88, 5, 70, 0, 0, 88, 89, 5, 97, 0, 0, 89, 90, 5, 108, 0, 0, 90, 91, 5, 115, 0, 0, 91, 92, 5, 101, 0, 0, 92, 24, 1, 0, 0, 0, 93, 97, 7, 0, 0, 0, 94, 96, 7, 1, 0, 0, 95, 94, 1, 0, 0, 0, 96, 99, 1, 0, 0, 0, 97, 95, 1, 0, 0, 0, 97, 98, 1, 0, 0, 0, 98, 26, 1, 0, 0, 0, 99, 97, 1, 0, 0, 0, 100, 102, 5, 45, 0, 0, 101, 100, 1, 0, 0, 0, 101, 102, 1, 0, 0, 0, 102, 103, 1, 0, 0, 0, 103, 107, 7, 2, 0, 0, 104, 106, 7, 3, 0, 0, 105, 104, 1, 0, 0, 0, 106, 109, 1, 0, 0, 0, 107, 105, 1, 0, 0, 0, 107, 108, 1, 0, 0, 0, 108, 112, 1, 0, 0, 0, 109, 107, 1, 0, 0, 0, 110, 112, 5, 48, 0, 0, 111, 101, 1, 0, 0, 0, 111, 110, 1, 0, 0, 0, 112, 28, 1, 0, 0, 0, 113, 115, 7, 3, 0, 0, 114, 113, 1, 0, 0, 0, 115, 116, 1, 0, 0, 0, 116, 114, 1, 0, 0, 0, 116, 117, 1, 0, 0, 0, 117, 118, 1, 0, 0, 0, 118, 122, 5, 46, 0, 0, 119, 121, 7, 3, 0, 0, 120, 119, 1, 0, 0, 0, 121, 124, 1, 0, 0, 0, 122, 120, 1, 0, 0, 0, 122, 123, 1, 0, 0, 0, 123, 132, 1, 0, 0, 0, 124, 122, 1, 0, 0, 0, 125, 127, 5, 46, 0, 0, 126, 128, 7, 3, 0, 0, 127, 126, 1, 0, 0, 0, 128, 129, 1, 0, 0, 0, 129, 127, 1, 0, 0, 0, 129, 130, 1, 0, 0, 0, 130, 132, 1, 0, 0, 0, 131, 114, 1, 0, 0, 0, 131, 125, 1, 0, 0, 0, 132, 30, 1, 0, 0, 0, 133, 134, 5, 47, 0, 0, 134, 135, 5, 47, 0, 0, 135, 139, 1, 0, 0, 0, 136, 138, 9, 0, 0, 0, 137, 136, 1, 0, 0, 0, 138, 141, 1, 0, 0, 0, 139, 140, 1, 0, 0, 0, 139, 137, 1, 0, 0, 0, 140, 143, 1, 0, 0, 0, 141, 139, 1, 0, 0, 0, 142, 144, 5, 13, 0, 0, 143, 142, 1, 0, 0, 0, 143, 144, 1, 0, 0, 0, 144, 145, 1, 0, 0, 0, 145, 146, 5, 10, 0, 0, 146, 147, 1, 0, 0, 0, 147, 148, 6, 15, 0, 0, 148, 32, 1, 0, 0, 0, 149, 151, 7, 4, 0, 0, 150, 149, 1, 0, 0, 0, 151, 152, 1, 0, 0, 0, 152, 150, 1, 0, 0, 0, 152, 153, 1, 0, 0, 0, 153, 154, 1, 0, 0, 0, 154, 155, 6, 16, 0, 0, 155, 34, 1, 0, 0, 0, 12, 0, 97, 101, 107, 111, 116, 122, 129, 131, 139, 143, 152, 1, 6, 0, 0] \ No newline at end of file +[4, 0, 17, 155, 6, -1, 2, 0, 7, 0, 2, 1, 7, 1, 2, 2, 7, 2, 2, 3, 7, 3, 2, 4, 7, 4, 2, 5, 7, 5, 2, 6, 7, 6, 2, 7, 7, 7, 2, 8, 7, 8, 2, 9, 7, 9, 2, 10, 7, 10, 2, 11, 7, 11, 2, 12, 7, 12, 2, 13, 7, 13, 2, 14, 7, 14, 2, 15, 7, 15, 2, 16, 7, 16, 1, 0, 1, 0, 1, 1, 1, 1, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 3, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 4, 1, 5, 1, 5, 1, 5, 1, 5, 1, 6, 1, 6, 1, 6, 1, 6, 1, 6, 1, 7, 1, 7, 1, 8, 1, 8, 1, 9, 1, 9, 1, 10, 1, 10, 1, 10, 1, 10, 1, 10, 1, 11, 1, 11, 1, 11, 1, 11, 1, 11, 1, 11, 1, 12, 1, 12, 5, 12, 95, 8, 12, 10, 12, 12, 12, 98, 9, 12, 1, 13, 3, 13, 101, 8, 13, 1, 13, 1, 13, 5, 13, 105, 8, 13, 10, 13, 12, 13, 108, 9, 13, 1, 13, 3, 13, 111, 8, 13, 1, 14, 4, 14, 114, 8, 14, 11, 14, 12, 14, 115, 1, 14, 1, 14, 5, 14, 120, 8, 14, 10, 14, 12, 14, 123, 9, 14, 1, 14, 1, 14, 4, 14, 127, 8, 14, 11, 14, 12, 14, 128, 3, 14, 131, 8, 14, 1, 15, 1, 15, 1, 15, 1, 15, 5, 15, 137, 8, 15, 10, 15, 12, 15, 140, 9, 15, 1, 15, 3, 15, 143, 8, 15, 1, 15, 1, 15, 1, 15, 1, 15, 1, 16, 4, 16, 150, 8, 16, 11, 16, 12, 16, 151, 1, 16, 1, 16, 1, 138, 0, 17, 1, 1, 3, 2, 5, 3, 7, 4, 9, 5, 11, 6, 13, 7, 15, 8, 17, 9, 19, 10, 21, 11, 23, 12, 25, 13, 27, 14, 29, 15, 31, 16, 33, 17, 1, 0, 5, 3, 0, 65, 90, 95, 95, 97, 122, 4, 0, 48, 57, 65, 90, 95, 95, 97, 122, 1, 0, 49, 57, 1, 0, 48, 57, 3, 0, 9, 10, 12, 13, 32, 32, 165, 0, 1, 1, 0, 0, 0, 0, 3, 1, 0, 0, 0, 0, 5, 1, 0, 0, 0, 0, 7, 1, 0, 0, 0, 0, 9, 1, 0, 0, 0, 0, 11, 1, 0, 0, 0, 0, 13, 1, 0, 0, 0, 0, 15, 1, 0, 0, 0, 0, 17, 1, 0, 0, 0, 0, 19, 1, 0, 0, 0, 0, 21, 1, 0, 0, 0, 0, 23, 1, 0, 0, 0, 0, 25, 1, 0, 0, 0, 0, 27, 1, 0, 0, 0, 0, 29, 1, 0, 0, 0, 0, 31, 1, 0, 0, 0, 0, 33, 1, 0, 0, 0, 1, 35, 1, 0, 0, 0, 3, 37, 1, 0, 0, 0, 5, 39, 1, 0, 0, 0, 7, 48, 1, 0, 0, 0, 9, 57, 1, 0, 0, 0, 11, 66, 1, 0, 0, 0, 13, 70, 1, 0, 0, 0, 15, 75, 1, 0, 0, 0, 17, 77, 1, 0, 0, 0, 19, 79, 1, 0, 0, 0, 21, 81, 1, 0, 0, 0, 23, 86, 1, 0, 0, 0, 25, 92, 1, 0, 0, 0, 27, 110, 1, 0, 0, 0, 29, 130, 1, 0, 0, 0, 31, 132, 1, 0, 0, 0, 33, 149, 1, 0, 0, 0, 35, 36, 5, 123, 0, 0, 36, 2, 1, 0, 0, 0, 37, 38, 5, 125, 0, 0, 38, 4, 1, 0, 0, 0, 39, 40, 5, 115, 0, 0, 40, 41, 5, 101, 0, 0, 41, 42, 5, 113, 0, 0, 42, 43, 5, 117, 0, 0, 43, 44, 5, 101, 0, 0, 44, 45, 5, 110, 0, 0, 45, 46, 5, 99, 0, 0, 46, 47, 5, 101, 0, 0, 47, 6, 1, 0, 0, 0, 48, 49, 5, 115, 0, 0, 49, 50, 5, 101, 0, 0, 50, 51, 5, 108, 0, 0, 51, 52, 5, 101, 0, 0, 52, 53, 5, 99, 0, 0, 53, 54, 5, 116, 0, 0, 54, 55, 5, 111, 0, 0, 55, 56, 5, 114, 0, 0, 56, 8, 1, 0, 0, 0, 57, 58, 5, 112, 0, 0, 58, 59, 5, 97, 0, 0, 59, 60, 5, 114, 0, 0, 60, 61, 5, 97, 0, 0, 61, 62, 5, 108, 0, 0, 62, 63, 5, 108, 0, 0, 63, 64, 5, 101, 0, 0, 64, 65, 5, 108, 0, 0, 65, 10, 1, 0, 0, 0, 66, 67, 5, 97, 0, 0, 67, 68, 5, 99, 0, 0, 68, 69, 5, 116, 0, 0, 69, 12, 1, 0, 0, 0, 70, 71, 5, 99, 0, 0, 71, 72, 5, 111, 0, 0, 72, 73, 5, 110, 0, 0, 73, 74, 5, 100, 0, 0, 74, 14, 1, 0, 0, 0, 75, 76, 5, 40, 0, 0, 76, 16, 1, 0, 0, 0, 77, 78, 5, 41, 0, 0, 78, 18, 1, 0, 0, 0, 79, 80, 5, 44, 0, 0, 80, 20, 1, 0, 0, 0, 81, 82, 5, 84, 0, 0, 82, 83, 5, 114, 0, 0, 83, 84, 5, 117, 0, 0, 84, 85, 5, 101, 0, 0, 85, 22, 1, 0, 0, 0, 86, 87, 5, 70, 0, 0, 87, 88, 5, 97, 0, 0, 88, 89, 5, 108, 0, 0, 89, 90, 5, 115, 0, 0, 90, 91, 5, 101, 0, 0, 91, 24, 1, 0, 0, 0, 92, 96, 7, 0, 0, 0, 93, 95, 7, 1, 0, 0, 94, 93, 1, 0, 0, 0, 95, 98, 1, 0, 0, 0, 96, 94, 1, 0, 0, 0, 96, 97, 1, 0, 0, 0, 97, 26, 1, 0, 0, 0, 98, 96, 1, 0, 0, 0, 99, 101, 5, 45, 0, 0, 100, 99, 1, 0, 0, 0, 100, 101, 1, 0, 0, 0, 101, 102, 1, 0, 0, 0, 102, 106, 7, 2, 0, 0, 103, 105, 7, 3, 0, 0, 104, 103, 1, 0, 0, 0, 105, 108, 1, 0, 0, 0, 106, 104, 1, 0, 0, 0, 106, 107, 1, 0, 0, 0, 107, 111, 1, 0, 0, 0, 108, 106, 1, 0, 0, 0, 109, 111, 5, 48, 0, 0, 110, 100, 1, 0, 0, 0, 110, 109, 1, 0, 0, 0, 111, 28, 1, 0, 0, 0, 112, 114, 7, 3, 0, 0, 113, 112, 1, 0, 0, 0, 114, 115, 1, 0, 0, 0, 115, 113, 1, 0, 0, 0, 115, 116, 1, 0, 0, 0, 116, 117, 1, 0, 0, 0, 117, 121, 5, 46, 0, 0, 118, 120, 7, 3, 0, 0, 119, 118, 1, 0, 0, 0, 120, 123, 1, 0, 0, 0, 121, 119, 1, 0, 0, 0, 121, 122, 1, 0, 0, 0, 122, 131, 1, 0, 0, 0, 123, 121, 1, 0, 0, 0, 124, 126, 5, 46, 0, 0, 125, 127, 7, 3, 0, 0, 126, 125, 1, 0, 0, 0, 127, 128, 1, 0, 0, 0, 128, 126, 1, 0, 0, 0, 128, 129, 1, 0, 0, 0, 129, 131, 1, 0, 0, 0, 130, 113, 1, 0, 0, 0, 130, 124, 1, 0, 0, 0, 131, 30, 1, 0, 0, 0, 132, 133, 5, 47, 0, 0, 133, 134, 5, 47, 0, 0, 134, 138, 1, 0, 0, 0, 135, 137, 9, 0, 0, 0, 136, 135, 1, 0, 0, 0, 137, 140, 1, 0, 0, 0, 138, 139, 1, 0, 0, 0, 138, 136, 1, 0, 0, 0, 139, 142, 1, 0, 0, 0, 140, 138, 1, 0, 0, 0, 141, 143, 5, 13, 0, 0, 142, 141, 1, 0, 0, 0, 142, 143, 1, 0, 0, 0, 143, 144, 1, 0, 0, 0, 144, 145, 5, 10, 0, 0, 145, 146, 1, 0, 0, 0, 146, 147, 6, 15, 0, 0, 147, 32, 1, 0, 0, 0, 148, 150, 7, 4, 0, 0, 149, 148, 1, 0, 0, 0, 150, 151, 1, 0, 0, 0, 151, 149, 1, 0, 0, 0, 151, 152, 1, 0, 0, 0, 152, 153, 1, 0, 0, 0, 153, 154, 6, 16, 0, 0, 154, 34, 1, 0, 0, 0, 12, 0, 96, 100, 106, 110, 115, 121, 128, 130, 138, 142, 151, 1, 6, 0, 0] \ No newline at end of file diff --git a/robowaiter/behavior_tree/ptml/ptmlLexer.py b/robowaiter/behavior_tree/ptml/ptmlLexer.py index 5fe957a..0cee05f 100644 --- a/robowaiter/behavior_tree/ptml/ptmlLexer.py +++ b/robowaiter/behavior_tree/ptml/ptmlLexer.py @@ -1,4 +1,4 @@ -# Generated from E:/Projects/UE5/HARIX_RDKSim/Plugins/HarixSim/Python/ptml/ptml.g4 by ANTLR 4.13.1 +# Generated from ptml.g4 by ANTLR 4.13.1 from antlr4 import * from io import StringIO import sys @@ -10,62 +10,61 @@ else: def serializedATN(): return [ - 4,0,17,156,6,-1,2,0,7,0,2,1,7,1,2,2,7,2,2,3,7,3,2,4,7,4,2,5,7,5, + 4,0,17,155,6,-1,2,0,7,0,2,1,7,1,2,2,7,2,2,3,7,3,2,4,7,4,2,5,7,5, 2,6,7,6,2,7,7,7,2,8,7,8,2,9,7,9,2,10,7,10,2,11,7,11,2,12,7,12,2, 13,7,13,2,14,7,14,2,15,7,15,2,16,7,16,1,0,1,0,1,1,1,1,1,2,1,2,1, 2,1,2,1,2,1,2,1,2,1,2,1,2,1,3,1,3,1,3,1,3,1,3,1,3,1,3,1,3,1,3,1, - 4,1,4,1,4,1,4,1,4,1,4,1,4,1,4,1,4,1,5,1,5,1,5,1,5,1,5,1,6,1,6,1, - 6,1,6,1,6,1,7,1,7,1,8,1,8,1,9,1,9,1,10,1,10,1,10,1,10,1,10,1,11, - 1,11,1,11,1,11,1,11,1,11,1,12,1,12,5,12,96,8,12,10,12,12,12,99,9, - 12,1,13,3,13,102,8,13,1,13,1,13,5,13,106,8,13,10,13,12,13,109,9, - 13,1,13,3,13,112,8,13,1,14,4,14,115,8,14,11,14,12,14,116,1,14,1, - 14,5,14,121,8,14,10,14,12,14,124,9,14,1,14,1,14,4,14,128,8,14,11, - 14,12,14,129,3,14,132,8,14,1,15,1,15,1,15,1,15,5,15,138,8,15,10, - 15,12,15,141,9,15,1,15,3,15,144,8,15,1,15,1,15,1,15,1,15,1,16,4, - 16,151,8,16,11,16,12,16,152,1,16,1,16,1,139,0,17,1,1,3,2,5,3,7,4, - 9,5,11,6,13,7,15,8,17,9,19,10,21,11,23,12,25,13,27,14,29,15,31,16, - 33,17,1,0,5,3,0,65,90,95,95,97,122,4,0,48,57,65,90,95,95,97,122, - 1,0,49,57,1,0,48,57,3,0,9,10,12,13,32,32,166,0,1,1,0,0,0,0,3,1,0, - 0,0,0,5,1,0,0,0,0,7,1,0,0,0,0,9,1,0,0,0,0,11,1,0,0,0,0,13,1,0,0, - 0,0,15,1,0,0,0,0,17,1,0,0,0,0,19,1,0,0,0,0,21,1,0,0,0,0,23,1,0,0, - 0,0,25,1,0,0,0,0,27,1,0,0,0,0,29,1,0,0,0,0,31,1,0,0,0,0,33,1,0,0, - 0,1,35,1,0,0,0,3,37,1,0,0,0,5,39,1,0,0,0,7,48,1,0,0,0,9,57,1,0,0, - 0,11,66,1,0,0,0,13,71,1,0,0,0,15,76,1,0,0,0,17,78,1,0,0,0,19,80, - 1,0,0,0,21,82,1,0,0,0,23,87,1,0,0,0,25,93,1,0,0,0,27,111,1,0,0,0, - 29,131,1,0,0,0,31,133,1,0,0,0,33,150,1,0,0,0,35,36,5,123,0,0,36, - 2,1,0,0,0,37,38,5,125,0,0,38,4,1,0,0,0,39,40,5,115,0,0,40,41,5,101, - 0,0,41,42,5,113,0,0,42,43,5,117,0,0,43,44,5,101,0,0,44,45,5,110, - 0,0,45,46,5,99,0,0,46,47,5,101,0,0,47,6,1,0,0,0,48,49,5,115,0,0, - 49,50,5,101,0,0,50,51,5,108,0,0,51,52,5,101,0,0,52,53,5,99,0,0,53, - 54,5,116,0,0,54,55,5,111,0,0,55,56,5,114,0,0,56,8,1,0,0,0,57,58, - 5,112,0,0,58,59,5,97,0,0,59,60,5,114,0,0,60,61,5,97,0,0,61,62,5, - 108,0,0,62,63,5,108,0,0,63,64,5,101,0,0,64,65,5,108,0,0,65,10,1, - 0,0,0,66,67,5,116,0,0,67,68,5,97,0,0,68,69,5,115,0,0,69,70,5,107, - 0,0,70,12,1,0,0,0,71,72,5,99,0,0,72,73,5,111,0,0,73,74,5,110,0,0, - 74,75,5,100,0,0,75,14,1,0,0,0,76,77,5,40,0,0,77,16,1,0,0,0,78,79, - 5,41,0,0,79,18,1,0,0,0,80,81,5,44,0,0,81,20,1,0,0,0,82,83,5,84,0, - 0,83,84,5,114,0,0,84,85,5,117,0,0,85,86,5,101,0,0,86,22,1,0,0,0, - 87,88,5,70,0,0,88,89,5,97,0,0,89,90,5,108,0,0,90,91,5,115,0,0,91, - 92,5,101,0,0,92,24,1,0,0,0,93,97,7,0,0,0,94,96,7,1,0,0,95,94,1,0, - 0,0,96,99,1,0,0,0,97,95,1,0,0,0,97,98,1,0,0,0,98,26,1,0,0,0,99,97, - 1,0,0,0,100,102,5,45,0,0,101,100,1,0,0,0,101,102,1,0,0,0,102,103, - 1,0,0,0,103,107,7,2,0,0,104,106,7,3,0,0,105,104,1,0,0,0,106,109, - 1,0,0,0,107,105,1,0,0,0,107,108,1,0,0,0,108,112,1,0,0,0,109,107, - 1,0,0,0,110,112,5,48,0,0,111,101,1,0,0,0,111,110,1,0,0,0,112,28, - 1,0,0,0,113,115,7,3,0,0,114,113,1,0,0,0,115,116,1,0,0,0,116,114, - 1,0,0,0,116,117,1,0,0,0,117,118,1,0,0,0,118,122,5,46,0,0,119,121, - 7,3,0,0,120,119,1,0,0,0,121,124,1,0,0,0,122,120,1,0,0,0,122,123, - 1,0,0,0,123,132,1,0,0,0,124,122,1,0,0,0,125,127,5,46,0,0,126,128, - 7,3,0,0,127,126,1,0,0,0,128,129,1,0,0,0,129,127,1,0,0,0,129,130, - 1,0,0,0,130,132,1,0,0,0,131,114,1,0,0,0,131,125,1,0,0,0,132,30,1, - 0,0,0,133,134,5,47,0,0,134,135,5,47,0,0,135,139,1,0,0,0,136,138, - 9,0,0,0,137,136,1,0,0,0,138,141,1,0,0,0,139,140,1,0,0,0,139,137, - 1,0,0,0,140,143,1,0,0,0,141,139,1,0,0,0,142,144,5,13,0,0,143,142, - 1,0,0,0,143,144,1,0,0,0,144,145,1,0,0,0,145,146,5,10,0,0,146,147, - 1,0,0,0,147,148,6,15,0,0,148,32,1,0,0,0,149,151,7,4,0,0,150,149, - 1,0,0,0,151,152,1,0,0,0,152,150,1,0,0,0,152,153,1,0,0,0,153,154, - 1,0,0,0,154,155,6,16,0,0,155,34,1,0,0,0,12,0,97,101,107,111,116, - 122,129,131,139,143,152,1,6,0,0 + 4,1,4,1,4,1,4,1,4,1,4,1,4,1,4,1,4,1,5,1,5,1,5,1,5,1,6,1,6,1,6,1, + 6,1,6,1,7,1,7,1,8,1,8,1,9,1,9,1,10,1,10,1,10,1,10,1,10,1,11,1,11, + 1,11,1,11,1,11,1,11,1,12,1,12,5,12,95,8,12,10,12,12,12,98,9,12,1, + 13,3,13,101,8,13,1,13,1,13,5,13,105,8,13,10,13,12,13,108,9,13,1, + 13,3,13,111,8,13,1,14,4,14,114,8,14,11,14,12,14,115,1,14,1,14,5, + 14,120,8,14,10,14,12,14,123,9,14,1,14,1,14,4,14,127,8,14,11,14,12, + 14,128,3,14,131,8,14,1,15,1,15,1,15,1,15,5,15,137,8,15,10,15,12, + 15,140,9,15,1,15,3,15,143,8,15,1,15,1,15,1,15,1,15,1,16,4,16,150, + 8,16,11,16,12,16,151,1,16,1,16,1,138,0,17,1,1,3,2,5,3,7,4,9,5,11, + 6,13,7,15,8,17,9,19,10,21,11,23,12,25,13,27,14,29,15,31,16,33,17, + 1,0,5,3,0,65,90,95,95,97,122,4,0,48,57,65,90,95,95,97,122,1,0,49, + 57,1,0,48,57,3,0,9,10,12,13,32,32,165,0,1,1,0,0,0,0,3,1,0,0,0,0, + 5,1,0,0,0,0,7,1,0,0,0,0,9,1,0,0,0,0,11,1,0,0,0,0,13,1,0,0,0,0,15, + 1,0,0,0,0,17,1,0,0,0,0,19,1,0,0,0,0,21,1,0,0,0,0,23,1,0,0,0,0,25, + 1,0,0,0,0,27,1,0,0,0,0,29,1,0,0,0,0,31,1,0,0,0,0,33,1,0,0,0,1,35, + 1,0,0,0,3,37,1,0,0,0,5,39,1,0,0,0,7,48,1,0,0,0,9,57,1,0,0,0,11,66, + 1,0,0,0,13,70,1,0,0,0,15,75,1,0,0,0,17,77,1,0,0,0,19,79,1,0,0,0, + 21,81,1,0,0,0,23,86,1,0,0,0,25,92,1,0,0,0,27,110,1,0,0,0,29,130, + 1,0,0,0,31,132,1,0,0,0,33,149,1,0,0,0,35,36,5,123,0,0,36,2,1,0,0, + 0,37,38,5,125,0,0,38,4,1,0,0,0,39,40,5,115,0,0,40,41,5,101,0,0,41, + 42,5,113,0,0,42,43,5,117,0,0,43,44,5,101,0,0,44,45,5,110,0,0,45, + 46,5,99,0,0,46,47,5,101,0,0,47,6,1,0,0,0,48,49,5,115,0,0,49,50,5, + 101,0,0,50,51,5,108,0,0,51,52,5,101,0,0,52,53,5,99,0,0,53,54,5,116, + 0,0,54,55,5,111,0,0,55,56,5,114,0,0,56,8,1,0,0,0,57,58,5,112,0,0, + 58,59,5,97,0,0,59,60,5,114,0,0,60,61,5,97,0,0,61,62,5,108,0,0,62, + 63,5,108,0,0,63,64,5,101,0,0,64,65,5,108,0,0,65,10,1,0,0,0,66,67, + 5,97,0,0,67,68,5,99,0,0,68,69,5,116,0,0,69,12,1,0,0,0,70,71,5,99, + 0,0,71,72,5,111,0,0,72,73,5,110,0,0,73,74,5,100,0,0,74,14,1,0,0, + 0,75,76,5,40,0,0,76,16,1,0,0,0,77,78,5,41,0,0,78,18,1,0,0,0,79,80, + 5,44,0,0,80,20,1,0,0,0,81,82,5,84,0,0,82,83,5,114,0,0,83,84,5,117, + 0,0,84,85,5,101,0,0,85,22,1,0,0,0,86,87,5,70,0,0,87,88,5,97,0,0, + 88,89,5,108,0,0,89,90,5,115,0,0,90,91,5,101,0,0,91,24,1,0,0,0,92, + 96,7,0,0,0,93,95,7,1,0,0,94,93,1,0,0,0,95,98,1,0,0,0,96,94,1,0,0, + 0,96,97,1,0,0,0,97,26,1,0,0,0,98,96,1,0,0,0,99,101,5,45,0,0,100, + 99,1,0,0,0,100,101,1,0,0,0,101,102,1,0,0,0,102,106,7,2,0,0,103,105, + 7,3,0,0,104,103,1,0,0,0,105,108,1,0,0,0,106,104,1,0,0,0,106,107, + 1,0,0,0,107,111,1,0,0,0,108,106,1,0,0,0,109,111,5,48,0,0,110,100, + 1,0,0,0,110,109,1,0,0,0,111,28,1,0,0,0,112,114,7,3,0,0,113,112,1, + 0,0,0,114,115,1,0,0,0,115,113,1,0,0,0,115,116,1,0,0,0,116,117,1, + 0,0,0,117,121,5,46,0,0,118,120,7,3,0,0,119,118,1,0,0,0,120,123,1, + 0,0,0,121,119,1,0,0,0,121,122,1,0,0,0,122,131,1,0,0,0,123,121,1, + 0,0,0,124,126,5,46,0,0,125,127,7,3,0,0,126,125,1,0,0,0,127,128,1, + 0,0,0,128,126,1,0,0,0,128,129,1,0,0,0,129,131,1,0,0,0,130,113,1, + 0,0,0,130,124,1,0,0,0,131,30,1,0,0,0,132,133,5,47,0,0,133,134,5, + 47,0,0,134,138,1,0,0,0,135,137,9,0,0,0,136,135,1,0,0,0,137,140,1, + 0,0,0,138,139,1,0,0,0,138,136,1,0,0,0,139,142,1,0,0,0,140,138,1, + 0,0,0,141,143,5,13,0,0,142,141,1,0,0,0,142,143,1,0,0,0,143,144,1, + 0,0,0,144,145,5,10,0,0,145,146,1,0,0,0,146,147,6,15,0,0,147,32,1, + 0,0,0,148,150,7,4,0,0,149,148,1,0,0,0,150,151,1,0,0,0,151,149,1, + 0,0,0,151,152,1,0,0,0,152,153,1,0,0,0,153,154,6,16,0,0,154,34,1, + 0,0,0,12,0,96,100,106,110,115,121,128,130,138,142,151,1,6,0,0 ] class ptmlLexer(Lexer): @@ -86,7 +85,7 @@ class ptmlLexer(Lexer): T__9 = 10 T__10 = 11 T__11 = 12 - Names = 13 + String = 13 Integer = 14 Float = 15 LINE_COMMENT = 16 @@ -97,14 +96,14 @@ class ptmlLexer(Lexer): modeNames = [ "DEFAULT_MODE" ] literalNames = [ "", - "'{'", "'}'", "'sequence'", "'selector'", "'parallel'", "'task'", + "'{'", "'}'", "'sequence'", "'selector'", "'parallel'", "'act'", "'cond'", "'('", "')'", "','", "'True'", "'False'" ] symbolicNames = [ "", - "Names", "Integer", "Float", "LINE_COMMENT", "WS" ] + "String", "Integer", "Float", "LINE_COMMENT", "WS" ] ruleNames = [ "T__0", "T__1", "T__2", "T__3", "T__4", "T__5", "T__6", - "T__7", "T__8", "T__9", "T__10", "T__11", "Names", "Integer", + "T__7", "T__8", "T__9", "T__10", "T__11", "String", "Integer", "Float", "LINE_COMMENT", "WS" ] grammarFileName = "ptml.g4" diff --git a/robowaiter/behavior_tree/ptml/ptmlLexer.tokens b/robowaiter/behavior_tree/ptml/ptmlLexer.tokens index 1f7a813..6f604be 100644 --- a/robowaiter/behavior_tree/ptml/ptmlLexer.tokens +++ b/robowaiter/behavior_tree/ptml/ptmlLexer.tokens @@ -10,7 +10,7 @@ T__8=9 T__9=10 T__10=11 T__11=12 -Names=13 +String=13 Integer=14 Float=15 LINE_COMMENT=16 @@ -20,7 +20,7 @@ WS=17 'sequence'=3 'selector'=4 'parallel'=5 -'task'=6 +'act'=6 'cond'=7 '('=8 ')'=9 diff --git a/robowaiter/behavior_tree/ptml/ptmlListener.py b/robowaiter/behavior_tree/ptml/ptmlListener.py index 41056b2..536aaf2 100644 --- a/robowaiter/behavior_tree/ptml/ptmlListener.py +++ b/robowaiter/behavior_tree/ptml/ptmlListener.py @@ -1,4 +1,4 @@ -# Generated from E:/Projects/UE5/HARIX_RDKSim/Plugins/HarixSim/Python/ptml/ptml.g4 by ANTLR 4.13.1 +# Generated from ptml.g4 by ANTLR 4.13.1 from antlr4 import * if "." in __name__: from .ptmlParser import ptmlParser diff --git a/robowaiter/behavior_tree/ptml/ptmlParser.py b/robowaiter/behavior_tree/ptml/ptmlParser.py index 9af021d..d0c787d 100644 --- a/robowaiter/behavior_tree/ptml/ptmlParser.py +++ b/robowaiter/behavior_tree/ptml/ptmlParser.py @@ -1,4 +1,4 @@ -# Generated from E:/Projects/UE5/HARIX_RDKSim/Plugins/HarixSim/Python/ptml/ptml.g4 by ANTLR 4.13.1 +# Generated from ptml.g4 by ANTLR 4.13.1 # encoding: utf-8 from antlr4 import * from io import StringIO @@ -10,26 +10,27 @@ else: def serializedATN(): return [ - 4,1,17,62,2,0,7,0,2,1,7,1,2,2,7,2,2,3,7,3,2,4,7,4,2,5,7,5,1,0,4, + 4,1,17,64,2,0,7,0,2,1,7,1,2,2,7,2,2,3,7,3,2,4,7,4,2,5,7,5,1,0,4, 0,14,8,0,11,0,12,0,15,1,0,1,0,1,1,1,1,1,1,1,1,4,1,24,8,1,11,1,12, 1,25,1,1,1,1,1,2,1,2,1,2,1,2,3,2,34,8,2,1,3,1,3,1,3,1,3,3,3,40,8, - 3,1,3,1,3,1,4,1,4,1,4,3,4,47,8,4,1,4,1,4,1,4,1,4,3,4,53,8,4,5,4, - 55,8,4,10,4,12,4,58,9,4,1,5,1,5,1,5,0,0,6,0,2,4,6,8,10,0,2,1,0,6, - 7,1,0,11,12,66,0,13,1,0,0,0,2,19,1,0,0,0,4,33,1,0,0,0,6,35,1,0,0, - 0,8,46,1,0,0,0,10,59,1,0,0,0,12,14,3,2,1,0,13,12,1,0,0,0,14,15,1, - 0,0,0,15,13,1,0,0,0,15,16,1,0,0,0,16,17,1,0,0,0,17,18,5,0,0,1,18, - 1,1,0,0,0,19,20,3,4,2,0,20,23,5,1,0,0,21,24,3,6,3,0,22,24,3,2,1, - 0,23,21,1,0,0,0,23,22,1,0,0,0,24,25,1,0,0,0,25,23,1,0,0,0,25,26, - 1,0,0,0,26,27,1,0,0,0,27,28,5,2,0,0,28,3,1,0,0,0,29,34,5,3,0,0,30, - 34,5,4,0,0,31,32,5,5,0,0,32,34,5,14,0,0,33,29,1,0,0,0,33,30,1,0, - 0,0,33,31,1,0,0,0,34,5,1,0,0,0,35,36,7,0,0,0,36,37,5,13,0,0,37,39, - 5,8,0,0,38,40,3,8,4,0,39,38,1,0,0,0,39,40,1,0,0,0,40,41,1,0,0,0, - 41,42,5,9,0,0,42,7,1,0,0,0,43,47,5,14,0,0,44,47,5,15,0,0,45,47,3, - 10,5,0,46,43,1,0,0,0,46,44,1,0,0,0,46,45,1,0,0,0,47,56,1,0,0,0,48, - 52,5,10,0,0,49,53,5,14,0,0,50,53,5,15,0,0,51,53,3,10,5,0,52,49,1, - 0,0,0,52,50,1,0,0,0,52,51,1,0,0,0,53,55,1,0,0,0,54,48,1,0,0,0,55, - 58,1,0,0,0,56,54,1,0,0,0,56,57,1,0,0,0,57,9,1,0,0,0,58,56,1,0,0, - 0,59,60,7,1,0,0,60,11,1,0,0,0,8,15,23,25,33,39,46,52,56 + 3,1,3,1,3,1,4,1,4,1,4,1,4,3,4,48,8,4,1,4,1,4,1,4,1,4,1,4,3,4,55, + 8,4,5,4,57,8,4,10,4,12,4,60,9,4,1,5,1,5,1,5,0,0,6,0,2,4,6,8,10,0, + 2,1,0,6,7,1,0,11,12,70,0,13,1,0,0,0,2,19,1,0,0,0,4,33,1,0,0,0,6, + 35,1,0,0,0,8,47,1,0,0,0,10,61,1,0,0,0,12,14,3,2,1,0,13,12,1,0,0, + 0,14,15,1,0,0,0,15,13,1,0,0,0,15,16,1,0,0,0,16,17,1,0,0,0,17,18, + 5,0,0,1,18,1,1,0,0,0,19,20,3,4,2,0,20,23,5,1,0,0,21,24,3,6,3,0,22, + 24,3,2,1,0,23,21,1,0,0,0,23,22,1,0,0,0,24,25,1,0,0,0,25,23,1,0,0, + 0,25,26,1,0,0,0,26,27,1,0,0,0,27,28,5,2,0,0,28,3,1,0,0,0,29,34,5, + 3,0,0,30,34,5,4,0,0,31,32,5,5,0,0,32,34,5,14,0,0,33,29,1,0,0,0,33, + 30,1,0,0,0,33,31,1,0,0,0,34,5,1,0,0,0,35,36,7,0,0,0,36,37,5,13,0, + 0,37,39,5,8,0,0,38,40,3,8,4,0,39,38,1,0,0,0,39,40,1,0,0,0,40,41, + 1,0,0,0,41,42,5,9,0,0,42,7,1,0,0,0,43,48,5,14,0,0,44,48,5,15,0,0, + 45,48,3,10,5,0,46,48,5,13,0,0,47,43,1,0,0,0,47,44,1,0,0,0,47,45, + 1,0,0,0,47,46,1,0,0,0,48,58,1,0,0,0,49,54,5,10,0,0,50,55,5,14,0, + 0,51,55,5,15,0,0,52,55,3,10,5,0,53,55,5,13,0,0,54,50,1,0,0,0,54, + 51,1,0,0,0,54,52,1,0,0,0,54,53,1,0,0,0,55,57,1,0,0,0,56,49,1,0,0, + 0,57,60,1,0,0,0,58,56,1,0,0,0,58,59,1,0,0,0,59,9,1,0,0,0,60,58,1, + 0,0,0,61,62,7,1,0,0,62,11,1,0,0,0,8,15,23,25,33,39,47,54,58 ] class ptmlParser ( Parser ): @@ -43,13 +44,13 @@ class ptmlParser ( Parser ): sharedContextCache = PredictionContextCache() literalNames = [ "", "'{'", "'}'", "'sequence'", "'selector'", - "'parallel'", "'task'", "'cond'", "'('", "')'", "','", + "'parallel'", "'act'", "'cond'", "'('", "')'", "','", "'True'", "'False'" ] symbolicNames = [ "", "", "", "", "", "", "", "", "", "", "", "", - "", "Names", "Integer", "Float", "LINE_COMMENT", + "", "String", "Integer", "Float", "LINE_COMMENT", "WS" ] RULE_root = 0 @@ -75,7 +76,7 @@ class ptmlParser ( Parser ): T__9=10 T__10=11 T__11=12 - Names=13 + String=13 Integer=14 Float=15 LINE_COMMENT=16 @@ -304,8 +305,8 @@ class ptmlParser ( Parser ): super().__init__(parent, invokingState) self.parser = parser - def Names(self): - return self.getToken(ptmlParser.Names, 0) + def String(self): + return self.getToken(ptmlParser.String, 0) def action_parm(self): return self.getTypedRuleContext(ptmlParser.Action_parmContext,0) @@ -340,13 +341,13 @@ class ptmlParser ( Parser ): self._errHandler.reportMatch(self) self.consume() self.state = 36 - self.match(ptmlParser.Names) + self.match(ptmlParser.String) self.state = 37 self.match(ptmlParser.T__7) self.state = 39 self._errHandler.sync(self) _la = self._input.LA(1) - if (((_la) & ~0x3f) == 0 and ((1 << _la) & 55296) != 0): + if (((_la) & ~0x3f) == 0 and ((1 << _la) & 63488) != 0): self.state = 38 self.action_parm() @@ -388,6 +389,12 @@ class ptmlParser ( Parser ): return self.getTypedRuleContext(ptmlParser.BooleanContext,i) + def String(self, i:int=None): + if i is None: + return self.getTokens(ptmlParser.String) + else: + return self.getToken(ptmlParser.String, i) + def getRuleIndex(self): return ptmlParser.RULE_action_parm @@ -409,7 +416,7 @@ class ptmlParser ( Parser ): self._la = 0 # Token type try: self.enterOuterAlt(localctx, 1) - self.state = 46 + self.state = 47 self._errHandler.sync(self) token = self._input.LA(1) if token in [14]: @@ -424,34 +431,42 @@ class ptmlParser ( Parser ): self.state = 45 self.boolean() pass + elif token in [13]: + self.state = 46 + self.match(ptmlParser.String) + pass else: raise NoViableAltException(self) - self.state = 56 + self.state = 58 self._errHandler.sync(self) _la = self._input.LA(1) while _la==10: - self.state = 48 + self.state = 49 self.match(ptmlParser.T__9) - self.state = 52 + self.state = 54 self._errHandler.sync(self) token = self._input.LA(1) if token in [14]: - self.state = 49 + self.state = 50 self.match(ptmlParser.Integer) pass elif token in [15]: - self.state = 50 + self.state = 51 self.match(ptmlParser.Float) pass elif token in [11, 12]: - self.state = 51 + self.state = 52 self.boolean() pass + elif token in [13]: + self.state = 53 + self.match(ptmlParser.String) + pass else: raise NoViableAltException(self) - self.state = 58 + self.state = 60 self._errHandler.sync(self) _la = self._input.LA(1) @@ -493,7 +508,7 @@ class ptmlParser ( Parser ): self._la = 0 # Token type try: self.enterOuterAlt(localctx, 1) - self.state = 59 + self.state = 61 _la = self._input.LA(1) if not(_la==11 or _la==12): self._errHandler.recoverInline(self) diff --git a/robowaiter/behavior_tree/ptml/ptmlTranslator.py b/robowaiter/behavior_tree/ptml/ptmlTranslator.py index d0b13b4..b6fe0ed 100644 --- a/robowaiter/behavior_tree/ptml/ptmlTranslator.py +++ b/robowaiter/behavior_tree/ptml/ptmlTranslator.py @@ -76,9 +76,9 @@ class ptmlTranslator(ptmlListener): # Enter a parse tree produced by ptmlParser#action_sign. def enterAction_sign(self, ctx: ptmlParser.Action_signContext): - # cond / task + # cond / act node_type = str(ctx.children[0]) - name = str(ctx.Names()) + name = str(ctx.String()) # if have params args = [] @@ -88,22 +88,21 @@ class ptmlTranslator(ptmlListener): for i in params.children: if isinstance(i, ptmlParser.BooleanContext): args.append(str(i.children[0])) + elif str(i)==',': + args.append(',') else: - args.append(str(i)) + args.append(f"'{i}'") args = "".join(args) - import sys - sys.path.append(self.behaviour_lib_path) - - exec("from {} import {}".format(name, name)) + exec(f"from {name} import {name}") # - tag = "cond_" + short_uuid() if node_type == "cond" else "task_" + short_uuid() + # tag = "cond_" + short_uuid() if node_type == "cond" else "task_" + short_uuid() - node = eval( - "{}('{}', scene, {})".format(name, tag, args), {"scene": self.scene, **locals()} - ) + print(f'create node: {name}({args})') + node = eval(f"{name}({args})") + node.set_scene(self.scene) # connect self.stack[-1].add_child(node) diff --git a/robowaiter/behavior_tree/ptml/ptml_test.py b/robowaiter/behavior_tree/ptml/ptml_test.py deleted file mode 100644 index bdcd3d2..0000000 --- a/robowaiter/behavior_tree/ptml/ptml_test.py +++ /dev/null @@ -1,22 +0,0 @@ -import os -import py_trees as ptree - -from scene import scene -from ptmlCompiler import load - - -if __name__ == '__main__': - - project_path = "" - - ptml_path = os.path.join(project_path, 'CoffeeDelivery.ptml') - behavior_lib_path = os.path.join(project_path, '../../behavior_lib') - - scene = control.Scene(sceneID=0) - # load - scene.load_BT(ptml_path, behavior_lib_path) - # ptree.display.render_dot_tree(bt) - # build and tick - scene.BT = ptree.trees.BehaviourTree(scene.BT) - # todo: tick this bt - print(scene.BT) \ No newline at end of file diff --git a/robowaiter/behavior_tree/ptml/sequence_oufbg8fg.dot b/robowaiter/behavior_tree/ptml/sequence_oufbg8fg.dot deleted file mode 100644 index 79e4ba9..0000000 --- a/robowaiter/behavior_tree/ptml/sequence_oufbg8fg.dot +++ /dev/null @@ -1,45 +0,0 @@ -digraph pastafarianism { -ordering=out; -graph [fontname="times-roman"]; -node [fontname="times-roman"]; -edge [fontname="times-roman"]; -sequence_oUFBg8fG [fillcolor=orange, fontcolor=black, fontsize=9, label=sequence_oUFBg8fG, shape=box, style=filled]; -selector_mPAUGPy3 [fillcolor=cyan, fontcolor=black, fontsize=9, label=selector_mPAUGPy3, shape=octagon, style=filled]; -sequence_oUFBg8fG -> selector_mPAUGPy3; -cond_G7w6nx8B [fillcolor=gray, fontcolor=black, fontsize=9, label=cond_G7w6nx8B, shape=ellipse, style=filled]; -selector_mPAUGPy3 -> cond_G7w6nx8B; -task_XjF4V8bW [fillcolor=gray, fontcolor=black, fontsize=9, label=task_XjF4V8bW, shape=ellipse, style=filled]; -selector_mPAUGPy3 -> task_XjF4V8bW; -sequence_n3nzZvgz [fillcolor=orange, fontcolor=black, fontsize=9, label=sequence_n3nzZvgz, shape=box, style=filled]; -selector_mPAUGPy3 -> sequence_n3nzZvgz; -cond_KCt2aubU [fillcolor=gray, fontcolor=black, fontsize=9, label=cond_KCt2aubU, shape=ellipse, style=filled]; -sequence_n3nzZvgz -> cond_KCt2aubU; -task_a8inFHcZ [fillcolor=gray, fontcolor=black, fontsize=9, label=task_a8inFHcZ, shape=ellipse, style=filled]; -sequence_n3nzZvgz -> task_a8inFHcZ; -task_YF2aiEPZ [fillcolor=gray, fontcolor=black, fontsize=9, label=task_YF2aiEPZ, shape=ellipse, style=filled]; -sequence_n3nzZvgz -> task_YF2aiEPZ; -parallel_WEe6n4ym [fillcolor=gold, fontcolor=black, fontsize=9, label="parallel_WEe6n4ym\ntype", shape=parallelogram, style=filled]; -sequence_n3nzZvgz -> parallel_WEe6n4ym; -cond_mRQoZb2w [fillcolor=gray, fontcolor=black, fontsize=9, label=cond_mRQoZb2w, shape=ellipse, style=filled]; -parallel_WEe6n4ym -> cond_mRQoZb2w; -task_MW4BQ2XF [fillcolor=gray, fontcolor=black, fontsize=9, label=task_MW4BQ2XF, shape=ellipse, style=filled]; -parallel_WEe6n4ym -> task_MW4BQ2XF; -selector_tU6G7YK6 [fillcolor=cyan, fontcolor=black, fontsize=9, label=selector_tU6G7YK6, shape=octagon, style=filled]; -sequence_oUFBg8fG -> selector_tU6G7YK6; -cond_UMTzm9kK [fillcolor=gray, fontcolor=black, fontsize=9, label=cond_UMTzm9kK, shape=ellipse, style=filled]; -selector_tU6G7YK6 -> cond_UMTzm9kK; -task_fnpwP2Lu [fillcolor=gray, fontcolor=black, fontsize=9, label=task_fnpwP2Lu, shape=ellipse, style=filled]; -selector_tU6G7YK6 -> task_fnpwP2Lu; -selector_qm8Zexyr [fillcolor=cyan, fontcolor=black, fontsize=9, label=selector_qm8Zexyr, shape=octagon, style=filled]; -sequence_oUFBg8fG -> selector_qm8Zexyr; -cond_g2umcjH4 [fillcolor=gray, fontcolor=black, fontsize=9, label=cond_g2umcjH4, shape=ellipse, style=filled]; -selector_qm8Zexyr -> cond_g2umcjH4; -task_H5nwyj3V [fillcolor=gray, fontcolor=black, fontsize=9, label=task_H5nwyj3V, shape=ellipse, style=filled]; -selector_qm8Zexyr -> task_H5nwyj3V; -selector_9SJMhckE [fillcolor=cyan, fontcolor=black, fontsize=9, label=selector_9SJMhckE, shape=octagon, style=filled]; -sequence_oUFBg8fG -> selector_9SJMhckE; -cond_D8fqA35b [fillcolor=gray, fontcolor=black, fontsize=9, label=cond_D8fqA35b, shape=ellipse, style=filled]; -selector_9SJMhckE -> cond_D8fqA35b; -task_EikNgjy4 [fillcolor=gray, fontcolor=black, fontsize=9, label=task_EikNgjy4, shape=ellipse, style=filled]; -selector_9SJMhckE -> task_EikNgjy4; -} diff --git a/robowaiter/behavior_tree/ptml/CoffeeDelivery.ptml b/robowaiter/behavior_tree/ptml/test/CoffeeDelivery.ptml similarity index 64% rename from robowaiter/behavior_tree/ptml/CoffeeDelivery.ptml rename to robowaiter/behavior_tree/ptml/test/CoffeeDelivery.ptml index 7f1b11f..f6ce360 100644 --- a/robowaiter/behavior_tree/ptml/CoffeeDelivery.ptml +++ b/robowaiter/behavior_tree/ptml/test/CoffeeDelivery.ptml @@ -9,27 +9,27 @@ sequence{ selector{ cond CoffeeCupFound() - task FindCoffeeCup() + act FindCoffeeCup() sequence{ cond SeqTest() - task Move(1.2, 2, 2.3, True) - task Grasp() + act Move(1.2, 2, 2.3, True) + act Grasp() parallel 3 { - cond Istask() - task TestTask() + cond Isact() + act Testact() } } } selector{ cond CoffeeCupGrasped() - task GraspCoffeeCup() + act GraspCoffeeCup() } selector{ cond DestinationAReached() - task ReachDestinationA() + act ReachDestinationA() } selector{ cond CoffeeCupPlaced() - task PlaceCoffeeCup() + act PlaceCoffeeCup() } } diff --git a/robowaiter/behavior_tree/ptml/test/Test.ptml b/robowaiter/behavior_tree/ptml/test/Test.ptml new file mode 100644 index 0000000..62564fa --- /dev/null +++ b/robowaiter/behavior_tree/ptml/test/Test.ptml @@ -0,0 +1,7 @@ +selector{ + sequence{ + cond Chatting() + act DealChat() + } + act At(Coffee,Table) +} diff --git a/robowaiter/behavior_tree/ptml/test/__init__.py b/robowaiter/behavior_tree/ptml/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robowaiter/behavior_tree/ptml/test/ptml_test.py b/robowaiter/behavior_tree/ptml/test/ptml_test.py new file mode 100644 index 0000000..d4c84ba --- /dev/null +++ b/robowaiter/behavior_tree/ptml/test/ptml_test.py @@ -0,0 +1,28 @@ +import os +import py_trees as ptree + +from robowaiter.scene.scene import Scene +from robowaiter.behavior_tree.ptml.ptmlCompiler import load + +import os +from robowaiter import Robot, task_map + + +if __name__ == '__main__': + TASK_NAME = 'OT' + + # create robot + project_path = "../../../" + ptml_path = os.path.join(project_path, 'behavior_tree/ptml/test/Test.ptml') + behavior_lib_path = os.path.join(project_path, 'behavior_lib') + + robot = Robot(ptml_path, behavior_lib_path) + + # create task + task = task_map[TASK_NAME](robot) + + ptree.display.render_dot_tree(robot.bt.root,name="test") + # build and tick + # scene.BT = ptree.trees.BehaviourTree(scene.BT) + # todo: tick this bt + print(robot.bt) \ No newline at end of file diff --git a/robowaiter/behavior_tree/test/ptml.g4 b/robowaiter/behavior_tree/test/ptml.g4 new file mode 100644 index 0000000..0a545a9 --- /dev/null +++ b/robowaiter/behavior_tree/test/ptml.g4 @@ -0,0 +1,20 @@ +grammar ptml; + +root : tree+ EOF; + +tree : internal_node '{' (action_sign|tree)+ '}' ; +internal_node : 'sequence' | 'selector' | 'parallel' Integer ; +action_sign : ('task'|'cond') Names '(' action_parm? ')'; +action_parm : (Integer|Float|boolean) (',' (Integer|Float|boolean))* ; +// var_decls : var_type Names ; +// var_type : 'int' | 'float' | 'bool' | 'string' ; +boolean : 'True' | 'False' ; + +Names : [a-zA-Z_][a-zA-Z_0-9]* ; +Integer : '-'?[1-9][0-9]* | '0' ; +Float : [0-9]+'.'[0-9]* | '.'[0-9]+ ; + +// comments +LINE_COMMENT : '//' .*? '\r'?'\n' -> skip ; +// useless +WS : [ \t\u000C\r\n]+ -> skip ; diff --git a/robowaiter/robot/Default.ptml b/robowaiter/robot/Default.ptml index 52435e2..a854839 100644 --- a/robowaiter/robot/Default.ptml +++ b/robowaiter/robot/Default.ptml @@ -1,4 +1,7 @@ -sequence{ - cond Chatting() - task DealChat() +selector{ + sequence{ + cond Chatting() + act DealChat() + } + act At("Coffee","Table") } diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index f266f8c..c44f692 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -59,15 +59,16 @@ class Scene: collision: str, info: str """ - def __init__(self,robot, sceneID=0): + def __init__(self,robot=None, sceneID=0): self.sceneID = sceneID self.use_offset = True self.start_time = time.time() self.time = 0 # init robot - robot.set_scene(self) - robot.load_BT() + if robot: + robot.set_scene(self) + robot.load_BT() self.robot = robot diff --git a/robowaiter/scene/tasks/Open_tasks.py b/robowaiter/scene/tasks/Open_tasks.py index 47a4792..973b2bb 100644 --- a/robowaiter/scene/tasks/Open_tasks.py +++ b/robowaiter/scene/tasks/Open_tasks.py @@ -17,13 +17,16 @@ class SceneOT(Scene): super().__init__(robot) def _reset(self): - self.reset_sim() - self.add_walker(1085, 2630, 220) self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) + def _run(self): pass def _step(self): - pass \ No newline at end of file + + if int(self.time) == 5: + print("顾客说:给我一杯咖啡") + self.chat_bubble('顾客说:给我一杯咖啡') + self.state['chat_list'].append('给我一杯咖啡') diff --git a/run_robowaiter.py b/run_robowaiter.py index afef9f9..a137bf8 100644 --- a/run_robowaiter.py +++ b/run_robowaiter.py @@ -1,7 +1,7 @@ import os from robowaiter import Robot, task_map -TASK_NAME = 'GQA' +TASK_NAME = 'OT' # create robot project_path = "./robowaiter" diff --git a/robowaiter/behavior_lib/Behavior.py b/zoo/behavior_lib/Behavior.py similarity index 90% rename from robowaiter/behavior_lib/Behavior.py rename to zoo/behavior_lib/Behavior.py index 0dc9b61..850459a 100644 --- a/robowaiter/behavior_lib/Behavior.py +++ b/zoo/behavior_lib/Behavior.py @@ -1,7 +1,7 @@ import py_trees as ptree from typing import Any -# base Behavior +# _base Behavior class Bahavior(ptree.behaviour.Behaviour): scene = None def __init__(self, name: str, scene): @@ -15,7 +15,7 @@ class Bahavior(ptree.behaviour.Behaviour): return super().initialise() def _update(self) -> ptree.common.Status: - print("this is just a base behavior node.") + print("this is just a _base behavior node.") def update(self) -> ptree.common.Status: diff --git a/robowaiter/behavior_lib/Chatting.py b/zoo/behavior_lib/Chatting.py similarity index 91% rename from robowaiter/behavior_lib/Chatting.py rename to zoo/behavior_lib/Chatting.py index 284e5c1..563c23f 100644 --- a/robowaiter/behavior_lib/Chatting.py +++ b/zoo/behavior_lib/Chatting.py @@ -1,6 +1,6 @@ import py_trees as ptree from typing import Any -from robowaiter.behavior_lib.Behavior import Bahavior +from robowaiter.behavior_lib._base.Behavior import Bahavior class Chatting(Bahavior): def __init__(self, name: str, scene): diff --git a/robowaiter/behavior_lib/CoffeeCupFound.py b/zoo/behavior_lib/CoffeeCupFound.py similarity index 90% rename from robowaiter/behavior_lib/CoffeeCupFound.py rename to zoo/behavior_lib/CoffeeCupFound.py index 3357a01..e7130ec 100644 --- a/robowaiter/behavior_lib/CoffeeCupFound.py +++ b/zoo/behavior_lib/CoffeeCupFound.py @@ -1,6 +1,6 @@ import py_trees as ptree from typing import Any -from robowaiter.behavior_lib.Behavior import Bahavior +from robowaiter.behavior_lib._base.Behavior import Bahavior class CoffeeCupFound(Bahavior): def __init__(self, name: str, scene): diff --git a/robowaiter/behavior_lib/CoffeeCupGrasped.py b/zoo/behavior_lib/CoffeeCupGrasped.py similarity index 90% rename from robowaiter/behavior_lib/CoffeeCupGrasped.py rename to zoo/behavior_lib/CoffeeCupGrasped.py index a2e00b8..32c010a 100644 --- a/robowaiter/behavior_lib/CoffeeCupGrasped.py +++ b/zoo/behavior_lib/CoffeeCupGrasped.py @@ -1,6 +1,6 @@ import py_trees as ptree from typing import Any -from robowaiter.behavior_lib.Behavior import Bahavior +from robowaiter.behavior_lib._base.Behavior import Bahavior class CoffeeCupGrasped(Bahavior): def __init__(self, name: str, scene): diff --git a/robowaiter/behavior_lib/CoffeeCupPlaced.py b/zoo/behavior_lib/CoffeeCupPlaced.py similarity index 100% rename from robowaiter/behavior_lib/CoffeeCupPlaced.py rename to zoo/behavior_lib/CoffeeCupPlaced.py diff --git a/robowaiter/behavior_lib/DealChat.py b/zoo/behavior_lib/DealChat.py similarity index 92% rename from robowaiter/behavior_lib/DealChat.py rename to zoo/behavior_lib/DealChat.py index be8c3c2..c1fee07 100644 --- a/robowaiter/behavior_lib/DealChat.py +++ b/zoo/behavior_lib/DealChat.py @@ -1,6 +1,6 @@ import py_trees as ptree from typing import Any -from robowaiter.behavior_lib.Behavior import Bahavior +from robowaiter.behavior_lib._base.Behavior import Bahavior from robowaiter.llm_client.ask_llm import ask_llm class DealChat(Bahavior): diff --git a/robowaiter/behavior_lib/DestinationAReached.py b/zoo/behavior_lib/DestinationAReached.py similarity index 100% rename from robowaiter/behavior_lib/DestinationAReached.py rename to zoo/behavior_lib/DestinationAReached.py diff --git a/robowaiter/behavior_lib/FindCoffeeCup.py b/zoo/behavior_lib/FindCoffeeCup.py similarity index 100% rename from robowaiter/behavior_lib/FindCoffeeCup.py rename to zoo/behavior_lib/FindCoffeeCup.py diff --git a/zoo/behavior_lib/Grasp.py b/zoo/behavior_lib/Grasp.py new file mode 100644 index 0000000..57c5708 --- /dev/null +++ b/zoo/behavior_lib/Grasp.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class Grasp(ptree.behaviour.Behaviour): + + def __init__(self, name: str, scene): + super().__init__(name) + + def setup(self, **kwargs: Any) -> None: + return super().setup(**kwargs) + + def initialise(self) -> None: + return super().initialise() + + def _update(self) -> ptree.common.Status: + print('Start checking IsChatting...') + return ptree.common.Status.SUCCESS + + def terminate(self, new_status: ptree.common.Status) -> None: + return super().terminate(new_status) + \ No newline at end of file diff --git a/robowaiter/behavior_lib/GraspCoffeeCup.py b/zoo/behavior_lib/GraspCoffeeCup.py similarity index 100% rename from robowaiter/behavior_lib/GraspCoffeeCup.py rename to zoo/behavior_lib/GraspCoffeeCup.py diff --git a/robowaiter/behavior_lib/Istask.py b/zoo/behavior_lib/Istask.py similarity index 100% rename from robowaiter/behavior_lib/Istask.py rename to zoo/behavior_lib/Istask.py diff --git a/zoo/behavior_lib/MoveTo.py b/zoo/behavior_lib/MoveTo.py new file mode 100644 index 0000000..516e9dc --- /dev/null +++ b/zoo/behavior_lib/MoveTo.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class MoveTo(ptree.behaviour.Behaviour): + + def __init__(self, name: str, scene, a, b, c, d): + super().__init__(name) + + def setup(self, **kwargs: Any) -> None: + return super().setup(**kwargs) + + def initialise(self) -> None: + return super().initialise() + + def _update(self) -> ptree.common.Status: + print('Start checking IsChatting...') + return ptree.common.Status.SUCCESS + + def terminate(self, new_status: ptree.common.Status) -> None: + return super().terminate(new_status) + \ No newline at end of file diff --git a/robowaiter/behavior_lib/PlaceCoffeeCup.py b/zoo/behavior_lib/PlaceCoffeeCup.py similarity index 100% rename from robowaiter/behavior_lib/PlaceCoffeeCup.py rename to zoo/behavior_lib/PlaceCoffeeCup.py diff --git a/robowaiter/behavior_lib/ReachDestinationA.py b/zoo/behavior_lib/ReachDestinationA.py similarity index 100% rename from robowaiter/behavior_lib/ReachDestinationA.py rename to zoo/behavior_lib/ReachDestinationA.py diff --git a/robowaiter/behavior_lib/SeqTest.py b/zoo/behavior_lib/SeqTest.py similarity index 100% rename from robowaiter/behavior_lib/SeqTest.py rename to zoo/behavior_lib/SeqTest.py diff --git a/robowaiter/behavior_lib/TestTask.py b/zoo/behavior_lib/TestTask.py similarity index 100% rename from robowaiter/behavior_lib/TestTask.py rename to zoo/behavior_lib/TestTask.py diff --git a/zoo/behavior_lib/__init__.py b/zoo/behavior_lib/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/zoo/opt_bt_expansion/BehaviorTree.py b/zoo/opt_bt_expansion/BehaviorTree.py new file mode 100644 index 0000000..4513a20 --- /dev/null +++ b/zoo/opt_bt_expansion/BehaviorTree.py @@ -0,0 +1,95 @@ + +#叶结点 +class Leaf: + def __init__(self,type,content,mincost): + self.type=type + self.content=content #conditionset or action + self.parent=None + self.parent_index=0 + self.mincost=mincost + + # tick 叶节点,返回返回值以及对应的条件或行动对象self.content + def tick(self,state): + if self.type=='cond': + if self.content <= state: + return 'success',self.content + else: + return 'failure',self.content + if self.type=='act': + if self.content.pre<=state: + return 'running',self.content #action + else: + return 'failure',self.content + + def __str__(self): + print( self.content) + return '' + + def print_nodes(self): + print(self.content) + + def count_size(self): + return 1 + + +#可能包含控制结点的行为树 +class ControlBT: + def __init__(self,type): + self.type=type + self.children=[] + self.parent=None + self.parent_index=0 + + + def add_child(self,subtree_list): + for subtree in subtree_list: + self.children.append(subtree) + subtree.parent=self + subtree.parent_index=len(self.children)-1 + + # tick行为树,根据不同控制结点逻辑tick子结点 + def tick(self,state): + if len(self.children) < 1: + print("error,no child") + if self.type =='?':#选择结点,即或结点 + for child in self.children: + val,obj=child.tick(state) + if val=='success': + return val,obj + if val=='running': + return val,obj + return 'failure','?fails' + if self.type =='>':#顺序结点,即与结点 + for child in self.children: + val,obj=child.tick(state) + if val=='failure': + return val,obj + if val=='running': + return val,obj + return 'success', '>success' + if self.type =='act':#行动结点 + return self.children[0].tick(state) + if self.type =='cond':#条件结点 + return self.children[0].tick(state) + + def getFirstChild(self): + return self.children[0] + + def __str__(self): + print(self.type+'\n') + for child in self.children: + print (child) + return '' + + def print_nodes(self): + print(self.type) + for child in self.children: + child.print_nodes() + + # 递归统计树中结点数 + def count_size(self): + result=1 + for child in self.children: + result+= child.count_size() + return result + diff --git a/opt_bt_expansion/MakeCoffee.ptml b/zoo/opt_bt_expansion/MakeCoffee.ptml similarity index 100% rename from opt_bt_expansion/MakeCoffee.ptml rename to zoo/opt_bt_expansion/MakeCoffee.ptml diff --git a/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py b/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py new file mode 100644 index 0000000..6ec11d5 --- /dev/null +++ b/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py @@ -0,0 +1,210 @@ +import copy +import random +from robowaiter.behavior_tree.obtea.BehaviorTree import Leaf,ControlBT + +class CondActPair: + def __init__(self, cond_leaf,act_leaf): + self.cond_leaf = cond_leaf + self.act_leaf = act_leaf + +#定义行动类,行动包括前提、增加和删除影响 +class Action: + def __init__(self,name='anonymous action',pre=set(),add=set(),del_set=set(),cost=1): + self.pre=copy.deepcopy(pre) + self.add=copy.deepcopy(add) + self.del_set=copy.deepcopy(del_set) + self.name=name + self.cost=cost + + def __str__(self): + return self.name + +#生成随机状态 +def generate_random_state(num): + result = set() + for i in range(0,num): + if random.random()>0.5: + result.add(i) + return result +#从状态和行动生成后继状态 +def state_transition(state,action): + if not action.pre <= state: + print ('error: action not applicable') + return state + new_state=(state | action.add) - action.del_set + return new_state + + +#本文所提出的完备规划算法 +class OptBTExpAlgorithm: + def __init__(self,verbose=False): + self.bt = None + self.nodes=[] + self.traversed=[] + self.mounted=[] + self.conditions=[] + self.conditions_index=[] + self.verbose=verbose + + def clear(self): + self.bt = None + self.nodes = [] + self.traversed = [] + self.conditions = [] + self.conditions_index = [] + + #运行规划算法,从初始状态、目标状态和可用行动,计算行为树self.bt + def run_algorithm(self,goal,actions): + if self.verbose: + print("\n算法开始!") + + + self.bt = ControlBT(type='cond') + # 初始行为树只包含目标条件 + gc_node = Leaf(type='cond', content=goal,mincost=0) # 为了统一,都成对出现 + ga_node = Leaf(type='act', content=None, mincost=0) + subtree = ControlBT(type='?') + subtree.add_child([copy.deepcopy(gc_node)]) # 子树首先保留所扩展结 + self.bt.add_child([subtree]) + + # self.conditions.append(goal) + cond_anc_pair = CondActPair(cond_leaf=gc_node,act_leaf=ga_node) + self.nodes.append(copy.deepcopy(cond_anc_pair)) # the set of explored but unexpanded conditions + self.traversed = [goal] # the set of expanded conditions + + + while len(self.nodes)!=0: + + # Find the condition for the shortest cost path + pair_node = None + min_cost = float ('inf') + index= -1 + for i,cond_anc_pair in enumerate(self.nodes): + if cond_anc_pair.cond_leaf.mincost < min_cost: + min_cost = cond_anc_pair.cond_leaf.mincost + pair_node = copy.deepcopy(cond_anc_pair) + index = i + break + + if self.verbose: + print("选择扩展条件结点:",pair_node.cond_leaf.content) + # Update self.nodes and self.traversed + self.nodes.pop(index) # the set of explored but unexpanded conditions. self.nodes.remove(pair_node) + c = pair_node.cond_leaf.content # 子树所扩展结点对应的条件(一个文字的set) + + # Mount the action node and extend BT. T = Eapand(T,c,A(c)) + if c!=goal: + sequence_structure = ControlBT(type='>') + sequence_structure.add_child( + [copy.deepcopy(pair_node.cond_leaf), copy.deepcopy(pair_node.act_leaf)]) + subtree.add_child([copy.deepcopy(sequence_structure)]) # subtree 是回不断变化的,它的父亲是self.bt + if self.verbose: + print("完成扩展 a_node= %s,对应的新条件 c_attr= %s,mincost=%d" \ + % (cond_anc_pair.act_leaf.content.name, cond_anc_pair.cond_leaf.content, + cond_anc_pair.cond_leaf.mincost)) + + if self.verbose: + print("遍历所有动作, 寻找符合条件的动作") + # 遍历所有动作, 寻找符合条件的动作 + current_mincost = pair_node.cond_leaf.mincost # 当前的最短路径是多少 + + for i in range(0, len(actions)): + if not c & ((actions[i].pre | actions[i].add) - actions[i].del_set) <= set(): + if (c - actions[i].del_set) == c: + if self.verbose: + print("———— 满足条件可以扩展") + c_attr = (actions[i].pre | c) - actions[i].add + + # 剪枝操作,现在的条件是以前扩展过的条件的超集 + valid = True + for j in self.traversed: # 剪枝操作 + if j <= c_attr: + valid = False + if self.verbose: + print("———— --被剪枝") + break + + if valid: + # 把符合条件的动作节点都放到列表里 + if self.verbose: + print("———— -- %s 符合条件放入列表" % actions[i].name) + c_attr_node = Leaf(type='cond', content=c_attr, mincost=current_mincost + actions[i].cost) + a_attr_node = Leaf(type='act', content=actions[i], mincost=current_mincost + actions[i].cost) + cond_anc_pair = CondActPair(cond_leaf=c_attr_node, act_leaf=a_attr_node) + self.nodes.append(copy.deepcopy(cond_anc_pair)) # condition node list + self.traversed.append(c_attr) # 重点 the set of expanded conditions + + if self.verbose: + print("算法结束!\n") + return True + + def print_solution(self): + print("========= BT ==========") # 树的bfs遍历 + nodes_ls = [] + nodes_ls.append(self.bt) + while len(nodes_ls) != 0: + parnode = nodes_ls[0] + print("Parrent:", parnode.type) + for child in parnode.children: + if isinstance(child, Leaf): + print("---- Leaf:", child.content) + elif isinstance(child, ControlBT): + print("---- ControlBT:", child.type) + nodes_ls.append(child) + print() + nodes_ls.pop(0) + print("========= BT ==========\n") + + # 返回所有能到达目标状态的初始状态 + def get_all_state_leafs(self): + state_leafs=[] + + nodes_ls = [] + nodes_ls.append(self.bt) + while len(nodes_ls) != 0: + parnode = nodes_ls[0] + for child in parnode.children: + if isinstance(child, Leaf): + if child.type == "cond": + state_leafs.append(child.content) + elif isinstance(child, ControlBT): + nodes_ls.append(child) + nodes_ls.pop(0) + + return state_leafs + + + # 树的dfs + def dfs_ptml(self,parnode): + for child in parnode.children: + if isinstance(child, Leaf): + if child.type == 'cond': + self.ptml_string += "cond " + c_set_str = ', '.join(map(str, child.content)) + "\n" + self.ptml_string += c_set_str + elif child.type == 'act': + self.ptml_string += 'act '+child.content.name+"\n" + elif isinstance(child, ControlBT): + if parnode.type == '?': + self.ptml_string += "selector{\n" + self.dfs_ptml(parnode=child) + elif parnode.type == '>': + self.ptml_string += "sequence{\n" + self.dfs_ptml( parnode=child) + self.ptml_string += '}\n' + + + def get_ptml(self): + self.ptml_string = "selector{\n" + self.dfs_ptml(self.bt.children[0]) + self.ptml_string += '}\n' + return self.ptml_string + + + def save_ptml_file(self,file_name): + self.ptml_string = "selector{\n" + self.dfs_ptml(self.bt.children[0]) + self.ptml_string += '}\n' + with open(f'./{file_name}.ptml', 'w') as file: + file.write(self.ptml_string) + return self.ptml_string diff --git a/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png b/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png new file mode 100644 index 0000000..6453647 Binary files /dev/null and b/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png differ diff --git a/zoo/opt_bt_expansion/README.md b/zoo/opt_bt_expansion/README.md new file mode 100644 index 0000000..1ddd4a6 --- /dev/null +++ b/zoo/opt_bt_expansion/README.md @@ -0,0 +1,125 @@ + + +## 代码说明 + +### 1. `BehaviorTree.py` 实现行为树叶子结点和非叶子结点的定义 + +- **Leaf**:表示叶节点,可以是动作(`act`)或条件(`cond`)。 +- **ControlBT**:代表可能包含控制节点的行为树。它们可以是选择器(`?`)、序列(`>`)、动作节点(`act`)或条件节点(`cond`)。 +- 上述两个类都包含 `tick` 方法。 + +### 2. `OptimalBTExpansionAlgorithm.py` 实现最优行为树扩展算法 + +![image-20231103191141047](README.assets/image-20231103191141047.png) + +定义行动类 +```python +#定义行动类,行动包括前提、增加和删除影响 +class Action: + def __init__(self,name='anonymous action',pre=set(),add=set(),del_set=set(),cost=1): + self.pre=copy.deepcopy(pre) + self.add=copy.deepcopy(add) + self.del_set=copy.deepcopy(del_set) + self.name=name + self.cost=cost + + def __str__(self): + return self.name +``` + +调用算法 +```python +algo = OptBTExpAlgorithm(verbose=True) +algo.clear() +algo.run_algorithm(start, goal, actions) # 使用算法得到行为树在 algo.bt +algo.print_solution() # 打印行为树 +val, obj = algo.bt.tick(state) # 执行行为树 +algo.save_ptml_file("bt.ptml") # 保存行为树为 ptml 文件 +``` + +### 3. **`tools.py`** 实现打印数据、行为树测试等模块 + +使用方法 + +```python +print_action_data_table(goal,start,actions) # 打印所有变量 + +# 行为树鲁棒性测试,随机生成规划问题 +# 设置生成规划问题集的超参数:文字数、解深度、迭代次数 +seed=1 +literals_num=10 +depth = 10 +iters= 10 +BTTest(seed=seed,literals_num=literals_num,depth=depth,iters=iters) +``` + +### 4. `example.py` 中设计规划案例 goals, start, actions + +```python +def MoveBtoB (): + actions=[] + a = Action(name="Move(b,ab)") + a.pre={'Free(ab)','WayClear'} + a.add={'At(b,ab)'} + a.del_set= {'Free(ab)','At(b,pb)'} + a.cost = 1 + actions.append(a) + + a=Action(name="Move(s,ab)") + a.pre={'Free(ab)'} + a.add={'Free(ab)','WayClear'} + a.del_set={'Free(ab)','At(s,ps)'} + a.cost = 1 + actions.append(a) + + a=Action(name="Move(s,as)") + a.pre={'Free(as)'} + a.add={'At(s,ps)','WayClear'} + a.del_set={'Free(as)','At(s,ps)'} + a.cost = 1 + actions.append(a) + + start = {'Free(ab)','Free(as)','At(b,pb)','At(s,ps)'} + goal= {'At(b,ab)'} + return goal,start,actions +``` + +### 5. `opt_bt_exp_main.py` 为主函数,在此演示如何调用最优行为树扩展算法得到完全扩展最优行为树 + +初始化的时候:传入 actions (包含 pre,add,del,cost). +调用的时候,传入 goal 状态集合 (set类型),返回完全最优扩展行为树的 ptml 形式 (string类型) + +```python +actions=[ + Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1) + ………… +] +algo = BTOptExpInterface(actions) + +goal = {'At(Table,Coffee)'} +ptml_string = algo.process(goal,start) +print(ptml_string) + +``` +两种检测方法,用于检测当前状态 `start` 能否到达目标状态 `goal` + +```python +# 判断初始状态能否到达目标状态 +start = {'At(Robot,Bar)', 'Holding(VacuumCup)', 'Available(Table)', 'Available(CoffeeMachine)','Available(FrontDesk)'} + +# 方法一:算法返回所有可能的初始状态,在里面看看有没有对应的初始状态 +right_bt = algo.find_all_leaf_states_contain_start(start) +if not right_bt: + print("ERROR1: The current state cannot reach the goal state!") +else: + print("Right1: The current state can reach the goal state!") + +# 方法二:预先跑一边行为树,看能否到达目标状态 +right_bt2 = algo.run_bt_from_start(goal,start) +if not right_bt2: + print("ERROR2: The current state cannot reach the goal state!") +else: + print("Right2: The current state can reach the goal state!") + +``` + diff --git a/zoo/opt_bt_expansion/__init__.py b/zoo/opt_bt_expansion/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/zoo/opt_bt_expansion/examples.py b/zoo/opt_bt_expansion/examples.py new file mode 100644 index 0000000..019af9b --- /dev/null +++ b/zoo/opt_bt_expansion/examples.py @@ -0,0 +1,174 @@ + +from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action + + + +def MakeCoffee(): + actions=[ + Action(name='Put(Table,Coffee)', pre={'Holding(Coffee)','At(Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), + Action(name='Put(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), + + Action(name='Grasp(Coffee)', pre={'NotHolding','At(Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), + + Action(name='MoveTo(Table)', pre={'Exist(Table)'}, add={'At(Table)'}, del_set={'At(FrontDesk)','At(Coffee)','At(CoffeeMachine)'}, cost=1), + Action(name='MoveTo(Coffee)', pre={'Exist(Coffee)'}, add={'At(Coffee)'}, del_set={'At(FrontDesk)','At(Table)','At(CoffeeMachine)'}, cost=1), + Action(name='MoveTo(CoffeeMachine)', pre={'Exist(CoffeeMachine)'}, add={'At(CoffeeMachine)'}, del_set={'At(FrontDesk)','At(Coffee)','At(Table)'}, cost=1), + + Action(name='OpCoffeeMachine', pre={'At(CoffeeMachine)','NotHolding'}, add={'Exist(Coffee)','At(Coffee)'}, del_set=set(), cost=1), + ] + + start = {'At(FrontDesk)','Holding(VacuumCup)','Exist(Table)','Exist(CoffeeMachine)','Exist(FrontDesk)'} + goal = {'At(Table,Coffee)'} + return goal,start,actions + +# 本例子中,将 VacuumCup 放到 FrontDesk,比 MoveTo(Table) 再 Put(Table,VacuumCup) 的 cost 要小 +def MakeCoffeeCost(): + actions=[ + Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), + Action(name='PutDown(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Robot,Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), + + Action(name='PickUp(Coffee)', pre={'NotHolding','At(Robot,Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), + + Action(name='MoveTo(Table)', pre={'Available(Table)'}, add={'At(Robot,Table)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,CoffeeMachine)'}, cost=1), + Action(name='MoveTo(Coffee)', pre={'Available(Coffee)'}, add={'At(Robot,Coffee)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Table)','At(Robot,CoffeeMachine)'}, cost=1), + Action(name='MoveTo(CoffeeMachine)', pre={'Available(CoffeeMachine)'}, add={'At(Robot,CoffeeMachine)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,Table)'}, cost=1), + + Action(name='OpCoffeeMachine', pre={'At(Robot,CoffeeMachine)','NotHolding'}, add={'Available(Coffee)','At(Robot,Coffee)'}, del_set=set(), cost=1), + ] + + start = {'At(Robot,Bar)','Holding(VacuumCup)','Available(Table)','Available(CoffeeMachine)','Available(FrontDesk)'} + goal = {'At(Table,Coffee)'} + + return goal,start,actions + + +# test +def Test(): + actions=[ + Action(name='a1', pre={6}, add={0,2,4}, del_set={1,5}, cost=1), + Action(name='a2', pre=set(), add={0,1}, del_set=set(), cost=1), + Action(name='a3', pre={1,6}, add={0,2,3,5}, del_set={1,6}, cost=1), + Action(name='a4', pre={0,2,3}, add={4,5}, del_set={0,6}, cost=1), + Action(name='a5', pre={0,1,4}, add={2,3,6}, del_set={0}, cost=1), + ] + + start = {1,2,6} + goal={0,1,2,4,6} + return goal,start,actions + +# def Test(): +# actions=[ +# Action(name='a1', pre={2}, add={1}, del_set=set(), cost=1), +# Action(name='a2', pre=set(), add={1}, del_set={0,2}, cost=1), +# Action(name='a3', pre={1}, add=set(), del_set={0,2}, cost=1), +# Action(name='a4', pre=set(), add={0}, del_set=set(), cost=1), +# Action(name='a5', pre={1}, add={0,2}, del_set={1}, cost=1), +# Action(name='a6', pre={1}, add=set(), del_set={0,1,2}, cost=1), +# Action(name='a7', pre={1}, add={2}, del_set={0, 2}, cost=1), +# ] +# +# start = {1,2} +# goal={0,1} +# return goal,start,actions + + +# todo: 最原始的例子 +def MoveBtoB_num (): + actions=[] + a = Action(name='a1') + a.pre={1,4} + a.add={"c_goal"} + a.del_set={1,4} + a.cost = 1 + actions.append(a) + + a=Action(name='a2') + a.pre={1,2,3} + a.add={"c_goal"} + a.del_set={1,2,3} + a.cost = 1 + actions.append(a) + + a=Action(name='a3') + a.pre={1,2} + a.add={4} + a.del_set={2} + a.cost = 1 + actions.append(a) + + a=Action(name='a4') + a.pre={"c_start"} + a.add={1,2,3} + a.del_set={"c_start",4} + a.cost = 1 + actions.append(a) + + start = {"c_start"} + goal={"c_goal"} + return goal,start,actions + + +# todo: 最原始的例子 +def MoveBtoB (): + actions=[] + a = Action(name="Move(b,ab)") #'movebtob' + a.pre={'Free(ab)','WayClear'} #{1,2} + a.add={'At(b,ab)'} #{3} + a.del_set= {'Free(ab)','At(b,pb)'} #{1,4} + a.cost = 1 + actions.append(a) + + a=Action(name="Move(s,ab)") #'moveatob' + a.pre={'Free(ab)'} #{1} + a.add={'Free(ab)','WayClear'} #{5,2} + a.del_set={'Free(ab)','At(s,ps)'} #{1,6} + a.cost = 1 + actions.append(a) + + a=Action(name="Move(s,as)") #'moveatoa' + a.pre={'Free(as)'} #{7} + a.add={'At(s,ps)','WayClear'} #{8,2} + a.del_set={'Free(as)','At(s,ps)'} #{7,6} + a.cost = 1 + actions.append(a) + + start = {'Free(ab)','Free(as)','At(b,pb)','At(s,ps)'} #{1,7,4,6} + goal= {'At(b,ab)'} #{3} + return goal,start,actions + + +# 小蔡师兄论文里的例子 +def Cond2BelongsToCond3(): + actions=[] + a = Action(name='a1') + a.pre={1,4} + a.add={"c_goal"} + a.del_set={1,4} + a.cost = 1 + actions.append(a) + + a=Action(name='a2') + a.pre={1,2,3} + a.add={"c_goal"} + a.del_set={1,2,3} + a.cost = 100 + actions.append(a) + + a=Action(name='a3') + a.pre={1,2} + a.add={4} + a.del_set={2} + a.cost = 1 + actions.append(a) + + a=Action(name='a4') + a.pre={"c_start"} + a.add={1,2,3} + a.del_set={"c_start",4} + a.cost = 1 + actions.append(a) + + start = {"c_start"} + goal={"c_goal"} + return goal,start,actions + diff --git a/opt_bt_expansion/opt_bt_exp_main.py b/zoo/opt_bt_expansion/opt_bt_exp_main.py similarity index 92% rename from opt_bt_expansion/opt_bt_exp_main.py rename to zoo/opt_bt_expansion/opt_bt_exp_main.py index 5047370..87329b6 100644 --- a/opt_bt_expansion/opt_bt_exp_main.py +++ b/zoo/opt_bt_expansion/opt_bt_exp_main.py @@ -1,7 +1,4 @@ -from opt_bt_expansion.BehaviorTree import Leaf,ControlBT # 行为结点类:叶子结点和非叶子节点 -from opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm,state_transition # 调用最优行为树扩展算法 -from opt_bt_expansion.tools import print_action_data_table,BTTest -from opt_bt_expansion.examples import MoveBtoB_num,MoveBtoB,Cond2BelongsToCond3 # 导入三个例子 +from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm,state_transition # 调用最优行为树扩展算法 from opt_bt_expansion.examples import * diff --git a/zoo/opt_bt_expansion/tools.py b/zoo/opt_bt_expansion/tools.py new file mode 100644 index 0000000..1af057d --- /dev/null +++ b/zoo/opt_bt_expansion/tools.py @@ -0,0 +1,167 @@ + + +from tabulate import tabulate +import numpy as np +import random +from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm +import time + + +def print_action_data_table(goal,start,actions): + data = [] + for a in actions: + data.append([a.name ,a.pre ,a.add ,a.del_set ,a.cost]) + data.append(["Goal" ,goal ," " ,"Start" ,start]) + print(tabulate(data, headers=["Name", "Pre", "Add" ,"Del" ,"Cost"], tablefmt="fancy_grid")) # grid plain simple github fancy_grid + + +# 从状态随机生成一个行动 +def generate_from_state(act,state,num): + for i in range(0,num): + if i in state: + if random.random() >0.5: + act.pre.add(i) + if random.random() >0.5: + act.del_set.add(i) + continue + if random.random() > 0.5: + act.add.add(i) + continue + if random.random() >0.5: + act.del_set.add(i) + +def print_action(act): + print (act.pre) + print(act.add) + print(act.del_set) + + + +#行为树测试代码 +def BTTest(seed=1,literals_num=10,depth=10,iters=10,total_count=1000): + print("============= BT Test ==============") + random.seed(seed) + # 设置生成规划问题集的超参数:文字数、解深度、迭代次数 + literals_num=literals_num + depth = depth + iters= iters + total_tree_size = [] + total_action_num = [] + total_state_num = [] + total_steps_num=[] + #fail_count=0 + #danger_count=0 + success_count =0 + failure_count = 0 + planning_time_total = 0.0 + # 实验1000次 + for count in range (total_count): + + action_num = 1 + + # 生成一个规划问题,包括随机的状态和行动,以及目标状态 + states = [] + actions = [] + start = generate_random_state(literals_num) + state = start + states.append(state) + #print (state) + for i in range (0,depth): + a = Action() + generate_from_state(a,state,literals_num) + if not a in actions: + a.name = "a"+str(action_num) + action_num+=1 + actions.append(a) + state = state_transition(state,a) + if state in states: + pass + else: + states.append(state) + #print(state) + + goal = states[-1] + state = start + for i in range (0,iters): + a = Action() + generate_from_state(a,state,literals_num) + if not a in actions: + a.name = "a"+str(action_num) + action_num+=1 + actions.append(a) + state = state_transition(state,a) + if state in states: + pass + else: + states.append(state) + state = random.sample(states,1)[0] + + # 选择测试本文算法btalgorithm,或对比算法weakalgorithm + algo = OptBTExpAlgorithm() + #algo = Weakalgorithm() + start_time = time.time() + # print_action_data_table(goal, start, list(actions)) + if algo.run_algorithm(start, goal, actions):#运行算法,规划后行为树为algo.bt + total_tree_size.append( algo.bt.count_size()-1) + # algo.print_solution() # 打印行为树 + else: + print ("error") + end_time = time.time() + planning_time_total += (end_time-start_time) + + #开始从初始状态运行行为树,测试 + state=start + steps=0 + val, obj = algo.bt.tick(state)#tick行为树,obj为所运行的行动 + while val !='success' and val !='failure':#运行直到行为树成功或失败 + state = state_transition(state,obj) + val, obj = algo.bt.tick(state) + if(val == 'failure'): + print("bt fails at step",steps) + steps+=1 + if(steps>=500):#至多运行500步 + break + if not goal <= state:#错误解,目标条件不在执行后状态满足 + #print ("wrong solution",steps) + failure_count+=1 + + else:#正确解,满足目标条件 + #print ("right solution",steps) + success_count+=1 + total_steps_num.append(steps) + algo.clear() + total_action_num.append(len(actions)) + total_state_num.append(len(states)) + + print ("success:",success_count,"failure:",failure_count)#算法成功和失败次数 + print("Total Tree Size: mean=",np.mean(total_tree_size), "std=",np.std(total_tree_size, ddof=1))#1000次测试树大小 + print ("Total Steps Num: mean=",np.mean(total_steps_num),"std=",np.std(total_steps_num,ddof=1)) + print ("Average number of states:",np.mean(total_state_num))#1000次问题的平均状态数 + print ("Average number of actions",np.mean(total_action_num))#1000次问题的平均行动数 + print("Planning Time Total:",planning_time_total,planning_time_total/1000.0) + print("============ End BT Test ===========") + + # xiao cai + # success: 1000 failure: 0 + # Total Tree Size: mean= 35.303 std= 29.71336526001515 + # Total Steps Num: mean= 1.898 std= 0.970844240101644 + # Average number of states: 20.678 + # Average number of actions 20.0 + # Planning Time Total: 0.6280641555786133 0.0006280641555786133 + + # our start + # success: 1000 failure: 0 + # Total Tree Size: mean= 17.945 std= 12.841997192488865 + # Total Steps Num: mean= 1.785 std= 0.8120556843187752 + # Average number of states: 20.678 + # Average number of actions 20.0 + # Planning Time Total: 1.4748523235321045 0.0014748523235321046 + + # our + # success: 1000 failure: 0 + # Total Tree Size: mean= 48.764 std= 20.503626574406358 + # Total Steps Num: mean= 1.785 std= 0.8120556843187752 + # Average number of states: 20.678 + # Average number of actions 20.0 + # Planning Time Total: 3.3271877765655518 0.0033271877765655516 +