EmbodiedAI
/
RoboWaiter
mirror of https://github.com/HPCL-EI/RoboWaiter.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

更改ptml中的包导入方式，使得包内包外都可以调用；Scene类添加load_BT方法

This commit is contained in:
Antiman-cmyk 2023-10-11 20:26:58 +08:00
parent c323fa1da9
commit 9dbd8f2770
7 changed files with 76 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -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/

0
behavior_tree/__init__.py Normal file
View File

0
ptml/__init__.py Normal file
View File

30
ptml/ptmlCompiler.py
View File

@ -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
return ptml.bt_root

98
ptml/ptmlTranslator.py
View File

@ -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

2
ptml/ptml_test.py
View File

@ -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')

6
scene_utils/control.py
View File

@ -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")