diff --git a/robowaiter/behavior_lib/CoffeeCupFound.py b/robowaiter/behavior_lib/CoffeeCupFound.py new file mode 100644 index 0000000..3357a01 --- /dev/null +++ b/robowaiter/behavior_lib/CoffeeCupFound.py @@ -0,0 +1,20 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib.Behavior import Bahavior + +class CoffeeCupFound(Bahavior): + def __init__(self, name: str, scene): + super().__init__(name, scene) + + 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) diff --git a/robowaiter/behavior_lib/CoffeeCupGrasped.py b/robowaiter/behavior_lib/CoffeeCupGrasped.py new file mode 100644 index 0000000..a2e00b8 --- /dev/null +++ b/robowaiter/behavior_lib/CoffeeCupGrasped.py @@ -0,0 +1,22 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib.Behavior import Bahavior + +class CoffeeCupGrasped(Bahavior): + def __init__(self, name: str, scene): + super().__init__(name, scene) + + + 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/CoffeeCupPlaced.py b/robowaiter/behavior_lib/CoffeeCupPlaced.py new file mode 100644 index 0000000..d4cdb35 --- /dev/null +++ b/robowaiter/behavior_lib/CoffeeCupPlaced.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class CoffeeCupPlaced(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/DestinationAReached.py b/robowaiter/behavior_lib/DestinationAReached.py new file mode 100644 index 0000000..9092023 --- /dev/null +++ b/robowaiter/behavior_lib/DestinationAReached.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class DestinationAReached(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/FindCoffeeCup.py b/robowaiter/behavior_lib/FindCoffeeCup.py new file mode 100644 index 0000000..6781c50 --- /dev/null +++ b/robowaiter/behavior_lib/FindCoffeeCup.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class FindCoffeeCup(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/Grasp.py b/robowaiter/behavior_lib/Grasp.py new file mode 100644 index 0000000..69d42fd --- /dev/null +++ b/robowaiter/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/robowaiter/behavior_lib/GraspCoffeeCup.py new file mode 100644 index 0000000..285804e --- /dev/null +++ b/robowaiter/behavior_lib/GraspCoffeeCup.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class GraspCoffeeCup(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/Istask.py b/robowaiter/behavior_lib/Istask.py new file mode 100644 index 0000000..4a1922c --- /dev/null +++ b/robowaiter/behavior_lib/Istask.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class Istask(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/Move.py b/robowaiter/behavior_lib/Move.py new file mode 100644 index 0000000..4922b73 --- /dev/null +++ b/robowaiter/behavior_lib/Move.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class Move(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/robowaiter/behavior_lib/PlaceCoffeeCup.py new file mode 100644 index 0000000..7cf0bbd --- /dev/null +++ b/robowaiter/behavior_lib/PlaceCoffeeCup.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class PlaceCoffeeCup(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/ReachDestinationA.py b/robowaiter/behavior_lib/ReachDestinationA.py new file mode 100644 index 0000000..18fbe03 --- /dev/null +++ b/robowaiter/behavior_lib/ReachDestinationA.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class ReachDestinationA(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/SeqTest.py b/robowaiter/behavior_lib/SeqTest.py new file mode 100644 index 0000000..e07ab74 --- /dev/null +++ b/robowaiter/behavior_lib/SeqTest.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class SeqTest(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/TestTask.py b/robowaiter/behavior_lib/TestTask.py new file mode 100644 index 0000000..c9c53d7 --- /dev/null +++ b/robowaiter/behavior_lib/TestTask.py @@ -0,0 +1,21 @@ +import py_trees as ptree +from typing import Any + +class TestTask(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/__init__.py b/robowaiter/behavior_lib/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robowaiter/behavior_tree/__init__.py b/robowaiter/behavior_tree/__init__.py new file mode 100644 index 0000000..dac458f --- /dev/null +++ b/robowaiter/behavior_tree/__init__.py @@ -0,0 +1,3 @@ + +# from robowaiter.behavior_tree.behavior_tree import BehaviorTree +from robowaiter.behavior_tree.behavior_tree import load_bt_from_ptml \ No newline at end of file diff --git a/robowaiter/behavior_tree/behavior_tree.py b/robowaiter/behavior_tree/behavior_tree.py new file mode 100644 index 0000000..3867407 --- /dev/null +++ b/robowaiter/behavior_tree/behavior_tree.py @@ -0,0 +1,19 @@ +import py_trees as ptree +from robowaiter.behavior_tree.ptml import ptmlCompiler + + +def load_bt_from_ptml(scene, ptml_path, behavior_lib_path): + ptml_bt = ptmlCompiler.load(scene, ptml_path, behavior_lib_path) + bt = ptree.trees.BehaviourTree(ptml_bt) + + with open(ptml_path, 'r') as f: + ptml = f.read() + + print(f'BT loaded: \n {ptml}') + + # print(ptree.display.unicode_tree(root=bt.root, show_status=True)) + return bt + +# class BehaviorTree(ptree): +# def __init__(self): +# super().__init__() \ No newline at end of file diff --git a/robowaiter/behavior_tree/ptml/CoffeeDelivery.ptml b/robowaiter/behavior_tree/ptml/CoffeeDelivery.ptml new file mode 100644 index 0000000..7f1b11f --- /dev/null +++ b/robowaiter/behavior_tree/ptml/CoffeeDelivery.ptml @@ -0,0 +1,35 @@ +//sequence: +// act action1() +// act action2(2, 2.3, True) +// +// parallel 2: +// act action3(int a, float b) +// act action4() + +sequence{ + selector{ + cond CoffeeCupFound() + task FindCoffeeCup() + sequence{ + cond SeqTest() + task Move(1.2, 2, 2.3, True) + task Grasp() + parallel 3 { + cond Istask() + task TestTask() + } + } + } + selector{ + cond CoffeeCupGrasped() + task GraspCoffeeCup() + } + selector{ + cond DestinationAReached() + task ReachDestinationA() + } + selector{ + cond CoffeeCupPlaced() + task PlaceCoffeeCup() + } +} diff --git a/robowaiter/behavior_tree/ptml/__init__.py b/robowaiter/behavior_tree/ptml/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robowaiter/behavior_tree/ptml/ptml.g4 b/robowaiter/behavior_tree/ptml/ptml.g4 new file mode 100644 index 0000000..0a545a9 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/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/behavior_tree/ptml/ptml.interp b/robowaiter/behavior_tree/ptml/ptml.interp new file mode 100644 index 0000000..a96ef7d --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptml.interp @@ -0,0 +1,51 @@ +token literal names: +null +'{' +'}' +'sequence' +'selector' +'parallel' +'task' +'cond' +'(' +')' +',' +'True' +'False' +null +null +null +null +null + +token symbolic names: +null +null +null +null +null +null +null +null +null +null +null +null +null +Names +Integer +Float +LINE_COMMENT +WS + +rule names: +root +tree +internal_node +action_sign +action_parm +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 diff --git a/robowaiter/behavior_tree/ptml/ptml.tokens b/robowaiter/behavior_tree/ptml/ptml.tokens new file mode 100644 index 0000000..1f7a813 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptml.tokens @@ -0,0 +1,29 @@ +T__0=1 +T__1=2 +T__2=3 +T__3=4 +T__4=5 +T__5=6 +T__6=7 +T__7=8 +T__8=9 +T__9=10 +T__10=11 +T__11=12 +Names=13 +Integer=14 +Float=15 +LINE_COMMENT=16 +WS=17 +'{'=1 +'}'=2 +'sequence'=3 +'selector'=4 +'parallel'=5 +'task'=6 +'cond'=7 +'('=8 +')'=9 +','=10 +'True'=11 +'False'=12 diff --git a/robowaiter/behavior_tree/ptml/ptmlCompiler.py b/robowaiter/behavior_tree/ptml/ptmlCompiler.py new file mode 100644 index 0000000..548369d --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptmlCompiler.py @@ -0,0 +1,46 @@ +import os +from antlr4 import * + +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(scene, ptml_path: str, behaviour_lib_path: str): + """_summary_ + + Args: + ptml_path (str): _description_ + behaviour_lib_path (str): _description_ + + Raises: + FileNotFoundError: _description_ + FileNotFoundError: _description_ + """ + # error handle + if not os.path.exists(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) + ) + + # noting fault, go next + 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(scene, behaviour_lib_path) # listener mode + walker.walk(ptml, tree) + + return ptml.bt_root diff --git a/robowaiter/behavior_tree/ptml/ptmlLexer.interp b/robowaiter/behavior_tree/ptml/ptmlLexer.interp new file mode 100644 index 0000000..823b753 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptmlLexer.interp @@ -0,0 +1,68 @@ +token literal names: +null +'{' +'}' +'sequence' +'selector' +'parallel' +'task' +'cond' +'(' +')' +',' +'True' +'False' +null +null +null +null +null + +token symbolic names: +null +null +null +null +null +null +null +null +null +null +null +null +null +Names +Integer +Float +LINE_COMMENT +WS + +rule names: +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 +Float +LINE_COMMENT +WS + +channel names: +DEFAULT_TOKEN_CHANNEL +HIDDEN + +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 diff --git a/robowaiter/behavior_tree/ptml/ptmlLexer.py b/robowaiter/behavior_tree/ptml/ptmlLexer.py new file mode 100644 index 0000000..5fe957a --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptmlLexer.py @@ -0,0 +1,119 @@ +# Generated from E:/Projects/UE5/HARIX_RDKSim/Plugins/HarixSim/Python/ptml/ptml.g4 by ANTLR 4.13.1 +from antlr4 import * +from io import StringIO +import sys +if sys.version_info[1] > 5: + from typing import TextIO +else: + from typing.io import TextIO + + +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, + 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 + ] + +class ptmlLexer(Lexer): + + atn = ATNDeserializer().deserialize(serializedATN()) + + decisionsToDFA = [ DFA(ds, i) for i, ds in enumerate(atn.decisionToState) ] + + T__0 = 1 + T__1 = 2 + T__2 = 3 + T__3 = 4 + T__4 = 5 + T__5 = 6 + T__6 = 7 + T__7 = 8 + T__8 = 9 + T__9 = 10 + T__10 = 11 + T__11 = 12 + Names = 13 + Integer = 14 + Float = 15 + LINE_COMMENT = 16 + WS = 17 + + channelNames = [ u"DEFAULT_TOKEN_CHANNEL", u"HIDDEN" ] + + modeNames = [ "DEFAULT_MODE" ] + + literalNames = [ "", + "'{'", "'}'", "'sequence'", "'selector'", "'parallel'", "'task'", + "'cond'", "'('", "')'", "','", "'True'", "'False'" ] + + symbolicNames = [ "", + "Names", "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", + "Float", "LINE_COMMENT", "WS" ] + + grammarFileName = "ptml.g4" + + def __init__(self, input=None, output:TextIO = sys.stdout): + super().__init__(input, output) + self.checkVersion("4.13.1") + self._interp = LexerATNSimulator(self, self.atn, self.decisionsToDFA, PredictionContextCache()) + self._actions = None + self._predicates = None + + diff --git a/robowaiter/behavior_tree/ptml/ptmlLexer.tokens b/robowaiter/behavior_tree/ptml/ptmlLexer.tokens new file mode 100644 index 0000000..1f7a813 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptmlLexer.tokens @@ -0,0 +1,29 @@ +T__0=1 +T__1=2 +T__2=3 +T__3=4 +T__4=5 +T__5=6 +T__6=7 +T__7=8 +T__8=9 +T__9=10 +T__10=11 +T__11=12 +Names=13 +Integer=14 +Float=15 +LINE_COMMENT=16 +WS=17 +'{'=1 +'}'=2 +'sequence'=3 +'selector'=4 +'parallel'=5 +'task'=6 +'cond'=7 +'('=8 +')'=9 +','=10 +'True'=11 +'False'=12 diff --git a/robowaiter/behavior_tree/ptml/ptmlListener.py b/robowaiter/behavior_tree/ptml/ptmlListener.py new file mode 100644 index 0000000..41056b2 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptmlListener.py @@ -0,0 +1,66 @@ +# Generated from E:/Projects/UE5/HARIX_RDKSim/Plugins/HarixSim/Python/ptml/ptml.g4 by ANTLR 4.13.1 +from antlr4 import * +if "." in __name__: + from .ptmlParser import ptmlParser +else: + from ptmlParser import ptmlParser + +# This class defines a complete listener for a parse tree produced by ptmlParser. +class ptmlListener(ParseTreeListener): + + # Enter a parse tree produced by ptmlParser#root. + def enterRoot(self, ctx:ptmlParser.RootContext): + pass + + # Exit a parse tree produced by ptmlParser#root. + def exitRoot(self, ctx:ptmlParser.RootContext): + pass + + + # Enter a parse tree produced by ptmlParser#tree. + def enterTree(self, ctx:ptmlParser.TreeContext): + pass + + # Exit a parse tree produced by ptmlParser#tree. + def exitTree(self, ctx:ptmlParser.TreeContext): + pass + + + # 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): + pass + + + # Enter a parse tree produced by ptmlParser#action_sign. + def enterAction_sign(self, ctx:ptmlParser.Action_signContext): + pass + + # Exit a parse tree produced by ptmlParser#action_sign. + 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): + pass + + # Exit a parse tree produced by ptmlParser#action_parm. + def exitAction_parm(self, ctx:ptmlParser.Action_parmContext): + pass + + + # Enter a parse tree produced by ptmlParser#boolean. + def enterBoolean(self, ctx:ptmlParser.BooleanContext): + pass + + # Exit a parse tree produced by ptmlParser#boolean. + def exitBoolean(self, ctx:ptmlParser.BooleanContext): + pass + + + +del ptmlParser \ No newline at end of file diff --git a/robowaiter/behavior_tree/ptml/ptmlParser.py b/robowaiter/behavior_tree/ptml/ptmlParser.py new file mode 100644 index 0000000..9af021d --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptmlParser.py @@ -0,0 +1,514 @@ +# Generated from E:/Projects/UE5/HARIX_RDKSim/Plugins/HarixSim/Python/ptml/ptml.g4 by ANTLR 4.13.1 +# encoding: utf-8 +from antlr4 import * +from io import StringIO +import sys +if sys.version_info[1] > 5: + from typing import TextIO +else: + from typing.io import TextIO + +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, + 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 + ] + +class ptmlParser ( Parser ): + + grammarFileName = "ptml.g4" + + atn = ATNDeserializer().deserialize(serializedATN()) + + decisionsToDFA = [ DFA(ds, i) for i, ds in enumerate(atn.decisionToState) ] + + sharedContextCache = PredictionContextCache() + + literalNames = [ "", "'{'", "'}'", "'sequence'", "'selector'", + "'parallel'", "'task'", "'cond'", "'('", "')'", "','", + "'True'", "'False'" ] + + symbolicNames = [ "", "", "", "", + "", "", "", "", + "", "", "", "", + "", "Names", "Integer", "Float", "LINE_COMMENT", + "WS" ] + + RULE_root = 0 + RULE_tree = 1 + RULE_internal_node = 2 + RULE_action_sign = 3 + RULE_action_parm = 4 + RULE_boolean = 5 + + ruleNames = [ "root", "tree", "internal_node", "action_sign", "action_parm", + "boolean" ] + + EOF = Token.EOF + T__0=1 + T__1=2 + T__2=3 + T__3=4 + T__4=5 + T__5=6 + T__6=7 + T__7=8 + T__8=9 + T__9=10 + T__10=11 + T__11=12 + Names=13 + Integer=14 + Float=15 + LINE_COMMENT=16 + WS=17 + + def __init__(self, input:TokenStream, output:TextIO = sys.stdout): + super().__init__(input, output) + self.checkVersion("4.13.1") + self._interp = ParserATNSimulator(self, self.atn, self.decisionsToDFA, self.sharedContextCache) + self._predicates = None + + + + + class RootContext(ParserRuleContext): + __slots__ = 'parser' + + def __init__(self, parser, parent:ParserRuleContext=None, invokingState:int=-1): + super().__init__(parent, invokingState) + self.parser = parser + + def EOF(self): + return self.getToken(ptmlParser.EOF, 0) + + def tree(self, i:int=None): + if i is None: + return self.getTypedRuleContexts(ptmlParser.TreeContext) + else: + return self.getTypedRuleContext(ptmlParser.TreeContext,i) + + + def getRuleIndex(self): + return ptmlParser.RULE_root + + def enterRule(self, listener:ParseTreeListener): + if hasattr( listener, "enterRoot" ): + listener.enterRoot(self) + + def exitRule(self, listener:ParseTreeListener): + if hasattr( listener, "exitRoot" ): + listener.exitRoot(self) + + + + + def root(self): + + localctx = ptmlParser.RootContext(self, self._ctx, self.state) + self.enterRule(localctx, 0, self.RULE_root) + self._la = 0 # Token type + try: + self.enterOuterAlt(localctx, 1) + self.state = 13 + self._errHandler.sync(self) + _la = self._input.LA(1) + while True: + self.state = 12 + self.tree() + self.state = 15 + self._errHandler.sync(self) + _la = self._input.LA(1) + if not ((((_la) & ~0x3f) == 0 and ((1 << _la) & 56) != 0)): + break + + self.state = 17 + self.match(ptmlParser.EOF) + except RecognitionException as re: + localctx.exception = re + self._errHandler.reportError(self, re) + self._errHandler.recover(self, re) + finally: + self.exitRule() + return localctx + + + class TreeContext(ParserRuleContext): + __slots__ = 'parser' + + def __init__(self, parser, parent:ParserRuleContext=None, invokingState:int=-1): + super().__init__(parent, invokingState) + self.parser = parser + + def internal_node(self): + return self.getTypedRuleContext(ptmlParser.Internal_nodeContext,0) + + + def action_sign(self, i:int=None): + if i is None: + return self.getTypedRuleContexts(ptmlParser.Action_signContext) + else: + return self.getTypedRuleContext(ptmlParser.Action_signContext,i) + + + def tree(self, i:int=None): + if i is None: + return self.getTypedRuleContexts(ptmlParser.TreeContext) + else: + return self.getTypedRuleContext(ptmlParser.TreeContext,i) + + + def getRuleIndex(self): + return ptmlParser.RULE_tree + + def enterRule(self, listener:ParseTreeListener): + if hasattr( listener, "enterTree" ): + listener.enterTree(self) + + def exitRule(self, listener:ParseTreeListener): + if hasattr( listener, "exitTree" ): + listener.exitTree(self) + + + + + def tree(self): + + localctx = ptmlParser.TreeContext(self, self._ctx, self.state) + self.enterRule(localctx, 2, self.RULE_tree) + self._la = 0 # Token type + try: + self.enterOuterAlt(localctx, 1) + self.state = 19 + self.internal_node() + self.state = 20 + self.match(ptmlParser.T__0) + self.state = 23 + self._errHandler.sync(self) + _la = self._input.LA(1) + while True: + self.state = 23 + self._errHandler.sync(self) + token = self._input.LA(1) + if token in [6, 7]: + self.state = 21 + self.action_sign() + pass + elif token in [3, 4, 5]: + self.state = 22 + self.tree() + pass + else: + raise NoViableAltException(self) + + self.state = 25 + self._errHandler.sync(self) + _la = self._input.LA(1) + if not ((((_la) & ~0x3f) == 0 and ((1 << _la) & 248) != 0)): + break + + self.state = 27 + self.match(ptmlParser.T__1) + except RecognitionException as re: + localctx.exception = re + self._errHandler.reportError(self, re) + self._errHandler.recover(self, re) + finally: + self.exitRule() + return localctx + + + class Internal_nodeContext(ParserRuleContext): + __slots__ = 'parser' + + def __init__(self, parser, parent:ParserRuleContext=None, invokingState:int=-1): + super().__init__(parent, invokingState) + self.parser = parser + + def Integer(self): + return self.getToken(ptmlParser.Integer, 0) + + def getRuleIndex(self): + return ptmlParser.RULE_internal_node + + def enterRule(self, listener:ParseTreeListener): + if hasattr( listener, "enterInternal_node" ): + listener.enterInternal_node(self) + + def exitRule(self, listener:ParseTreeListener): + if hasattr( listener, "exitInternal_node" ): + listener.exitInternal_node(self) + + + + + def internal_node(self): + + localctx = ptmlParser.Internal_nodeContext(self, self._ctx, self.state) + self.enterRule(localctx, 4, self.RULE_internal_node) + try: + self.state = 33 + self._errHandler.sync(self) + token = self._input.LA(1) + if token in [3]: + self.enterOuterAlt(localctx, 1) + self.state = 29 + self.match(ptmlParser.T__2) + pass + elif token in [4]: + self.enterOuterAlt(localctx, 2) + self.state = 30 + self.match(ptmlParser.T__3) + pass + elif token in [5]: + self.enterOuterAlt(localctx, 3) + self.state = 31 + self.match(ptmlParser.T__4) + self.state = 32 + self.match(ptmlParser.Integer) + pass + else: + raise NoViableAltException(self) + + except RecognitionException as re: + localctx.exception = re + self._errHandler.reportError(self, re) + self._errHandler.recover(self, re) + finally: + self.exitRule() + return localctx + + + class Action_signContext(ParserRuleContext): + __slots__ = 'parser' + + def __init__(self, parser, parent:ParserRuleContext=None, invokingState:int=-1): + super().__init__(parent, invokingState) + self.parser = parser + + def Names(self): + return self.getToken(ptmlParser.Names, 0) + + def action_parm(self): + return self.getTypedRuleContext(ptmlParser.Action_parmContext,0) + + + def getRuleIndex(self): + return ptmlParser.RULE_action_sign + + def enterRule(self, listener:ParseTreeListener): + if hasattr( listener, "enterAction_sign" ): + listener.enterAction_sign(self) + + def exitRule(self, listener:ParseTreeListener): + if hasattr( listener, "exitAction_sign" ): + listener.exitAction_sign(self) + + + + + def action_sign(self): + + localctx = ptmlParser.Action_signContext(self, self._ctx, self.state) + self.enterRule(localctx, 6, self.RULE_action_sign) + self._la = 0 # Token type + try: + self.enterOuterAlt(localctx, 1) + self.state = 35 + _la = self._input.LA(1) + if not(_la==6 or _la==7): + self._errHandler.recoverInline(self) + else: + self._errHandler.reportMatch(self) + self.consume() + self.state = 36 + self.match(ptmlParser.Names) + 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): + self.state = 38 + self.action_parm() + + + self.state = 41 + self.match(ptmlParser.T__8) + except RecognitionException as re: + localctx.exception = re + self._errHandler.reportError(self, re) + self._errHandler.recover(self, re) + finally: + self.exitRule() + return localctx + + + class Action_parmContext(ParserRuleContext): + __slots__ = 'parser' + + def __init__(self, parser, parent:ParserRuleContext=None, invokingState:int=-1): + super().__init__(parent, invokingState) + self.parser = parser + + def Integer(self, i:int=None): + if i is None: + return self.getTokens(ptmlParser.Integer) + else: + return self.getToken(ptmlParser.Integer, i) + + def Float(self, i:int=None): + if i is None: + return self.getTokens(ptmlParser.Float) + else: + return self.getToken(ptmlParser.Float, i) + + def boolean(self, i:int=None): + if i is None: + return self.getTypedRuleContexts(ptmlParser.BooleanContext) + else: + return self.getTypedRuleContext(ptmlParser.BooleanContext,i) + + + def getRuleIndex(self): + return ptmlParser.RULE_action_parm + + def enterRule(self, listener:ParseTreeListener): + if hasattr( listener, "enterAction_parm" ): + listener.enterAction_parm(self) + + def exitRule(self, listener:ParseTreeListener): + if hasattr( listener, "exitAction_parm" ): + listener.exitAction_parm(self) + + + + + def action_parm(self): + + localctx = ptmlParser.Action_parmContext(self, self._ctx, self.state) + self.enterRule(localctx, 8, self.RULE_action_parm) + self._la = 0 # Token type + try: + self.enterOuterAlt(localctx, 1) + self.state = 46 + self._errHandler.sync(self) + token = self._input.LA(1) + if token in [14]: + self.state = 43 + self.match(ptmlParser.Integer) + pass + elif token in [15]: + self.state = 44 + self.match(ptmlParser.Float) + pass + elif token in [11, 12]: + self.state = 45 + self.boolean() + pass + else: + raise NoViableAltException(self) + + self.state = 56 + self._errHandler.sync(self) + _la = self._input.LA(1) + while _la==10: + self.state = 48 + self.match(ptmlParser.T__9) + self.state = 52 + self._errHandler.sync(self) + token = self._input.LA(1) + if token in [14]: + self.state = 49 + self.match(ptmlParser.Integer) + pass + elif token in [15]: + self.state = 50 + self.match(ptmlParser.Float) + pass + elif token in [11, 12]: + self.state = 51 + self.boolean() + pass + else: + raise NoViableAltException(self) + + self.state = 58 + self._errHandler.sync(self) + _la = self._input.LA(1) + + except RecognitionException as re: + localctx.exception = re + self._errHandler.reportError(self, re) + self._errHandler.recover(self, re) + finally: + self.exitRule() + return localctx + + + class BooleanContext(ParserRuleContext): + __slots__ = 'parser' + + def __init__(self, parser, parent:ParserRuleContext=None, invokingState:int=-1): + super().__init__(parent, invokingState) + self.parser = parser + + + def getRuleIndex(self): + return ptmlParser.RULE_boolean + + def enterRule(self, listener:ParseTreeListener): + if hasattr( listener, "enterBoolean" ): + listener.enterBoolean(self) + + def exitRule(self, listener:ParseTreeListener): + if hasattr( listener, "exitBoolean" ): + listener.exitBoolean(self) + + + + + def boolean(self): + + localctx = ptmlParser.BooleanContext(self, self._ctx, self.state) + self.enterRule(localctx, 10, self.RULE_boolean) + self._la = 0 # Token type + try: + self.enterOuterAlt(localctx, 1) + self.state = 59 + _la = self._input.LA(1) + if not(_la==11 or _la==12): + self._errHandler.recoverInline(self) + else: + self._errHandler.reportMatch(self) + self.consume() + except RecognitionException as re: + localctx.exception = re + self._errHandler.reportError(self, re) + self._errHandler.recover(self, re) + finally: + self.exitRule() + return localctx + + + + + diff --git a/robowaiter/behavior_tree/ptml/ptmlTranslator.py b/robowaiter/behavior_tree/ptml/ptmlTranslator.py new file mode 100644 index 0000000..d0b13b4 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptmlTranslator.py @@ -0,0 +1,129 @@ +import shortuuid +import py_trees as ptree + +from antlr4 import * + +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. + + Args: + ptmlListener (_type_): _description_ + """ + + def __init__(self, scene, behaviour_lib_path) -> None: + super().__init__() + self.bt_root = None + self.stack = [] + self.scene = scene + self.behaviour_lib_path = behaviour_lib_path + + # Enter a parse tree produced by ptmlParser#root. + def enterRoot(self, ctx: ptmlParser.RootContext): + pass + + # Exit a parse tree produced by ptmlParser#root. + def exitRoot(self, ctx: ptmlParser.RootContext): + pass + + # Enter a parse tree produced by ptmlParser#tree. + def enterTree(self, ctx: ptmlParser.TreeContext): + type = str(ctx.internal_node().children[0]) + + match type: + case "sequence": + tag = "sequence_" + short_uuid() + node = ptree.composites.Sequence(name=tag, memory=False) + case "selector": + tag = "selector_" + short_uuid() + node = ptree.composites.Selector(name=tag, memory=False) + 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 + ) + case _: + 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): + 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): + pass + + # Enter a parse tree produced by ptmlParser#action_sign. + 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) + + import sys + + sys.path.append(self.behaviour_lib_path) + + exec("from {} import {}".format(name, name)) + # + tag = "cond_" + short_uuid() if node_type == "cond" else "task_" + short_uuid() + + node = eval( + "{}('{}', scene, {})".format(name, tag, args), {"scene": self.scene, **locals()} + ) + + # connect + self.stack[-1].add_child(node) + + # Exit a parse tree produced by ptmlParser#action_sign. + 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): + pass + + # Exit a parse tree produced by ptmlParser#action_parm. + def exitAction_parm(self, ctx: ptmlParser.Action_parmContext): + pass + + # Enter a parse tree produced by ptmlParser#boolean. + def enterBoolean(self, ctx: ptmlParser.BooleanContext): + pass + + # Exit a parse tree produced by ptmlParser#boolean. + def exitBoolean(self, ctx: ptmlParser.BooleanContext): + pass diff --git a/robowaiter/behavior_tree/ptml/ptml_test.py b/robowaiter/behavior_tree/ptml/ptml_test.py new file mode 100644 index 0000000..bdcd3d2 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/ptml_test.py @@ -0,0 +1,22 @@ +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 new file mode 100644 index 0000000..79e4ba9 --- /dev/null +++ b/robowaiter/behavior_tree/ptml/sequence_oufbg8fg.dot @@ -0,0 +1,45 @@ +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/proto/GrabSim_pb2.pyd b/robowaiter/proto/GrabSim_pb2.pyd new file mode 100644 index 0000000..5393aed Binary files /dev/null and b/robowaiter/proto/GrabSim_pb2.pyd differ diff --git a/robowaiter/proto/GrabSim_pb2_grpc.pyd b/robowaiter/proto/GrabSim_pb2_grpc.pyd new file mode 100644 index 0000000..e901e45 Binary files /dev/null and b/robowaiter/proto/GrabSim_pb2_grpc.pyd differ diff --git a/robowaiter/proto/__init__.py b/robowaiter/proto/__init__.py new file mode 100644 index 0000000..3cca4fd --- /dev/null +++ b/robowaiter/proto/__init__.py @@ -0,0 +1,2 @@ +from . import GrabSim_pb2 +from . import GrabSim_pb2_grpc diff --git a/robowaiter/scene/__init__.py b/robowaiter/scene/__init__.py new file mode 100644 index 0000000..f3578e9 --- /dev/null +++ b/robowaiter/scene/__init__.py @@ -0,0 +1,17 @@ + +from .scene import Scene +from robowaiter.scene.tasks.AEM import SceneAEM +from robowaiter.scene.tasks.GQA import SceneGQA +from robowaiter.scene.tasks.VLN import SceneVLN +from robowaiter.scene.tasks.VLM import SceneVLM +from robowaiter.scene.tasks.Open_tasks import SceneOT +from robowaiter.scene.tasks.Auto_tasks import SceneAT + +task_map = { + "AEM": SceneAEM, + "GQA": SceneGQA, + "VLN": SceneVLN, + "VLM": SceneVLM, + "OT": SceneOT, + "AT": SceneAT, +} \ No newline at end of file diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py new file mode 100644 index 0000000..943cfb1 --- /dev/null +++ b/robowaiter/scene/scene.py @@ -0,0 +1,275 @@ +import time +import grpc +import numpy as np + +from robowaiter.proto import GrabSim_pb2 +from robowaiter.proto import GrabSim_pb2_grpc + + +channel = grpc.insecure_channel( + "localhost:30001", + options=[ + ("grpc.max_send_message_length", 1024 * 1024 * 1024), + ("grpc.max_receive_message_length", 1024 * 1024 * 1024), + ], +) +stub = GrabSim_pb2_grpc.GrabSimStub(channel) + +animation_step = [4, 5, 7, 3, 3] +loc_offset = [-700, -1400] + + +def init_world(scene_num=1, mapID=3): + stub.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=mapID)) + time.sleep(3) # wait for the map to load + + +def image_extract(camera_data): + image = camera_data.images[0] + return np.frombuffer(image.data, dtype=image.dtype).reshape( + (image.height, image.width, image.channels) + ) + + +class Scene: + robot = None + state = {} + """ + # 当前场景的状态 + state: { + "chat_pool": [ #未处理的顾客的对话池 + { + "pos": 顾客的位置, + "chat": 顾客对话的内容 + } + ], + + "status": # 仿真器中的观测信息，见下方详细解释 + } + + status: + location: Dict[X: float, Y: float] + rotation: Dict[Yaw: float] + joints: List[Dict[name: str, location: Dict[X: float, Y: float, Z: float]]] + fingers: List[Dict[name: str, location: List[3 * Dict[X: float, Y: float, Z: float]]]] + objects[:-1]: List[Dict[name: str, location: Dict[X: float, Y: float, Z: float]]] + objects[-1]: Dict[name: "Hand", boxes: List[Dict[diagonals: List[4 * Dict[X0: float, Y0: float, Z0: float, X1: float, Y1: float, Z1: float]]]]] + walkers: List[name: str, pose: Dict[X: float, Y: float, Yaw: float], speed: float, target: Dict[X: float, Y: float, Yaw: float]] + timestamp: int, timestep: int + collision: str, info: str + """ + + def __init__(self,robot, sceneID=0): + self.sceneID = sceneID + self.use_offset = True + + # init robot + robot.set_scene(self) + robot.load_BT() + self.robot = robot + + + def run(self): + pass + + + @property + def status(self): + return stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) + + def reset_sim(self): + stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) + + # reset world + init_world() + + # reset state + self.state = { + "chatting_list": [] + } + + + + def reset(self): + self.reset_sim() + + def walker_control_generator(self, walkerID, autowalk, speed, X, Y, Yaw): + if self.use_offset: + X, Y = X + loc_offset[0], Y + loc_offset[1] + return GrabSim_pb2.WalkerControls.WControl( + id=walkerID, + autowalk=autowalk, + speed=speed, + pose=GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw), + ) + + def walk_to(self, X, Y, Yaw, velocity=150, dis_limit=100): + if self.use_offset: + X, Y = X + loc_offset[0], Y + loc_offset[1] + stub.Do( + GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.WalkTo, + values=[X, Y, Yaw, velocity, dis_limit], + ) + ) + + def reachable_check(self, X, Y, Yaw): + if self.use_offset: + X, Y = X + loc_offset[0], Y + loc_offset[1] + navigation_info = stub.Do( + GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.WalkTo, + values=[X, Y, Yaw], + ) + ).info + if navigation_info == "Unreachable": + return False + else: + return True + + def add_walker(self, X, Y, Yaw): + if self.use_offset: + X, Y = X + loc_offset[0], Y + loc_offset[1] + if self.reachable_check(X, Y, Yaw): + stub.AddWalker( + GrabSim_pb2.WalkerList( + walkers=[ + GrabSim_pb2.WalkerList.Walker( + id=0, + pose=GrabSim_pb2.Pose( + X=X, Y=Y, Yaw=Yaw + ), # Parameter id is useless + ) + ], + scene=self.sceneID, + ) + ) + + def remove_walker(self, *args): # take single walkerID or a list of walkerIDs + remove_list = [] + if isinstance(args[0], list): + remove_list = args[0] + else: + for walkerID in args: + # walkerID is the index of the walker in status.walkers. + # Since status.walkers is a list, some walkerIDs would change after removing a walker. + remove_list.append(walkerID) + stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID)) + + def clean_walker(self): + stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) + + def control_walker(self, control_list): + stub.ControlWalkers( + GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID) + ) + + def control_joints(self, angles): + stub.Do( + GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.RotateJoints, + values=angles, + ) + ) + + def add_object(self, type, X, Y, Z, Yaw=0): + if self.use_offset: + X, Y = X + loc_offset[0], Y + loc_offset[1] + stub.AddObjects( + GrabSim_pb2.ObjectList( + objects=[ + GrabSim_pb2.ObjectList.Object(x=X, y=Y, yaw=Yaw, z=Z, type=type) + ], + scene=self.sceneID, + ) + ) + + def remove_object(self, *args): # refer to remove_walker + remove_list = [] + if isinstance(args[0], list): + remove_list = args[0] + else: + for objectID in args: + remove_list.append(objectID) + stub.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID)) + + def clean_object(self): + stub.CleanObjects(GrabSim_pb2.SceneID(value=self.sceneID)) + + def grasp(self, handID, objectID): + stub.Do( + GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.Grasp, + values=[handID, objectID], + ) + ) + + def release(self, handID): + stub.Do( + GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.Release, + values=[handID], + ) + ) + + def get_camera_color(self, image_only=True): + camera_data = stub.Capture( + GrabSim_pb2.CameraList( + cameras=[GrabSim_pb2.CameraName.Head_Color], scene=self.sceneID + ) + ) + if image_only: + return image_extract(camera_data) + else: + return camera_data + + def get_camera_depth(self, image_only=True): + camera_data = stub.Capture( + GrabSim_pb2.CameraList( + cameras=[GrabSim_pb2.CameraName.Head_Depth], scene=self.sceneID + ) + ) + if image_only: + return image_extract(camera_data) + else: + return camera_data + + def get_camera_segment(self, image_only=True): + camera_data = stub.Capture( + GrabSim_pb2.CameraList( + cameras=[GrabSim_pb2.CameraName.Head_Segment], scene=self.sceneID + ) + ) + if image_only: + return image_extract(camera_data) + else: + return camera_data + + def chat_bubble(self, message): + stub.ControlRobot( + GrabSim_pb2.ControlInfo( + scene=self.sceneID, type=0, action=1, content=message + ) + ) + + def animation_control(self, animation_type): + # animation_type: 1:make coffee 2: pour water 3: grab food 4: mop floor 5: clean table + scene = stub.ControlRobot( + GrabSim_pb2.ControlInfo(scene=self.sceneID, type=animation_type, action=1) + ) + if scene.info == "action success": + for i in range(2, animation_step[animation_type - 1] + 1): + stub.ControlRobot( + GrabSim_pb2.ControlInfo( + scene=self.sceneID, type=animation_type, action=i + ) + ) + + def animation_reset(self): + stub.ControlRobot(GrabSim_pb2.ControlInfo(scene=self.sceneID, type=0, action=0)) + diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py new file mode 100644 index 0000000..39a94c7 --- /dev/null +++ b/robowaiter/scene/tasks/AEM.py @@ -0,0 +1,26 @@ +""" +环境主动探索和记忆 +要求输出探索结果（语义地图）对环境重点信息记忆。生成环境的语义拓扑地图，和不少于10个环境物品的识别和位置记忆，可以是图片或者文字或者格式化数据。 +""" + +from robowaiter.scene.scene import Scene + +class SceneAEM(Scene): + def __init__(self, robot): + super().__init__(robot) + + # control.init_world(1, 3) + + def reset(self): + self.reset_sim() + + self.add_object(0, 570, 1600, 85.5) # type与物品编号对应，具体参考README.md + self.add_object(1, 570, 1630, 85.5) + self.add_object(2, 570, 1660, 85.5) + self.add_object(3, 580, 1680, 85.5) + + # todo: 探索并获得语义地图 + print(self.status.objects) # 全部的物品信息，包括名称、位置等，与获得的语义地图进行对比 + + def run(self): + pass \ No newline at end of file diff --git a/robowaiter/scene/tasks/Auto_tasks.py b/robowaiter/scene/tasks/Auto_tasks.py new file mode 100644 index 0000000..dbc4c55 --- /dev/null +++ b/robowaiter/scene/tasks/Auto_tasks.py @@ -0,0 +1,26 @@ +""" +在特定环境下，机器人发现目标，可自主完成任务 +1. 打扫地面：地面有垃圾，机器人主动扫地、清理地面垃圾 +2. 收拾桌子：桌子上的污渍，机器人主动擦桌子 +3. 摆椅子：椅子不正，机器人主动摆正椅子 +4. 开灯：室内光线暗，机器人主动打开房屋的灯 +""" + +# todo: 通过行为树控制自动任务 + +from robowaiter.scene.scene import Scene + + +class SceneAT(Scene): + def __init__(self, robot): + 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): + self.chat_bubble("顾客说：请给我一杯咖啡") + diff --git a/robowaiter/scene/tasks/GQA.py b/robowaiter/scene/tasks/GQA.py new file mode 100644 index 0000000..0fd5ed3 --- /dev/null +++ b/robowaiter/scene/tasks/GQA.py @@ -0,0 +1,28 @@ +""" +具身多轮对话 GQA +点餐（order）的对话，咖啡厅服务员可以为客人（NPC）完成点餐基本对话 +场景对话（GQA）结合场景：询问卫生间、附近娱乐场所（数据来源自主定义） +开始条件：顾客NPC发出点餐指令 +结束条件：顾客NPC发出指令，表示不再需要服务 +""" + +# todo: 使用大模型进行对话，获得指令信息，适时结束对话 +# order = {...} + +from robowaiter.scene.scene import Scene + +class SceneGQA(Scene): + def __init__(self, robot): + 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): + self.chat_bubble("顾客说：123546567") + + diff --git a/robowaiter/scene/tasks/Open_tasks.py b/robowaiter/scene/tasks/Open_tasks.py new file mode 100644 index 0000000..cdcc6c7 --- /dev/null +++ b/robowaiter/scene/tasks/Open_tasks.py @@ -0,0 +1,31 @@ +""" +人提出请求，机器人完成任务 +1. 做咖啡（固定动画）：接收到做咖啡指令、走到咖啡机、拿杯子、操作咖啡机、取杯子、送到客人桌子上 +2. 倒水 +3. 夹点心 + +具体描述：设计一套点单规则（如菜单包含咖啡、水、点心等），按照规则拟造随机的订单。在收到订单后，通过大模型让机器人输出合理的备餐计划，并尝试在模拟环境中按照这个规划实现任务。 + +""" + +# todo: 接收点单信息，大模型生成任务规划 + +from robowaiter.scene.scene import Scene + +class SceneOT(Scene): + def __init__(self, robot): + 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): + self.chat_bubble("顾客说：请给我一杯咖啡") + + + + diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py new file mode 100644 index 0000000..af5d246 --- /dev/null +++ b/robowaiter/scene/tasks/VLM.py @@ -0,0 +1,36 @@ +""" +视觉语言操作 +机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板 +""" + +import time +from robowaiter.scene.scene import Scene + + +class SceneVLM(Scene): + def __init__(self, robot): + 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): + # 空调操作 + self.walk_to(950, 1260, 90) # 没法转向？ + # todo: 手臂操作 + time.sleep(5) + self.walk_to(947, 1900, 0) + + # 物品挪动 + # todo: 视觉导航至目标点，操作手臂至可抓位置 + """ + scene.grasp(1, your_objectID) + """ + + # todo: 视觉导航至目标点，找准释放位置 + """ + scene.release(1) + """ diff --git a/robowaiter/scene/tasks/VLN.py b/robowaiter/scene/tasks/VLN.py new file mode 100644 index 0000000..d7793d4 --- /dev/null +++ b/robowaiter/scene/tasks/VLN.py @@ -0,0 +1,39 @@ +""" +视觉语言导航 +识别顾客（NPC）靠近、打招呼、对话、领位导航到适合人数的空闲餐桌 +开始条件：监测到顾客靠近 +结束条件：完成领位，语音：“请问您想喝点什么？”，并等待下一步指令 +""" + +from robowaiter.scene.scene import Scene + + +class SceneVLN(Scene): + def __init__(self, robot): + 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): + # 实现单顾客领位 + self.add_walker(1085, 2630, 220) + self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) + + # todo: 监测到顾客靠近，打招呼，对话，识别获取空闲餐桌位置 + # 可以使用scene.chat_bubble(message)函数实现对话 + + """ + scene.walk_to(your_free_table_location) + time.sleep(5) + scene.control_walker([scene.walker_control_generator(your_free_table_location)]) + """ + + reach = True + if reach: + self.chat_bubble("请问您想喝点什么？") + + print(self.status.walkers) diff --git a/robowaiter/scene/tasks/__init__.py b/robowaiter/scene/tasks/__init__.py new file mode 100644 index 0000000..e69de29