diff --git a/robowaiter/algos/navigator/__init__.py b/robowaiter/algos/navigator/__init__.py index 2802065..e69de29 100644 --- a/robowaiter/algos/navigator/__init__.py +++ b/robowaiter/algos/navigator/__init__.py @@ -1,3 +0,0 @@ -from . import navigate -from . import dstar_lite - diff --git a/robowaiter/algos/navigator/navigate.py b/robowaiter/algos/navigator/navigate.py index a9d37b4..b4b2c71 100644 --- a/robowaiter/algos/navigator/navigate.py +++ b/robowaiter/algos/navigator/navigate.py @@ -7,7 +7,6 @@ import pickle import matplotlib.pyplot as plt import numpy as np -from robowaiter.scene import scene from robowaiter.algos.navigator.dstar_lite import DStarLite, euclidean_distance @@ -94,6 +93,7 @@ class Navigator: if __name__ == '__main__': # 根据map计算并保存cost_map + from robowaiter.scene import scene file_name = 'map_4.pkl' if os.path.exists(file_name): diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 635a22c..66245e9 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -81,9 +81,10 @@ class Bahavior(ptree.behaviour.Behaviour): # let behavior node interact with the scene - def set_scene(self, scene): - self.scene = scene - self.robot = scene.robot + def set_scene(self, scene=None): + if scene: + self.scene = scene + self.robot = scene.robot def setup(self, **kwargs: Any) -> None: return super().setup(**kwargs) diff --git a/robowaiter/behavior_lib/_base/Selector.py b/robowaiter/behavior_lib/_base/Selector.py index 1c84639..1b61ed0 100644 --- a/robowaiter/behavior_lib/_base/Selector.py +++ b/robowaiter/behavior_lib/_base/Selector.py @@ -3,6 +3,7 @@ from typing import Any class Selector(ptree.composites.Selector): print_name = "Selector" + ins_name = "Selector" type = "Selector" def __init__(self,*args,**kwargs): diff --git a/robowaiter/behavior_lib/_base/Sequence.py b/robowaiter/behavior_lib/_base/Sequence.py index 702a0d9..bd1b75c 100644 --- a/robowaiter/behavior_lib/_base/Sequence.py +++ b/robowaiter/behavior_lib/_base/Sequence.py @@ -3,6 +3,7 @@ from typing import Any class Sequence(ptree.composites.Sequence): print_name = "Sequence" + ins_name = "Selector" type = "Sequence" def __init__(self,*args,**kwargs): diff --git a/robowaiter/behavior_tree/ptml/ptmlCompiler.py b/robowaiter/behavior_tree/ptml/ptmlCompiler.py index 0be7cd5..ef21765 100644 --- a/robowaiter/behavior_tree/ptml/ptmlCompiler.py +++ b/robowaiter/behavior_tree/ptml/ptmlCompiler.py @@ -107,9 +107,12 @@ def format_trans_to_bracket(file_path: str) -> str: for i in range(level): ptml_new += '}' - import re - new_path = re.sub('\\\[a-zA-Z0-9_]*\.ptml', '/bracket_ptml.ptml', file_path) - with open(new_path, 'w+') as file: + file_name = os.path.basename(file_path).split(".")[0] + dir_path = os.path.dirname(file_path) + # import re + # new_path = re.sub('\\\[a-zA-Z0-9_]*\.ptml', '/bracket_ptml.ptml', file_path) + new_path = os.path.join(dir_path,file_name+"_bracket.ptml") + with open(new_path, 'w') as file: file.write(ptml_new) return new_path diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index f6e1909..a364d1c 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -35,7 +35,7 @@ class Robot(object): self.action_list = None - def set_scene(self,scene): + def set_scene(self,scene=None): self.scene = scene diff --git a/robowaiter/utils/bt/draw.py b/robowaiter/utils/bt/draw.py index f983e3a..8139bee 100644 --- a/robowaiter/utils/bt/draw.py +++ b/robowaiter/utils/bt/draw.py @@ -111,10 +111,10 @@ def dot_tree( if with_qualified_names: node_label += f"\n({utilities.get_fully_qualified_name(behaviour)})" ''' - if node_name == "Sequence": - node_name = ">" - if node_name == "Selector": - node_name = "?" + # if node_name == "Sequence": + # node_name = ">" + # if node_name == "Selector": + # node_name = "?" return node_name fontsize = 20 diff --git a/robowaiter/utils/draw_bt/Default.ptml b/robowaiter/utils/draw_bt/Default.ptml new file mode 100644 index 0000000..03745e6 --- /dev/null +++ b/robowaiter/utils/draw_bt/Default.ptml @@ -0,0 +1,17 @@ +selector + sequence + cond CustomerChatting() + act DealChat() + sequence + cond HasSubTask() + sequence + act SubTaskPlaceHolder() + sequence + cond FocusingCustomer() + act ServeCustomer() + sequence + cond NewCustomerComing() + selector + cond At(Robot,Bar) + act MoveTo(Bar) + act GreetCustomer() diff --git a/robowaiter/utils/draw_bt/Default_bracket.ptml b/robowaiter/utils/draw_bt/Default_bracket.ptml new file mode 100644 index 0000000..e18182b --- /dev/null +++ b/robowaiter/utils/draw_bt/Default_bracket.ptml @@ -0,0 +1,28 @@ +selector +{ + sequence +{ + cond CustomerChatting() + act DealChat() + +} sequence +{ + cond HasSubTask() + sequence +{ + act SubTaskPlaceHolder() + +} sequence + cond FocusingCustomer() + act ServeCustomer() + +} sequence +{ + cond NewCustomerComing() + selector +{ + cond At(Robot,Bar) + act MoveTo(Bar) + +} act GreetCustomer() +}} \ No newline at end of file diff --git a/robowaiter/utils/draw_bt/__init__.py b/robowaiter/utils/draw_bt/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robowaiter/utils/draw_bt/draw_bt.py b/robowaiter/utils/draw_bt/draw_bt.py new file mode 100644 index 0000000..4cbb292 --- /dev/null +++ b/robowaiter/utils/draw_bt/draw_bt.py @@ -0,0 +1,21 @@ +# from robowaiter.scene.scene import Scene +# from robowaiter.behavior_tree.ptml.ptmlCompiler import load + +import os +from robowaiter.robot.robot import Robot +from robowaiter.utils.bt.draw import render_dot_tree +from robowaiter.utils.basic import get_root_path +from robowaiter.utils.bt.load import load_bt_from_ptml + +if __name__ == '__main__': + + # create robot + root_path = get_root_path() + ptml_path = os.path.join(root_path, 'robowaiter/utils/draw_bt/Default.ptml') + behavior_lib_path = os.path.join(root_path, 'robowaiter/behavior_lib') + bt = load_bt_from_ptml(None, ptml_path, behavior_lib_path) + + + + render_dot_tree(bt.root,name="test") + # build and tick diff --git a/robowaiter/utils/draw_bt/test.dot b/robowaiter/utils/draw_bt/test.dot new file mode 100644 index 0000000..bd79eac --- /dev/null +++ b/robowaiter/utils/draw_bt/test.dot @@ -0,0 +1,39 @@ +digraph pastafarianism { +ordering=out; +graph [fontname="times-roman"]; +node [fontname="times-roman"]; +edge [fontname="times-roman"]; +"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=diamond, style=filled, width=0.01]; +"717a1290-56bc-41e1-9b4b-e6df93d40e64" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "717a1290-56bc-41e1-9b4b-e6df93d40e64"; +"b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2" [fillcolor=yellow, fontcolor=black, fontsize=20, label="CustomerChatting()", shape=ellipse, style=filled]; +"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2"; +"28015225-b7a4-4f38-b26f-4dbe5eea4154" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="DealChat()", shape=box, style=filled]; +"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "28015225-b7a4-4f38-b26f-4dbe5eea4154"; +"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e"; +"2f77f976-499c-4601-9d8c-86dd80d66dfa" [fillcolor=yellow, fontcolor=black, fontsize=20, label="HasSubTask()", shape=ellipse, style=filled]; +"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2f77f976-499c-4601-9d8c-86dd80d66dfa"; +"2428792c-3896-443d-8744-f5e286644fad" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2428792c-3896-443d-8744-f5e286644fad"; +"4bb7a9c3-521b-408f-b403-1fd25d54b192" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="SubTaskPlaceHolder()", shape=box, style=filled]; +"2428792c-3896-443d-8744-f5e286644fad" -> "4bb7a9c3-521b-408f-b403-1fd25d54b192"; +"64b2362a-d99e-4e99-8772-ad1419e53a2e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "64b2362a-d99e-4e99-8772-ad1419e53a2e"; +"fd7124ba-c9d1-4fde-8063-e8337d335121" [fillcolor=yellow, fontcolor=black, fontsize=20, label="FocusingCustomer()", shape=ellipse, style=filled]; +"64b2362a-d99e-4e99-8772-ad1419e53a2e" -> "fd7124ba-c9d1-4fde-8063-e8337d335121"; +"169ebec9-3645-4fbb-a533-5186a8e5967b" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="ServeCustomer()", shape=box, style=filled]; +"64b2362a-d99e-4e99-8772-ad1419e53a2e" -> "169ebec9-3645-4fbb-a533-5186a8e5967b"; +"bea12066-ecbd-49b0-8934-efcb2c38b5f5" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "bea12066-ecbd-49b0-8934-efcb2c38b5f5"; +"a847a0cc-c5af-4757-aa8c-8baef72788dc" [fillcolor=yellow, fontcolor=black, fontsize=20, label="NewCustomerComing()", shape=ellipse, style=filled]; +"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "a847a0cc-c5af-4757-aa8c-8baef72788dc"; +"99354f05-1716-46b2-9151-d88eac0a5b27" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=diamond, style=filled, width=0.01]; +"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "99354f05-1716-46b2-9151-d88eac0a5b27"; +"19de10a3-7554-43a3-b892-34cb2e32ab9a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="At(Robot,Bar)", shape=ellipse, style=filled]; +"99354f05-1716-46b2-9151-d88eac0a5b27" -> "19de10a3-7554-43a3-b892-34cb2e32ab9a"; +"4286d652-c4ef-4522-a7f1-b5c865dcc4c9" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="MoveTo(Bar)", shape=box, style=filled]; +"99354f05-1716-46b2-9151-d88eac0a5b27" -> "4286d652-c4ef-4522-a7f1-b5c865dcc4c9"; +"a3766a34-5152-4f82-8fba-8bf1f6b3830b" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="GreetCustomer()", shape=box, style=filled]; +"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "a3766a34-5152-4f82-8fba-8bf1f6b3830b"; +} diff --git a/robowaiter/utils/draw_bt/test.png b/robowaiter/utils/draw_bt/test.png new file mode 100644 index 0000000..773238d Binary files /dev/null and b/robowaiter/utils/draw_bt/test.png differ diff --git a/robowaiter/utils/draw_bt/test.svg b/robowaiter/utils/draw_bt/test.svg new file mode 100644 index 0000000..2ed2acf --- /dev/null +++ b/robowaiter/utils/draw_bt/test.svg @@ -0,0 +1,211 @@ + + + + + + +pastafarianism + + + +a3ac082f-ad46-4cc9-86b4-3e76c3c564d3 + +Selector + + + +717a1290-56bc-41e1-9b4b-e6df93d40e64 + +Sequence + + + +a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->717a1290-56bc-41e1-9b4b-e6df93d40e64 + + + + + +ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e + +Sequence + + + +a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e + + + + + +b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2 + +CustomerChatting() + + + +717a1290-56bc-41e1-9b4b-e6df93d40e64->b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2 + + + + + +28015225-b7a4-4f38-b26f-4dbe5eea4154 + +DealChat() + + + +717a1290-56bc-41e1-9b4b-e6df93d40e64->28015225-b7a4-4f38-b26f-4dbe5eea4154 + + + + + +2f77f976-499c-4601-9d8c-86dd80d66dfa + +HasSubTask() + + + +ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2f77f976-499c-4601-9d8c-86dd80d66dfa + + + + + +2428792c-3896-443d-8744-f5e286644fad + +Sequence + + + +ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2428792c-3896-443d-8744-f5e286644fad + + + + + +64b2362a-d99e-4e99-8772-ad1419e53a2e + +Sequence + + + +ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->64b2362a-d99e-4e99-8772-ad1419e53a2e + + + + + +bea12066-ecbd-49b0-8934-efcb2c38b5f5 + +Sequence + + + +ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->bea12066-ecbd-49b0-8934-efcb2c38b5f5 + + + + + +4bb7a9c3-521b-408f-b403-1fd25d54b192 + +SubTaskPlaceHolder() + + + +2428792c-3896-443d-8744-f5e286644fad->4bb7a9c3-521b-408f-b403-1fd25d54b192 + + + + + +fd7124ba-c9d1-4fde-8063-e8337d335121 + +FocusingCustomer() + + + +64b2362a-d99e-4e99-8772-ad1419e53a2e->fd7124ba-c9d1-4fde-8063-e8337d335121 + + + + + +169ebec9-3645-4fbb-a533-5186a8e5967b + +ServeCustomer() + + + +64b2362a-d99e-4e99-8772-ad1419e53a2e->169ebec9-3645-4fbb-a533-5186a8e5967b + + + + + +a847a0cc-c5af-4757-aa8c-8baef72788dc + +NewCustomerComing() + + + +bea12066-ecbd-49b0-8934-efcb2c38b5f5->a847a0cc-c5af-4757-aa8c-8baef72788dc + + + + + +99354f05-1716-46b2-9151-d88eac0a5b27 + +Selector + + + +bea12066-ecbd-49b0-8934-efcb2c38b5f5->99354f05-1716-46b2-9151-d88eac0a5b27 + + + + + +a3766a34-5152-4f82-8fba-8bf1f6b3830b + +GreetCustomer() + + + +bea12066-ecbd-49b0-8934-efcb2c38b5f5->a3766a34-5152-4f82-8fba-8bf1f6b3830b + + + + + +19de10a3-7554-43a3-b892-34cb2e32ab9a + +At(Robot,Bar) + + + +99354f05-1716-46b2-9151-d88eac0a5b27->19de10a3-7554-43a3-b892-34cb2e32ab9a + + + + + +4286d652-c4ef-4522-a7f1-b5c865dcc4c9 + +MoveTo(Bar) + + + +99354f05-1716-46b2-9151-d88eac0a5b27->4286d652-c4ef-4522-a7f1-b5c865dcc4c9 + + + + +