From 9dbd8f2770c7336a60654b05bb0b9abc3275354d Mon Sep 17 00:00:00 2001 From: Antiman-cmyk <1519764165@qq.com> Date: Wed, 11 Oct 2023 20:26:58 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=94=B9ptml=E4=B8=AD=E7=9A=84?= =?UTF-8?q?=E5=8C=85=E5=AF=BC=E5=85=A5=E6=96=B9=E5=BC=8F=EF=BC=8C=E4=BD=BF?= =?UTF-8?q?=E5=BE=97=E5=8C=85=E5=86=85=E5=8C=85=E5=A4=96=E9=83=BD=E5=8F=AF?= =?UTF-8?q?=E4=BB=A5=E8=B0=83=E7=94=A8=EF=BC=9BScene=E7=B1=BB=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0load=5FBT=E6=96=B9=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 2 +- behavior_tree/__init__.py | 0 ptml/__init__.py | 0 ptml/ptmlCompiler.py | 30 +++++++----- ptml/ptmlTranslator.py | 98 ++++++++++++++++++++------------------- ptml/ptml_test.py | 2 +- scene_utils/control.py | 6 +++ 7 files changed, 76 insertions(+), 62 deletions(-) create mode 100644 behavior_tree/__init__.py create mode 100644 ptml/__init__.py diff --git a/.gitignore b/.gitignore index 46cfe0c..cee336f 100644 --- a/.gitignore +++ b/.gitignore @@ -150,7 +150,7 @@ cython_debug/ # and can be added to the global gitignore or merged into this file. For a more nuclear # option (not recommended) you can uncomment the following to ignore the entire idea folder. .idea/ -test.py +/test.py # antlr tempory files .antlr/ diff --git a/behavior_tree/__init__.py b/behavior_tree/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ptml/__init__.py b/ptml/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ptml/ptmlCompiler.py b/ptml/ptmlCompiler.py index 58ddf67..020ee8f 100644 --- a/ptml/ptmlCompiler.py +++ b/ptml/ptmlCompiler.py @@ -1,8 +1,16 @@ import os from antlr4 import * -from ptmlTranslator import ptmlTranslator -from ptmlParser import ptmlParser as Parser -from ptmlLexer import ptmlLexer as Lexer + +if "." in __name__: + from .ptmlTranslator import ptmlTranslator + from .ptmlParser import ptmlParser as Parser + from .ptmlLexer import ptmlLexer as Lexer + +else: + from ptmlTranslator import ptmlTranslator + from ptmlParser import ptmlParser as Parser + from ptmlLexer import ptmlLexer as Lexer + def load(ptml_path: str, behaviour_lib_path: str): """_summary_ @@ -17,24 +25,22 @@ def load(ptml_path: str, behaviour_lib_path: str): """ # error handle if not os.path.exists(ptml_path): - raise FileNotFoundError( - 'Given a fault ptml path: {}'.format(ptml_path) - ) + raise FileNotFoundError("Given a fault ptml path: {}".format(ptml_path)) if not os.path.exists(behaviour_lib_path): raise FileNotFoundError( - 'Given a fault behaviour library path: {}'.format(behaviour_lib_path) + "Given a fault behaviour library path: {}".format(behaviour_lib_path) ) - + # noting fault, go next - input_stream = FileStream(ptml_path, encoding='utf-8') + input_stream = FileStream(ptml_path, encoding="utf-8") lexer = Lexer(input_stream) stream = CommonTokenStream(lexer) parser = Parser(stream) tree = parser.root() - + walker = ParseTreeWalker() - ptml = ptmlTranslator() # listener mode + ptml = ptmlTranslator() # listener mode walker.walk(ptml, tree) - return ptml.bt_root \ No newline at end of file + return ptml.bt_root diff --git a/ptml/ptmlTranslator.py b/ptml/ptmlTranslator.py index d2702c2..369760a 100644 --- a/ptml/ptmlTranslator.py +++ b/ptml/ptmlTranslator.py @@ -2,10 +2,16 @@ import shortuuid import py_trees as ptree from antlr4 import * -from ptmlListener import ptmlListener -from ptmlParser import ptmlParser -short_uuid = lambda : shortuuid.ShortUUID().random(length=8) +if "." in __name__: + from .ptmlListener import ptmlListener + from .ptmlParser import ptmlParser +else: + from ptmlListener import ptmlListener + from ptmlParser import ptmlParser + +short_uuid = lambda: shortuuid.ShortUUID().random(length=8) + class ptmlTranslator(ptmlListener): """Translate the ptml language to BT. @@ -13,117 +19,113 @@ class ptmlTranslator(ptmlListener): Args: ptmlListener (_type_): _description_ """ - + def __init__(self) -> None: super().__init__() self.bt_root = None - self.stack = [] - + self.stack = [] + # Enter a parse tree produced by ptmlParser#root. - def enterRoot(self, ctx:ptmlParser.RootContext): + def enterRoot(self, ctx: ptmlParser.RootContext): pass # Exit a parse tree produced by ptmlParser#root. - def exitRoot(self, ctx:ptmlParser.RootContext): + def exitRoot(self, ctx: ptmlParser.RootContext): pass - - + # Enter a parse tree produced by ptmlParser#tree. - def enterTree(self, ctx:ptmlParser.TreeContext): + def enterTree(self, ctx: ptmlParser.TreeContext): type = str(ctx.internal_node().children[0]) match type: - case 'sequence': - tag = 'sequence_' + short_uuid() + case "sequence": + tag = "sequence_" + short_uuid() node = ptree.composites.Sequence(name=tag, memory=False) - case 'selector': - tag = 'selector_' + short_uuid() + case "selector": + tag = "selector_" + short_uuid() node = ptree.composites.Selector(name=tag, memory=False) - case 'parallel': - tag = 'parallel_' + short_uuid() + case "parallel": + tag = "parallel_" + short_uuid() # threshold = int(ctx.children[1]) # default policy, success on all node = ptree.composites.Parallel( - name=tag, - policy=ptree.common.ParallelPolicy.SuccessOnAll + name=tag, policy=ptree.common.ParallelPolicy.SuccessOnAll ) case _: - raise TypeError( - 'Unknown Composite Type: {}'.format(type) - ) - + raise TypeError("Unknown Composite Type: {}".format(type)) + self.stack.append(node) # Exit a parse tree produced by ptmlParser#tree. - def exitTree(self, ctx:ptmlParser.TreeContext): + def exitTree(self, ctx: ptmlParser.TreeContext): if len(self.stack) >= 2: child = self.stack.pop() self.stack[-1].add_child(child) else: self.bt_root = self.stack[0] - # Enter a parse tree produced by ptmlParser#internal_node. - def enterInternal_node(self, ctx:ptmlParser.Internal_nodeContext): - pass - - # Exit a parse tree produced by ptmlParser#internal_node. - def exitInternal_node(self, ctx:ptmlParser.Internal_nodeContext): + def enterInternal_node(self, ctx: ptmlParser.Internal_nodeContext): pass + # Exit a parse tree produced by ptmlParser#internal_node. + def exitInternal_node(self, ctx: ptmlParser.Internal_nodeContext): + pass # Enter a parse tree produced by ptmlParser#action_sign. - def enterAction_sign(self, ctx:ptmlParser.Action_signContext): + def enterAction_sign(self, ctx: ptmlParser.Action_signContext): # cond / task node_type = str(ctx.children[0]) name = str(ctx.Names()) - + # if have params args = [] if len(ctx.children) > 4: params = ctx.action_parm() - + for i in params.children: if isinstance(i, ptmlParser.BooleanContext): args.append(str(i.children[0])) else: args.append(str(i)) - args = ''.join(args) - + args = "".join(args) + # dynamic import - behaviour_lib_config = './behaviour_lib' + if "." in __name__: # in package + behaviour_lib_config = "ptml/behaviour_lib" + else: + behaviour_lib_config = "./behaviour_lib" + import sys + sys.path.append(behaviour_lib_config) - exec('from {} import {}'.format(name, name)) - # - tag = 'cond_' + short_uuid() if node_type == 'cond' else 'task_' + short_uuid() + exec("from {} import {}".format(name, name)) + # + tag = "cond_" + short_uuid() if node_type == "cond" else "task_" + short_uuid() node = None - node = eval('{}(\'{}\', {})'.format(name, tag, args)) + node = eval("{}('{}', {})".format(name, tag, args)) # connect self.stack[-1].add_child(node) # print(self.stack) # Exit a parse tree produced by ptmlParser#action_sign. - def exitAction_sign(self, ctx:ptmlParser.Action_signContext): + def exitAction_sign(self, ctx: ptmlParser.Action_signContext): pass - # Enter a parse tree produced by ptmlParser#action_parm. - def enterAction_parm(self, ctx:ptmlParser.Action_parmContext): + def enterAction_parm(self, ctx: ptmlParser.Action_parmContext): pass # Exit a parse tree produced by ptmlParser#action_parm. - def exitAction_parm(self, ctx:ptmlParser.Action_parmContext): + def exitAction_parm(self, ctx: ptmlParser.Action_parmContext): pass - # Enter a parse tree produced by ptmlParser#boolean. - def enterBoolean(self, ctx:ptmlParser.BooleanContext): + def enterBoolean(self, ctx: ptmlParser.BooleanContext): pass # Exit a parse tree produced by ptmlParser#boolean. - def exitBoolean(self, ctx:ptmlParser.BooleanContext): + def exitBoolean(self, ctx: ptmlParser.BooleanContext): pass - \ No newline at end of file diff --git a/ptml/ptml_test.py b/ptml/ptml_test.py index 6bfb457..b9c8ea1 100644 --- a/ptml/ptml_test.py +++ b/ptml/ptml_test.py @@ -6,7 +6,7 @@ from ptmlCompiler import load if __name__ == '__main__': - project_path = "/home/wu/RoboWaiter/ptml" + project_path = "." ptml_path = os.path.join(project_path, 'CoffeeDelivery.ptml') behavior_lib_path = os.path.join(project_path, 'behaviour_lib') diff --git a/scene_utils/control.py b/scene_utils/control.py index 7293749..ce58029 100644 --- a/scene_utils/control.py +++ b/scene_utils/control.py @@ -6,6 +6,8 @@ import numpy as np from proto import GrabSim_pb2 from proto import GrabSim_pb2_grpc +from ptml import ptmlCompiler + channel = grpc.insecure_channel( "localhost:30001", options=[ @@ -55,6 +57,7 @@ class Scene: def __init__(self, sceneID): self.sceneID = sceneID + self.BT = None self.reset() @property @@ -216,3 +219,6 @@ class Scene: def animation_reset(self): stub.ControlRobot(GrabSim_pb2.ControlInfo(scene=self.sceneID, type=0, action=0)) + + def load_BT(self, ptml_path): + self.BT = ptmlCompiler.load(ptml_path, "ptml/behaviour_lib")