diff --git a/bracket_ptml.ptml b/bracket_ptml.ptml deleted file mode 100644 index be5a831..0000000 --- a/bracket_ptml.ptml +++ /dev/null @@ -1,4 +0,0 @@ -selector{ -cond At(Robot, Table) -} -} \ No newline at end of file diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py index aed1bc2..b402ed9 100644 --- a/robowaiter/behavior_lib/_base/Act.py +++ b/robowaiter/behavior_lib/_base/Act.py @@ -2,6 +2,7 @@ from robowaiter.behavior_lib._base.Behavior import Bahavior class Act(Bahavior): print_name_prefix = "act " + type = 'Act' def __init__(self,*args): super().__init__(*args) diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 74a1831..2a99f03 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -1,18 +1,7 @@ import py_trees as ptree from typing import Any import enum - -class Status(enum.Enum): - """An enumerator representing the status of a behavior.""" - - SUCCESS = ptree.common.Status.SUCCESS - """Behaviour check has passed, or execution of its action has finished with a successful result.""" - FAILURE = ptree.common.Status.FAILURE - """Behaviour check has failed, or execution of its action finished with a failed result.""" - RUNNING = ptree.common.Status.RUNNING - """Behaviour is in the middle of executing some action, result still pending.""" - INVALID = ptree.common.Status.INVALID - """Behaviour is uninitialised and/or in an inactive state, i.e. not currently being ticked.""" +from py_trees.common import Status # _base Behavior @@ -53,7 +42,6 @@ class Bahavior(ptree.behaviour.Behaviour): def update(self) -> Status: re = self._update() - print(f"{self.__class__.__name__}: {re.value}") return re def terminate(self, new_status: Status) -> None: diff --git a/robowaiter/behavior_lib/_base/Cond.py b/robowaiter/behavior_lib/_base/Cond.py index dc9f1d9..f8da01d 100644 --- a/robowaiter/behavior_lib/_base/Cond.py +++ b/robowaiter/behavior_lib/_base/Cond.py @@ -4,6 +4,7 @@ from robowaiter.behavior_lib._base.Behavior import Bahavior, Status class Cond(Bahavior): print_name_prefix = "cond " + type = 'Cond' def __init__(self,*args): super().__init__(*args) diff --git a/robowaiter/behavior_lib/_base/Selector.py b/robowaiter/behavior_lib/_base/Selector.py index 0dfd032..1c84639 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" + type = "Selector" def __init__(self,*args,**kwargs): super().__init__(*args,**kwargs) diff --git a/robowaiter/behavior_lib/_base/Sequence.py b/robowaiter/behavior_lib/_base/Sequence.py index 281ccfb..702a0d9 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" + type = "Sequence" def __init__(self,*args,**kwargs): super().__init__(*args,**kwargs) diff --git a/robowaiter/behavior_lib/act/DealChat.py b/robowaiter/behavior_lib/act/DealChat.py index 5a0c406..d30b7fd 100644 --- a/robowaiter/behavior_lib/act/DealChat.py +++ b/robowaiter/behavior_lib/act/DealChat.py @@ -1,8 +1,7 @@ import py_trees as ptree -from typing import Any from robowaiter.behavior_lib._base.Act import Act from robowaiter.llm_client.ask_llm import ask_llm -from robowaiter.behavior_tree.utils import load_bt_from_ptml,print_tree_from_root + fixed_answers = { "测试VLM：做一杯咖啡": ''' diff --git a/robowaiter/behavior_lib/act/DelSubTree.py b/robowaiter/behavior_lib/act/DelSubTree.py index 9b857d5..9c43c00 100644 --- a/robowaiter/behavior_lib/act/DelSubTree.py +++ b/robowaiter/behavior_lib/act/DelSubTree.py @@ -10,7 +10,5 @@ class DelSubTree(Act): def _update(self) -> ptree.common.Status: sub_task_tree = self.parent - print(self.scene.sub_task_seq.children) - print(sub_task_tree) self.scene.sub_task_seq.children.remove(sub_task_tree) return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_tree/__init__.py b/robowaiter/behavior_tree/__init__.py index ae3102d..8c15ec4 100644 --- a/robowaiter/behavior_tree/__init__.py +++ b/robowaiter/behavior_tree/__init__.py @@ -1,3 +1,2 @@ # from robowaiter.behavior_tree.behavior_tree import BehaviorTree -from robowaiter.behavior_tree.utils import load_bt_from_ptml \ No newline at end of file diff --git a/robowaiter/behavior_tree/ptml/test/ptml_test.py b/robowaiter/behavior_tree/ptml/test/ptml_test.py index cfec608..70d629f 100644 --- a/robowaiter/behavior_tree/ptml/test/ptml_test.py +++ b/robowaiter/behavior_tree/ptml/test/ptml_test.py @@ -1,12 +1,9 @@ -import os -import py_trees as ptree - # from robowaiter.scene.scene import Scene # from robowaiter.behavior_tree.ptml.ptmlCompiler import load import os from robowaiter import Robot, task_map - +from robowaiter.utils.bt.draw import render_dot_tree if __name__ == '__main__': TASK_NAME = 'OT' @@ -21,7 +18,7 @@ if __name__ == '__main__': # create task task = task_map[TASK_NAME](robot) - ptree.display.render_dot_tree(robot.bt.root,name="test") + render_dot_tree(robot.bt.root,name="test") # build and tick # scene.BT = ptree.trees.BehaviourTree(scene.BT) # todo: tick this bt diff --git a/robowaiter/behavior_tree/ptml/test/test.dot b/robowaiter/behavior_tree/ptml/test/test.dot index c8831ce..1efeebd 100644 --- a/robowaiter/behavior_tree/ptml/test/test.dot +++ b/robowaiter/behavior_tree/ptml/test/test.dot @@ -3,27 +3,27 @@ ordering=out; graph [fontname="times-roman"]; node [fontname="times-roman"]; edge [fontname="times-roman"]; -Selector [fillcolor=cyan, fontcolor=black, fontsize=9, label=Selector, shape=octagon, style=filled]; -"Selector*" [fillcolor=cyan, fontcolor=black, fontsize=9, label="Selector*", shape=octagon, style=filled]; -Selector -> "Selector*"; -HasMap [fillcolor=gray, fontcolor=black, fontsize=9, label=HasMap, shape=ellipse, style=filled]; -"Selector*" -> HasMap; -ExploreEnv [fillcolor=gray, fontcolor=black, fontsize=9, label=ExploreEnv, shape=ellipse, style=filled]; -"Selector*" -> ExploreEnv; -Sequence [fillcolor=orange, fontcolor=black, fontsize=9, label=Sequence, shape=box, style=filled]; -Selector -> Sequence; -Chatting [fillcolor=gray, fontcolor=black, fontsize=9, label=Chatting, shape=ellipse, style=filled]; -Sequence -> Chatting; -DealChat [fillcolor=gray, fontcolor=black, fontsize=9, label=DealChat, shape=ellipse, style=filled]; -Sequence -> DealChat; -"Sequence*" [fillcolor=orange, fontcolor=black, fontsize=9, label="Sequence*", shape=box, style=filled]; -Selector -> "Sequence*"; -HasSubTask [fillcolor=gray, fontcolor=black, fontsize=9, label=HasSubTask, shape=ellipse, style=filled]; -"Sequence*" -> HasSubTask; -"Sequence**" [fillcolor=orange, fontcolor=black, fontsize=9, label="Sequence**", shape=box, style=filled]; -"Sequence*" -> "Sequence**"; -"At(Robot,Table)" [fillcolor=gray, fontcolor=black, fontsize=9, label="At(Robot,Table)", shape=ellipse, style=filled]; -"Sequence**" -> "At(Robot,Table)"; -"At(Robot,Table)*" [fillcolor=gray, fontcolor=black, fontsize=9, label="At(Robot,Table)*", shape=ellipse, style=filled]; -"Sequence*" -> "At(Robot,Table)*"; +"34d1cf8a-42d6-49de-a458-0c7d3bffc436" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label="?", shape=diamond, style=filled, width=0.01]; +"33446211-2d5c-4e1d-bdb5-5ded443a713c" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label="?", shape=diamond, style=filled, width=0.01]; +"34d1cf8a-42d6-49de-a458-0c7d3bffc436" -> "33446211-2d5c-4e1d-bdb5-5ded443a713c"; +"96622c49-e2f6-4de9-9284-0c5eabbdd741" [fillcolor=yellow, fontcolor=black, fontsize=20, label=HasMap, shape=ellipse, style=filled]; +"33446211-2d5c-4e1d-bdb5-5ded443a713c" -> "96622c49-e2f6-4de9-9284-0c5eabbdd741"; +"dc007c72-0338-4616-bac2-bc39c81d2b77" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label=ExploreEnv, shape=box, style=filled]; +"33446211-2d5c-4e1d-bdb5-5ded443a713c" -> "dc007c72-0338-4616-bac2-bc39c81d2b77"; +"6fe9e522-557f-473a-a582-2f0d17d1a4f1" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=">", shape=octagon, style=filled, width=0.01]; +"34d1cf8a-42d6-49de-a458-0c7d3bffc436" -> "6fe9e522-557f-473a-a582-2f0d17d1a4f1"; +"cf3e4033-88b1-41bb-a638-26cc06e7a3dd" [fillcolor=yellow, fontcolor=black, fontsize=20, label=Chatting, shape=ellipse, style=filled]; +"6fe9e522-557f-473a-a582-2f0d17d1a4f1" -> "cf3e4033-88b1-41bb-a638-26cc06e7a3dd"; +"d2e8364e-dd83-4abb-8234-8466ff0c0483" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label=DealChat, shape=box, style=filled]; +"6fe9e522-557f-473a-a582-2f0d17d1a4f1" -> "d2e8364e-dd83-4abb-8234-8466ff0c0483"; +"d78eaf31-cc9d-484d-b564-dbd5912378fa" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=">", shape=octagon, style=filled, width=0.01]; +"34d1cf8a-42d6-49de-a458-0c7d3bffc436" -> "d78eaf31-cc9d-484d-b564-dbd5912378fa"; +"85284b02-fc8e-4418-8d6b-7a154d2004f6" [fillcolor=yellow, fontcolor=black, fontsize=20, label=HasSubTask, shape=ellipse, style=filled]; +"d78eaf31-cc9d-484d-b564-dbd5912378fa" -> "85284b02-fc8e-4418-8d6b-7a154d2004f6"; +"eb1bba56-55b1-4a71-8b31-0381812f588a" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=">", shape=octagon, style=filled, width=0.01]; +"d78eaf31-cc9d-484d-b564-dbd5912378fa" -> "eb1bba56-55b1-4a71-8b31-0381812f588a"; +"20b57b46-d59d-4b04-a4ed-eff12e6adc91" [fillcolor=yellow, fontcolor=black, fontsize=20, label="At(Robot,Table)", shape=ellipse, style=filled]; +"eb1bba56-55b1-4a71-8b31-0381812f588a" -> "20b57b46-d59d-4b04-a4ed-eff12e6adc91"; +"3d7f6aa9-62b5-4852-ab3e-ac0199cc46a8" [fillcolor=yellow, fontcolor=black, fontsize=20, label="At(Robot,Table)", shape=ellipse, style=filled]; +"d78eaf31-cc9d-484d-b564-dbd5912378fa" -> "3d7f6aa9-62b5-4852-ab3e-ac0199cc46a8"; } diff --git a/robowaiter/behavior_tree/ptml/test/test.png b/robowaiter/behavior_tree/ptml/test/test.png index 4eb2c3b..df4a1f9 100644 Binary files a/robowaiter/behavior_tree/ptml/test/test.png and b/robowaiter/behavior_tree/ptml/test/test.png differ diff --git a/robowaiter/behavior_tree/ptml/test/test.svg b/robowaiter/behavior_tree/ptml/test/test.svg index 0a8f7e8..fe9b90e 100644 --- a/robowaiter/behavior_tree/ptml/test/test.svg +++ b/robowaiter/behavior_tree/ptml/test/test.svg @@ -4,148 +4,148 @@ - - + + pastafarianism - - + + -Selector - -Selector +34d1cf8a-42d6-49de-a458-0c7d3bffc436 + +? - + -Selector* - -Selector* +33446211-2d5c-4e1d-bdb5-5ded443a713c + +? - + -Selector->Selector* - - +34d1cf8a-42d6-49de-a458-0c7d3bffc436->33446211-2d5c-4e1d-bdb5-5ded443a713c + + - + -Sequence - -Sequence +6fe9e522-557f-473a-a582-2f0d17d1a4f1 + +> - + -Selector->Sequence - - +34d1cf8a-42d6-49de-a458-0c7d3bffc436->6fe9e522-557f-473a-a582-2f0d17d1a4f1 + + - + -Sequence* - -Sequence* +d78eaf31-cc9d-484d-b564-dbd5912378fa + +> - + -Selector->Sequence* - - +34d1cf8a-42d6-49de-a458-0c7d3bffc436->d78eaf31-cc9d-484d-b564-dbd5912378fa + + - + -HasMap - -HasMap +96622c49-e2f6-4de9-9284-0c5eabbdd741 + +HasMap - + -Selector*->HasMap - - +33446211-2d5c-4e1d-bdb5-5ded443a713c->96622c49-e2f6-4de9-9284-0c5eabbdd741 + + - + -ExploreEnv - -ExploreEnv +dc007c72-0338-4616-bac2-bc39c81d2b77 + +ExploreEnv - + -Selector*->ExploreEnv - - +33446211-2d5c-4e1d-bdb5-5ded443a713c->dc007c72-0338-4616-bac2-bc39c81d2b77 + + - + -Chatting - -Chatting +cf3e4033-88b1-41bb-a638-26cc06e7a3dd + +Chatting - + -Sequence->Chatting - - +6fe9e522-557f-473a-a582-2f0d17d1a4f1->cf3e4033-88b1-41bb-a638-26cc06e7a3dd + + - + -DealChat - -DealChat +d2e8364e-dd83-4abb-8234-8466ff0c0483 + +DealChat - + -Sequence->DealChat - - +6fe9e522-557f-473a-a582-2f0d17d1a4f1->d2e8364e-dd83-4abb-8234-8466ff0c0483 + + - + -HasSubTask - -HasSubTask +85284b02-fc8e-4418-8d6b-7a154d2004f6 + +HasSubTask - + -Sequence*->HasSubTask - - +d78eaf31-cc9d-484d-b564-dbd5912378fa->85284b02-fc8e-4418-8d6b-7a154d2004f6 + + - + -Sequence** - -Sequence** +eb1bba56-55b1-4a71-8b31-0381812f588a + +> - + -Sequence*->Sequence** - - +d78eaf31-cc9d-484d-b564-dbd5912378fa->eb1bba56-55b1-4a71-8b31-0381812f588a + + - + -At(Robot,Table)* - -At(Robot,Table)* +3d7f6aa9-62b5-4852-ab3e-ac0199cc46a8 + +At(Robot,Table) - + -Sequence*->At(Robot,Table)* - - +d78eaf31-cc9d-484d-b564-dbd5912378fa->3d7f6aa9-62b5-4852-ab3e-ac0199cc46a8 + + - + -At(Robot,Table) - -At(Robot,Table) +20b57b46-d59d-4b04-a4ed-eff12e6adc91 + +At(Robot,Table) - + -Sequence**->At(Robot,Table) - - +eb1bba56-55b1-4a71-8b31-0381812f588a->20b57b46-d59d-4b04-a4ed-eff12e6adc91 + + diff --git a/robowaiter/llm_client/ask_llm.py b/robowaiter/llm_client/ask_llm.py index f82ce28..4644b6b 100644 --- a/robowaiter/llm_client/ask_llm.py +++ b/robowaiter/llm_client/ask_llm.py @@ -37,10 +37,7 @@ def ask_llm(question): if __name__ == '__main__': question = ''' - 以下是环境信息，请回答对话和目标状态 - [环境信息] - State: {At(Robot, Table), NotHolding, Available(SpongeGourd)} - chat_list: [(Customer, "桌子有点脏，能帮我擦一下吗？")] + python中如何通过类名字符串的方式来代替isinstance的作用 ''' print(ask_llm(question)) \ No newline at end of file diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index ef9a744..1f46560 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -1,8 +1,10 @@ import io import contextlib -from robowaiter.behavior_tree.utils import load_bt_from_ptml,find_node_by_name,print_tree_from_root -from robowaiter.behavior_tree.obtea.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm,state_transition # 调用最优行为树扩展算法 +from robowaiter.utils.bt.load import load_bt_from_ptml,find_node_by_name,print_tree_from_root +from robowaiter.utils.bt.visitor import StatusVisitor + +from robowaiter.behavior_tree.obtea.OptimalBTExpansionAlgorithm import Action # 调用最优行为树扩展算法 from robowaiter.behavior_tree.obtea.opt_bt_exp_main import BTOptExpInterface from robowaiter.behavior_lib.act.DelSubTree import DelSubTree @@ -22,6 +24,7 @@ class Robot(object): self.last_tick_output = "" self.action_list = None + def set_scene(self,scene): self.scene = scene @@ -32,7 +35,10 @@ class Robot(object): sub_task_seq = sub_task_place_holder.parent sub_task_seq.children.pop() self.scene.sub_task_seq = sub_task_seq - print(self.scene.sub_task_seq) + + self.bt_visitor = StatusVisitor() + self.bt.visitors.append(self.bt_visitor) + def expand_sub_task_tree(self,goal): if self.action_list is None: @@ -52,12 +58,12 @@ class Robot(object): # 加入删除子树的节点 seq = Sequence(name="Sequence", memory=False) - seq.children.append(sub_task_bt.root) + seq.add_child(sub_task_bt.root) del_sub_tree = DelSubTree() del_sub_tree.set_scene(self.scene) - seq.children.append(del_sub_tree) + seq.add_child(del_sub_tree) - self.scene.sub_task_seq.children.append(seq) + self.scene.sub_task_seq.add_child(seq) print("当前行为树为：") print_tree_from_root(self.bt.root) @@ -75,24 +81,16 @@ class Robot(object): self.next_response_time += self.response_frequency self.step_num += 1 - # 创建一个StringIO对象 - output = io.StringIO() + self.bt.tick() + bt_output = self.bt_visitor.output_str - # 将print输出重定向到StringIO对象 - with contextlib.redirect_stdout(output): - self.bt.tick() - - # 获取StringIO对象中的字符串值 - contents = output.getvalue() - output.close() - - if contents != self.last_tick_output: + if bt_output != self.last_tick_output: print(f"==== time:{self.scene.time:f}s ======") - print(contents) + print(bt_output) print("\n") - self.last_tick_output = contents + self.last_tick_output = bt_output if __name__ == '__main__': pass \ No newline at end of file diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index c4186fe..88cdecd 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -24,9 +24,10 @@ class SceneVLM(Scene): # self.move_task_area(i) # self.op_task_execute(i) - if op_type<=10: - self.move_task_area(op_type) - self.op_task_execute(op_type) - + # if op_type<=10: + # self.move_task_area(op_type) + # self.op_task_execute(op_type) + pass + def _step(self): pass diff --git a/robowaiter/utils/__init__.py b/robowaiter/utils/__init__.py new file mode 100644 index 0000000..af2a9db --- /dev/null +++ b/robowaiter/utils/__init__.py @@ -0,0 +1,5 @@ + + +from robowaiter.utils import * +from robowaiter.utils import * + diff --git a/robowaiter/utils/bt/__init__.py b/robowaiter/utils/bt/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robowaiter/utils/bt/draw.py b/robowaiter/utils/bt/draw.py new file mode 100644 index 0000000..4ab4852 --- /dev/null +++ b/robowaiter/utils/bt/draw.py @@ -0,0 +1,331 @@ + +############################################################################## +# Imports +############################################################################## + +import os +import typing +import uuid + +import pydot +from py_trees import behaviour +from py_trees import blackboard +from py_trees import common +from py_trees import composites +from py_trees import decorators +from py_trees import utilities + +COMPOSITE_NODE_SIZE = 0.01 + + +def dot_tree( + root: behaviour.Behaviour, + visibility_level: common.VisibilityLevel = common.VisibilityLevel.DETAIL, + collapse_decorators: bool = False, + with_blackboard_variables: bool = False, + with_qualified_names: bool = False): + """ + Paint your tree on a pydot graph. + + .. seealso:: :py:func:`render_dot_tree`. + + Args: + root (:class:`~py_trees.behaviour.Behaviour`): the root of a tree, or subtree + visibility_level (optional): collapse subtrees at or under this level + collapse_decorators (optional): only show the decorator (not the child), defaults to False + with_blackboard_variables (optional): add nodes for the blackboard variables + with_qualified_names (optional): print the class information for each behaviour in each node, defaults to False + + Returns: + pydot.Dot: graph + + Examples: + + .. code-block:: python + + # convert the pydot graph to a string object + print("{}".format(py_trees.display.dot_graph(root).to_string())) + """ + + def get_node_attributes(node): + blackbox_font_colours = {common.BlackBoxLevel.DETAIL: "dodgerblue", + common.BlackBoxLevel.COMPONENT: "lawngreen", + common.BlackBoxLevel.BIG_PICTURE: "white" + } + if node.type =="Selector": + attributes = ('diamond', 'cyan', 'black') # octagon + elif node.type =="Sequence": + attributes = ('octagon', 'orange', 'black') + elif isinstance(node, composites.Parallel): + attributes = ('parallelogram', 'gold', 'black') + elif isinstance(node, decorators.Decorator): + attributes = ('ellipse', 'ghostwhite', 'black') + elif node.type =="Act": + attributes = ('box', 'lawngreen', 'black') + else: + attributes = ('ellipse', 'yellow', 'black') + + try: + if node.blackbox_level != common.BlackBoxLevel.NOT_A_BLACKBOX: + attributes = (attributes[0], 'gray20', blackbox_font_colours[node.blackbox_level]) + except AttributeError: + # it's a blackboard client, not a behaviour, just pass + pass + return attributes + + def get_node_label(node_name, behaviour): + """ + This extracts a more detailed string (when applicable) to append to + that which will be used for the node name. + """ + # Custom handling of composites provided by this library. Not currently + # providing a generic mechanism for others to customise visualisations + # for their derived composites. + # prefix = "" + # policy = "" + ''' + if isinstance(behaviour, composites.Composite): + try: + if behaviour.memory: + prefix += console.circled_m + except AttributeError: + pass + try: + if behaviour.policy.synchronise: + prefix += console.lightning_bolt + except AttributeError: + pass + try: + policy = behaviour.policy.__class__.__name__ + except AttributeError: + pass + try: + indices = [str(behaviour.children.index(child)) for child in behaviour.policy.children] + policy += "({})".format(', '.join(sorted(indices))) + except AttributeError: + pass + + node_label = f"{prefix} {node_name}" if prefix else node_name + if policy: + node_label += f"\n{str(policy)}" + 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 = "?" + return node_name + + fontsize = 20 + blackboard_colour = "blue" # "dimgray" + graph = pydot.Dot(graph_type='digraph', ordering="out") + graph.set_name("pastafarianism") # consider making this unique to the tree sometime, e.g. based on the root name + # fonts: helvetica, times-bold, arial (times-roman is the default, but this helps some viewers, like kgraphviewer) + graph.set_graph_defaults(fontname='times-roman') # splines='curved' is buggy on 16.04, but would be nice to have + graph.set_node_defaults(fontname='times-roman') + graph.set_edge_defaults(fontname='times-roman') + (node_shape, node_colour, node_font_colour) = get_node_attributes(root) + root_name = str(root.id) + node_root = pydot.Node( + name=root_name, + label=get_node_label(root.name, root), + shape=node_shape, + style="filled", + fillcolor=node_colour, + fontsize=fontsize, + fontcolor=node_font_colour, + ) + if isinstance(root, composites.Composite): + node_root.set_height(COMPOSITE_NODE_SIZE) + node_root.set_width(COMPOSITE_NODE_SIZE) + graph.add_node(node_root) + behaviour_id_name_map = {root.id: str(root.id)} + + def add_children_and_edges(root, root_node, root_dot_name, visibility_level, collapse_decorators): + if isinstance(root, decorators.Decorator) and collapse_decorators: + return + if visibility_level < root.blackbox_level: + node_names = [] + for c in root.children: + (node_shape, node_colour, node_font_colour) = get_node_attributes(c) + node_name = str(c.id) + # while node_name in behaviour_id_name_map.values(): + # node_name += "" + behaviour_id_name_map[c.id] = node_name + # Node attributes can be found on page 5 of + # https://graphviz.gitlab.io/_pages/pdf/dot.1.pdf + # Attributes that may be useful: tooltip, xlabel + node = pydot.Node( + name=str(c.id), + label=get_node_label(c.name, c), + shape=node_shape, + style="filled", + fillcolor=node_colour, + fontsize=fontsize, + fontcolor=node_font_colour, + ) + if isinstance(c, composites.Composite): + node.set_height(COMPOSITE_NODE_SIZE) + node.set_width(COMPOSITE_NODE_SIZE) + node_names.append(node_name) + graph.add_node(node) + edge = pydot.Edge(root_dot_name, node_name) + graph.add_edge(edge) + if c.children != []: + add_children_and_edges(c, node, node_name, visibility_level, collapse_decorators) + + add_children_and_edges(root, node_root, root_name, visibility_level, collapse_decorators) + + def create_blackboard_client_node(blackboard_client_name: str): + return pydot.Node( + name=blackboard_client_name, + label=blackboard_client_name, + shape="ellipse", + style="filled", + color=blackboard_colour, + fillcolor="gray", + fontsize=fontsize - 2, + fontcolor=blackboard_colour, + ) + + def add_blackboard_nodes(blackboard_id_name_map: typing.Dict[uuid.UUID, str]): + data = blackboard.Blackboard.storage + metadata = blackboard.Blackboard.metadata + clients = blackboard.Blackboard.clients + # add client (that are not behaviour) nodes + subgraph = pydot.Subgraph( + graph_name="Blackboard", + id="Blackboard", + label="Blackboard", + rank="sink", + ) + + for unique_identifier, client_name in clients.items(): + if unique_identifier not in blackboard_id_name_map: + subgraph.add_node( + create_blackboard_client_node(client_name) + ) + # add key nodes + for key in blackboard.Blackboard.keys(): + try: + value = utilities.truncate(str(data[key]), 20) + label = key + ": " + "{}".format(value) + except KeyError: + label = key + ": " + "-" + blackboard_node = pydot.Node( + key, + label=label, + shape='box', + style="filled", + color=blackboard_colour, + fillcolor='white', + fontsize=fontsize - 1, + fontcolor=blackboard_colour, + width=0, height=0, fixedsize=False, # only big enough to fit text + ) + subgraph.add_node(blackboard_node) + for unique_identifier in metadata[key].read: + try: + edge = pydot.Edge( + blackboard_node, + blackboard_id_name_map[unique_identifier], + color="green", + constraint=False, + weight=0, + ) + except KeyError: + edge = pydot.Edge( + blackboard_node, + clients[unique_identifier].__getattribute__("name"), + color="green", + constraint=False, + weight=0, + ) + graph.add_edge(edge) + for unique_identifier in metadata[key].write: + try: + edge = pydot.Edge( + blackboard_id_name_map[unique_identifier], + blackboard_node, + color=blackboard_colour, + constraint=False, + weight=0, + ) + except KeyError: + edge = pydot.Edge( + clients[unique_identifier].__getattribute__("name"), + blackboard_node, + color=blackboard_colour, + constraint=False, + weight=0, + ) + graph.add_edge(edge) + graph.add_subgraph(subgraph) + + if with_blackboard_variables: + blackboard_id_name_map = {} + for b in root.iterate(): + for bb in b.blackboards: + blackboard_id_name_map[bb.id()] = behaviour_id_name_map[b.id] + add_blackboard_nodes(blackboard_id_name_map) + + return graph + + +def render_dot_tree(root: behaviour.Behaviour, + visibility_level: common.VisibilityLevel = common.VisibilityLevel.DETAIL, + collapse_decorators: bool = False, + name: str = None, + target_directory: str = os.getcwd(), + with_blackboard_variables: bool = False, + with_qualified_names: bool = False): + """ + Render the dot tree to .dot, .svg, .png. files in the current + working directory. These will be named with the root behaviour name. + + Args: + root: the root of a tree, or subtree + visibility_level: collapse subtrees at or under this level + collapse_decorators: only show the decorator (not the child) + name: name to use for the created files (defaults to the root behaviour name) + target_directory: default is to use the current working directory, set this to redirect elsewhere + with_blackboard_variables: add nodes for the blackboard variables + with_qualified_names: print the class names of each behaviour in the dot node + + Example: + + Render a simple tree to dot/svg/png file: + + .. graphviz:: dot/sequence.dot + + .. code-block:: python + + root = py_trees.composites.Sequence("Sequence") + for job in ["Action 1", "Action 2", "Action 3"]: + success_after_two = py_trees.behaviours.Count(name=job, + fail_until=0, + running_until=1, + success_until=10) + root.add_child(success_after_two) + py_trees.display.render_dot_tree(root) + + .. tip:: + + A good practice is to provide a command line argument for optional rendering of a program so users + can quickly visualise what tree the program will execute. + """ + graph = dot_tree( + root, visibility_level, collapse_decorators, + with_blackboard_variables=with_blackboard_variables, + with_qualified_names=with_qualified_names) + filename_wo_extension_to_convert = root.name if name is None else name + filename_wo_extension = utilities.get_valid_filename(filename_wo_extension_to_convert) + filenames = {} + for extension, writer in {"dot": graph.write, "png": graph.write_png, "svg": graph.write_svg}.items(): + filename = filename_wo_extension + '.' + extension + pathname = os.path.join(target_directory, filename) + print("Writing {}".format(pathname)) + writer(pathname) + filenames[extension] = pathname + return filenames diff --git a/robowaiter/behavior_tree/utils.py b/robowaiter/utils/bt/load.py similarity index 79% rename from robowaiter/behavior_tree/utils.py rename to robowaiter/utils/bt/load.py index 52e25df..cb7f61a 100644 --- a/robowaiter/behavior_tree/utils.py +++ b/robowaiter/utils/bt/load.py @@ -14,18 +14,6 @@ def load_bt_from_ptml(scene, ptml_path, behavior_lib_path): # print(ptree.display.unicode_tree(root=bt.root, show_status=True)) return bt -def load_bt_from_ptml_str(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:') - print_tree_from_root(bt.root) - # print(ptree.display.unicode_tree(root=bt.root, show_status=True)) - return bt - def print_tree_from_root(node, indent=0): """ diff --git a/robowaiter/utils/bt/visitor.py b/robowaiter/utils/bt/visitor.py new file mode 100644 index 0000000..8353ab6 --- /dev/null +++ b/robowaiter/utils/bt/visitor.py @@ -0,0 +1,30 @@ + +from py_trees.visitors import VisitorBase + + +class StatusVisitor(VisitorBase): + """ + Logging is done with the behaviour's logger. + """ + + def __init__(self) -> None: + super(StatusVisitor, self).__init__(full=False) + self.output_str = "" + + def initialise(self) -> None: + """Override if any resetting of variables needs to be performed between ticks (i.e. visitations).""" + self.output_str = "" + + + def run(self, behavior) -> None: + """ + Log behaviour information on the debug channel. + + Args: + behavior: behaviour being visited. + """ + if behavior.type in ("Sequence","Selector"): + return + else: + self.output_str += f"{behavior.print_name}: {behavior.status.value}\n" + diff --git a/run_robowaiter.py b/run_robowaiter.py index 8592471..1c11b81 100644 --- a/run_robowaiter.py +++ b/run_robowaiter.py @@ -1,7 +1,7 @@ import os from robowaiter import Robot, task_map -TASK_NAME = 'VLN' +TASK_NAME = 'VLM' # create robot project_path = "./robowaiter" diff --git a/sub_task.ptml b/sub_task.ptml index 5829971..3473e6b 100644 --- a/sub_task.ptml +++ b/sub_task.ptml @@ -1,7 +1,7 @@ selector{ -cond At(Robot,Table) +cond At(Coffee,Bar) selector{ -cond At(Robot,Bar) -act MoveTo(Table) +cond At(Robot,CoffeeMachine) +act MakeCoffee } }