update

README.md

@@ -12,164 +12,45 @@ pip install -e .
 ```
 以上步骤将完成robowaiter项目以及相关依赖库的安装
 
+### 安装UI
+1. 安装 [graphviz-9.0.0](https://gitlab.com/api/v4/projects/4207231/packages/generic/graphviz-releases/9.0.0/windows_10_cmake_Release_graphviz-install-9.0.0-win64.exe) (详见[官网](https://www.graphviz.org/download/#windows))
+2. 将软件安装目录的bin文件添加到系统环境中。如电脑是Windows系统，Graphviz安装在D:\Program Files (x86)\Graphviz2.38，该目录下有bin文件，将该路径添加到电脑系统环境变量path中，即D:\Program Files (x86)\Graphviz2.38\bin。
+
 ### 快速入门
 1. 安装UE及Harix插件，打开默认项目并运行
-2. 运行 run_robowaiter.py 文件即可实现机器人控制端与仿真器的交互
+2. 运行 tasks 文件夹下的任意场景即可实现机器人控制端与仿真器的交互
 
 
-# 运行流程介绍
+# 代码框架介绍
-run_robowaiter.py 入口文件如下：
-```python
-import os
-from robowaiter import Robot, task_map
 
-TASK_NAME = 'GQA'
-
-# create robot
-project_path = "./robowaiter"
-ptml_path = os.path.join(project_path, 'robot/Default.ptml')
-behavior_lib_path = os.path.join(project_path, 'behavior_lib')
-
-robot = Robot(ptml_path,behavior_lib_path)
-
-# create task
-task = task_map[TASK_NAME](robot)
-task.reset()
-task.run()
-```
 
 ## Robot 
 Robot是机器人类，包括从ptml加载行为树的方法，以及执行行为树的方法等
 
 
-## task_map
+## tasks
-task_map是任务字典，通过任务缩写来返回相应的场景类。
+tasks文件夹中存放的场景定义及运行代码。
 
-| 缩写 | 任务      |
+| 缩写                  | 任务          |
-|----|---------|
+|---------------------|-------------|
-| AEM  | 主动探索和记忆 |
+| AEM                 | 主动探索和记忆     |
-| GQA  | 具身多轮对话  |
+| GQA                 | 具身多轮对话      |
-| VLN  | 视觉语言导航  |
+| VLN                 | 视觉语言导航      |
-| VLM  | 视觉语言操作  |
+| VLM                 | 视觉语言操作      |
-| OT  | 复杂开放任务   |
+| OT                  | 复杂开放任务      |
-| AT  | 自主任务    |
+| AT                  | 自主任务        |
+| CafeDailyOperations | 整体展示：咖啡厅的一天 |
+| Interact            | 命令行自由交互     |
 
 
 ## Scene
 Scene是场景基类，task_map返回的任务场景都继承于Scene。
 该类实现了一些通用的场景操作接口。
 
-### 场景中物品类别
-
-| ID  | Item                 |
-|-----|----------------------|
-| 0   | Mug                  |
-| 1   | Banana               |
-| 2   | Toothpaste           |
-| 3   | Bread                |
-| 4   | Softdrink            |
-| 5   | Yogurt               |
-| 6   | ADMilk               |
-| 7   | VacuumCup            |
-| 8   | Bernachon            |
-| 9   | BottledDrink         |
-| 10  | PencilVase           |
-| 11  | Teacup               |
-| 12  | Caddy                |
-| 13  | Dictionary           |
-| 14  | Cake                 |
-| 15  | Date                 |
-| 16  | Stapler              |
-| 17  | LunchBox             |
-| 18  | Bracelet             |
-| 19  | MilkDrink            |
-| 20  | CocountWater         |
-| 21  | Walnut               |
-| 22  | HamSausage           |
-| 23  | GlueStick            |
-| 24  | AdhensiveTape        |
-| 25  | Calculator           |
-| 26  | Chess                |
-| 27  | Orange               |
-| 28  | Glass                |
-| 29  | Washbowl             |
-| 30  | Durian               |
-| 31  | Gum                  |
-| 32  | Towl                 |
-| 33  | OrangeJuice          |
-| 34  | Cardcase             |
-| 35  | RubikCube            |
-| 36  | StickyNotes          |
-| 37  | NFCJuice             |
-| 38  | SpringWater          |
-| 39  | Apple                |
-| 40  | Coffee               |
-| 41  | Gauze                |
-| 42  | Mangosteen           |
-| 43  | SesameSeedCake       |
-| 44  | Glove                |
-| 45  | Mouse                |
-| 46  | Kettle               |
-| 47  | Atomize              |
-| 48  | Chips                |
-| 49  | SpongeGourd          |
-| 50  | Garlic               |
-| 51  | Potato               |
-| 52  | Tray                 |
-| 53  | Hemomanometer        |
-| 54  | TennisBall           |
-| 55  | ToyDog               |
-| 56  | ToyBear              |
-| 57  | TeaTray              |
-| 58  | Sock                 |
-| 59  | Scarf                |
-| 60  | ToiletPaper          |
-| 61  | Milk                 |
-| 62  | Soap                 |
-| 63  | Novel                |
-| 64  | Watermelon           |
-| 65  | Tomato               |
-| 66  | CleansingFoam        |
-| 67  | CocountMilk          |
-| 68  | SugarlessGum         |
-| 69  | MedicalAdhensiveTape |
-| 70  | SourMilkDrink        |
-| 71  | PaperCup             |
-| 72  | Tissue               |
-| 73  | YogurtDrink          |
-| 74  | Newspaper            |
-| 75  | Box                  |
-| 76  | PaperCupStarbucks    |
-| 77  | CoffeeMachine        |
-| 78  | GingerLHand          |
-| 79  | GingerRHand          |
-| 80  | Straw                |
-| 81  | Cake                 |
-| 82  | Tray                 |
-| 83  | Bread                |
-| 84  | Glass                |
-| 85  | Door                 |
-| 86  | Mug                  |
-| 87  | Machine              |
-| 88  | Packaged Coffee      |
-| 89  | Cube Sugar           |
-| 90  | Apple                |
-| 91  | Spoon                |
-| 92  | Drinks               |
-| 93  | Drink                |
-| 94  | Take-Away Cup        |
-| 95  | Saucer               |
-| 96  | Trash Bin            |
-| 97  | Knife                |
-| 251 | Ginger               |
-| 252 | Floor                |
-| 253 | Roof                 |
-| 254 | Wall                 |
-注意：78及以后无法使用add_object方法生成
-
 # 调用大模型接口
 运行llm_client.py文件调用大模型进行多轮对话。
 ```shell
-python llm_client.py
+cd robowaiter/llm_client
+python multi_rounds.py
 ```
-输入字符即可等待回答，输入end表示对话结束。
+输入字符即可等待回答

requirements.txt

@@ -14,3 +14,4 @@ numpy~=1.26.0
 setuptools~=68.0.0
 pydot~=1.4.2
 colorama~=0.4.6
+pyqt5

robowaiter/algos/navigator/navigate.py

@@ -114,7 +114,7 @@ class Navigator:
                 self.scene.walk_to(next_pos[0], next_pos[1], math.degrees(yaw), velocity=self.v, dis_limit=10)
 
                 # 拍照片
-                if self.scene.take_picture:
+                if self.scene.show_ui:
                     self.scene.get_obstacle_point(self.scene.db, self.scene.status, map_ratio=self.scene.map_ratio)
 
                 self.planner.path = self.planner.path[next_step - 1:]  # 去除已走过的路径

robowaiter/behavior_lib/_base/Behavior.py

@@ -15,8 +15,12 @@ class Bahavior(ptree.behaviour.Behaviour):
     scene = None
     print_name_prefix = ""
     tables_for_placement = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3',"BrightTable6"}
-    all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk',
+    # all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk',
-                  'VacuumCup'}
+    #               'VacuumCup'}
+
+    all_object = {
+        'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk','VacuumCup',
+        'Chips', 'NFCJuice', 'Bernachon', 'ADMilk', 'SpringWater'}
 
     # BrightTable5 = Table4
     tables_for_guiding = {"QuietTable1","QuietTable2",
@@ -103,7 +107,7 @@ class Bahavior(ptree.behaviour.Behaviour):
 
 
 
-    # let behavior node interact with the scene
+    # let behavior node Interact with the scene
     def set_scene(self, scene=None):
         if scene:
             self.scene = scene

robowaiter/behavior_lib/act/DealChat.py

@@ -3,11 +3,22 @@ from robowaiter.behavior_lib._base.Act import Act
 
 from robowaiter.llm_client.multi_rounds import ask_llm, new_history
 import random
+from collections import deque
 
 
 # import spacy
 # nlp = spacy.load('en_core_web_lg')
 
+class History(deque):
+    def __init__(self,scene,customer_name):
+        super().__init__(maxlen=7)
+        self.scene = scene
+        self.customer_name = customer_name
+
+    def append(self, __x) -> None:
+        super().append(__x)
+        self.scene.ui_func(("new_history",self.customer_name, __x))
+
 
 class DealChat(Act):
     def __init__(self):
@@ -27,10 +38,15 @@ class DealChat(Act):
 
         if name == "Goal":
             self.create_sub_task(goal=sentence)
+            self.scene.ui_func(("new_history", "System", {
+                "role": "user",
+                "content": "set goal: " + sentence
+            }))
+
             return ptree.common.Status.RUNNING
 
         if name not in self.scene.state["chat_history"]:
-            self.scene.state["chat_history"][name] = new_history()
+            self.scene.state["chat_history"][name] = History(self.scene,name)
 
         history = self.scene.state["chat_history"][name]
         self.scene.state["attention"]["customer"] = name
@@ -64,6 +80,7 @@ class DealChat(Act):
 
         self.scene.robot.expand_sub_task_tree(goal_set)
 
+
     def get_object_info(self,**args):
         try:
             obj = args['obj']

robowaiter/behavior_lib/act/DelSubTree.py

@@ -19,4 +19,5 @@ class DelSubTree(Act):
 
         sub_task_tree = self.parent
         self.scene.sub_task_seq.children.remove(sub_task_tree)
+        self.scene.draw_current_bt()
         return Status.RUNNING

robowaiter/behavior_lib/act/FreeHands.py

@@ -0,0 +1,34 @@
+import py_trees as ptree
+from typing import Any
+from robowaiter.behavior_lib._base.Act import Act
+from robowaiter.behavior_lib._base.Behavior import Status
+
+class FreeHands(Act):
+    can_be_expanded = True
+    num_args = 0
+    valid_args = set()
+
+    def __init__(self, *args):
+        super().__init__(*args)
+
+
+    @classmethod
+    def get_info(cls):
+        info = {}
+        info["pre"]= set()
+        info['add'] = {f'Holding(Nothing)'}
+        info['del_set'] = {f'Holding({obj})' for obj in cls.all_object}
+        info['cost'] = 0
+        return info
+
+    def _update(self) -> ptree.common.Status:
+
+
+        if self.scene.show_ui:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio,update_info_count=1)
+
+        self.scene.state["condition_set"] |= (self.info["add"])
+        self.scene.state["condition_set"] -= self.info["del_set"]
+
+
+        return Status.RUNNING

robowaiter/behavior_lib/act/Make.py

@@ -39,6 +39,9 @@ class Make(Act):
 
     def _update(self) -> ptree.common.Status:
 
+        if self.scene.show_ui:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         self.scene.move_task_area(self.op_type)
         self.scene.op_task_execute(self.op_type)
 
@@ -54,7 +57,7 @@ class Make(Act):
                 #     obj_info = obj_dict[id]
                 #     obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
                 #     print(id,obj.name,obj_x,obj_y,obj_z)
-        if self.scene.take_picture:
+        if self.scene.show_ui:
             self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio,update_info_count=1)
 
         self.scene.state["condition_set"] |= (self.info["add"])

robowaiter/behavior_lib/act/MoveTo.py

@@ -21,7 +21,11 @@ class MoveTo(Act):
             info['pre'] |= {f'Exist({arg})'}
         info["add"] = {f'At(Robot,{arg})'}
         info["del_set"] = {f'At(Robot,{place})' for place in cls.valid_args if place != arg}
-        info['cost']=5
+        info['cost'] = 5
+        # if arg!='Anything':
+        #     info['cost']=5
+        # else:
+        #     info['cost']=0
         return info
 
 
@@ -33,7 +37,7 @@ class MoveTo(Act):
         # navigator.navigate_old(goal, animation=False)
 
         # 拍照片
-        if self.scene.take_picture:
+        if self.scene.show_ui:
             self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
 
         # #####################################
@@ -69,7 +73,7 @@ class MoveTo(Act):
             self.scene.move_to_obj(obj_id=obj_id)
             # #####################################
 
-        if self.scene.take_picture:
+        if self.scene.show_ui:
             self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
 
 

robowaiter/behavior_lib/act/PickUp.py

@@ -7,6 +7,7 @@ class PickUp(Act):
     can_be_expanded = True
     num_args = 1
     valid_args = Act.all_object
+    # valid_args.add("Anything")
     def __init__(self, *args):
         super().__init__(*args)
         self.target_obj = self.args[0]
@@ -20,6 +21,15 @@ class PickUp(Act):
         info["del_set"] = {f'Holding(Nothing)'}
         for place in cls.valid_args:
             info["del_set"] |= {f'On({arg},{place})'}
+        info['cost'] = 1
+
+        # if arg != 'Anything':
+        #     info['cost'] = 1
+        # else:
+        #     info['cost'] = 0
+        #
+        #     info["pre"] = {}
+
         return info
 
 
@@ -56,7 +66,7 @@ class PickUp(Act):
         self.scene.move_task_area(op_type=16, obj_id=obj_id)
         self.scene.op_task_execute(op_type=16, obj_id=obj_id)
 
-        if self.scene.take_picture:
+        if self.scene.show_ui:
             self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio,update_info_count=1)
 
         self.scene.state["condition_set"] |= (self.info["add"])

robowaiter/behavior_lib/act/PutDown.py

@@ -24,6 +24,14 @@ class PutDown(Act):
         info["del_set"] = {f'Holding({arg[0]})'}
 
         info['cost'] = 1
+
+        # if arg[0]!='Anything':
+        #     info['cost'] = 1
+        # else:
+        #     info['cost'] = 0
+        #     info["pre"] = {}
+        #     info["add"] = {f'Holding(Nothing)'}
+        #     info["del_set"] = {f'Holding({obj})' for obj in cls.valid_args if obj[0] != arg}
         return info
 
 
@@ -40,8 +48,11 @@ class PutDown(Act):
         Act.num_of_obj_on_place[self.target_place]+=1
 
         self.scene.move_task_area(op_type, release_pos=release_pos)
+
+        if self.target_obj == "Chips":
+            release_pos[2] +=3
         self.scene.op_task_execute(op_type, release_pos=release_pos)
-        if self.scene.take_picture:
+        if self.scene.show_ui:
             self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio,update_info_count=1)
 
         self.scene.state["condition_set"] |= (self.info["add"])

robowaiter/behavior_lib/act/Turn.py

@@ -74,9 +74,12 @@ class Turn(Act):
 
     def _update(self) -> ptree.common.Status:
 
+        if self.scene.show_ui:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         self.scene.move_task_area(self.op_type)
         self.scene.op_task_execute(self.op_type)
-        if self.scene.take_picture:
+        if self.scene.show_ui:
             self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
         self.scene.state["condition_set"] |= (self.info["add"])
         self.scene.state["condition_set"] -= self.info["del_set"]

robowaiter/behavior_lib/cond/FocusingCustomer.py

@@ -11,8 +11,8 @@ class FocusingCustomer(Cond):
     def _update(self) -> ptree.common.Status:
         # if self.scene.status?
         if "customer" in self.scene.state['attention']:
-            if self.scene.take_picture:
+            # if self.scene.take_picture:
-                self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+            #     self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
             return ptree.common.Status.SUCCESS
         else:
             goal = Cond.place_xy_yaw_dic['Bar']

robowaiter/behavior_lib/cond/NewCustomer.py

@@ -14,8 +14,9 @@ class NewCustomer(Cond):
 
     def _update(self) -> ptree.common.Status:
 
-        if self.scene.take_picture:
+        if self.scene.show_ui:
             self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         # 获取customer的位置
         # bar (247.0, 520.0, 100.0)
         close_to_bar = False

robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py

@@ -37,6 +37,17 @@ def state_transition(state,action):
     return new_state
 
 
+def conflict(c):
+    have_at = False
+    for str in c:
+        if 'At' in  str:
+            if not have_at:
+                have_at = True
+            else:
+                return True
+    return False
+
+
 #本文所提出的完备规划算法
 class OptBTExpAlgorithm:
     def __init__(self,verbose=False):
@@ -51,7 +62,8 @@ class OptBTExpAlgorithm:
     def clear(self):
         self.bt = None
         self.nodes = []
-        self.traversed = []
+        self.traversed = [] #存cond
+        self.expanded = [] #存整个
         self.conditions = []
         self.conditions_index = []
 
@@ -77,7 +89,6 @@ class OptBTExpAlgorithm:
         self.nodes.append(copy.deepcopy(cond_anc_pair)) # the set of explored but unexpanded conditions
         self.traversed = [goal] # the set of expanded conditions
 
-
         while len(self.nodes)!=0:
 
             #  Find the condition for the shortest cost path
@@ -85,6 +96,19 @@ class OptBTExpAlgorithm:
             min_cost = float ('inf')
             index= -1
             for i,cond_anc_pair in enumerate(self.nodes):
+
+                ########### 剪枝操作
+                # cond_tmp = cond_anc_pair.cond_leaf.content
+                # valid = True
+                # for pn in self.expanded:  # 剪枝操作
+                #     if isinstance(pn.act_leaf.content,Action):
+                #         if pn.act_leaf.content.name==cond_anc_pair.act_leaf.content.name and cond_tmp <= pn.cond_leaf.content:
+                #             valid = False
+                #             break
+                # if not valid:
+                #     continue
+                ########### 剪枝操作
+
                 if cond_anc_pair.cond_leaf.mincost < min_cost:
                     min_cost = cond_anc_pair.cond_leaf.mincost
                     pair_node = copy.deepcopy(cond_anc_pair)
@@ -100,10 +124,22 @@ class OptBTExpAlgorithm:
             # Mount the action node and extend BT. T = Eapand(T,c,A(c))
             if c!=goal:
                 if c!=set():
+
+                    # 挂在上去的时候判断要不要挂载
+                    ########### 剪枝操作 发现行不通
+                    # valid = True
+                    # for pn in self.expanded:  # 剪枝操作
+                    #     if isinstance(pn.act_leaf.content,Action):
+                    #         if pn.act_leaf.content.name==pair_node.act_leaf.content.name and c <= pn.cond_leaf.content:
+                    #             valid = False
+                    #             break
+                    # if valid:
+                    ########### 剪枝操作
                     sequence_structure = ControlBT(type='>')
                     sequence_structure.add_child(
                         [copy.deepcopy(pair_node.cond_leaf), copy.deepcopy(pair_node.act_leaf)])
                     subtree.add_child([copy.deepcopy(sequence_structure)])  # subtree 是回不断变化的，它的父亲是self.bt
+                    self.expanded.append(copy.deepcopy(pair_node))
                     # 增加实时条件判断，满足条件就不再扩展
                     if c <= self.scene.state["condition_set"]:
                         return True
@@ -128,6 +164,10 @@ class OptBTExpAlgorithm:
                             print("———— 满足条件可以扩展")
                         c_attr = (actions[i].pre | c) - actions[i].add
 
+                        # 这样剪枝存在错误性
+                        if conflict(c_attr):
+                            continue
+
                         # 剪枝操作,现在的条件是以前扩展过的条件的超集
                         valid = True
                         for j in self.traversed:  # 剪枝操作

robowaiter/llm_client/data/fix_questions.txt

@@ -9,7 +9,7 @@ ok, 有需要您再找我。
 做一杯咖啡
 好的，我马上做咖啡
 create_sub_task
-{"goal":"On(Coffee,CoffeeTable)"}00
+{"goal":"On(Coffee,CoffeeTable)"}
 
 不用了。
 好的，您有需要再跟我说
@@ -74,7 +74,7 @@ create_sub_task
 没事儿，为您服务是我的荣幸！
 
 大厅的桌子好啊，快带我去呀！
-好的好的，跟我来，我带你去找找。
+好的好的，请跟我来。
 create_sub_task
 {"goal":"At(Robot,WaterTable)"}
 
@@ -97,9 +97,9 @@ create_sub_task
 
 
 下班啦！别忘了打扫卫生。
-收到！请问您需要来点什么吗？
+收到！下班啦！
 create_sub_task
-"{""Is(Floor,Clean)"",""Is(Table1,Clean)"",""Is(Chairs,Clean)"",""Is(AC,Off)"",""Is(HallLight,Off)"",""Is(TubeLight,Off)"",""Is(Curtain,Off)""}"
+{"goal":"Is(Floor,Clean),Is(Table1,Clean),Is(Chairs,Clean),Is(AC,Off),Is(HallLight,Off),Is(TubeLight,Off),Is(Curtain,Off)"}
 
 
 请问洗手间在哪里？
@@ -112,6 +112,12 @@ get_object_info
 给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。
 好的，请稍等。
 create_sub_task
+{"goal":"On(Chips,WaterTable),On(NFCJuice,WaterTable)"}
+
+
+给我来份薯片和果汁，我坐在对面的桌子那儿。
+好的，请稍等。
+create_sub_task
 {"goal":"On(BottledDrink,WaterTable),On(Yogurt,WaterTable)"}
 
 
@@ -186,7 +192,17 @@ get_object_info
 get_object_info
 {"obj":"洗手间"}
 
+我带着孩子呢，想要宽敞亮堂的地方。
+好的，我明白了，那么我们推荐您到大厅的桌子，那里的空间比较宽敞，环境也比较明亮，适合带着孩子一起用餐。
+
+冰红茶
+好的
+create_sub_task
+{"goal":"On(Softdrink,Bar)"}
 
 
-
+水杯
+好的
+create_sub_task
+{"goal":"On(Glass,Bar)"}
 

robowaiter/proto/camera.py

@@ -342,7 +342,8 @@ def save_obj_info(img_data, objs_name):
     return objs_name
 
 
-def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
+# def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
+def get_obstacle_point(sence, db, scene, cur_obstacle_world_points, map_ratio):
     cur_obstacle_pixel_points = []
     object_pixels = {}
     obj_detect_count = 0
@@ -380,10 +381,11 @@ def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
     objs_id[251] = "walker"
     # plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None)
     # plt.show()
-    plt.subplot(2, 2, 1)
+    # plt.subplot(2, 2, 1)
     plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
     plt.axis("off")
-    plt.title("相机分割")
+    # plt.title("相机分割")
+    sence.send_img("img_label_seg")
     # plt.show()
 
     d_depth = np.transpose(d_depth, (1, 0, 2))
@@ -416,10 +418,11 @@ def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
         world_point = transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene)
         cur_obstacle_world_points.append([world_point[0], world_point[1]])
         # print(f"{pixel}：{[world_point[0], world_point[1]]}")
-    plt.subplot(2, 2, 2)
+    # plt.subplot(2, 2, 2)
     plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None)
     plt.axis('off')
-    plt.title("目标检测")
+    # plt.title("目标检测")
+    # sence.send_img("img_label_obj")
     # plt.tight_layout()
 
     for key, value in object_pixels.items():
@@ -472,10 +475,9 @@ def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
         # height = point2[0] - point1[0]
         # rect = patches.Rectangle((0, 255), 15, 30, linewidth=1, edgecolor='g',
         #                          facecolor='none')
-
+    sence.send_img("img_label_obj")
-    plt.subplot(2, 7, 14)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
+    # plt.subplot(2, 7, 14)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
-
+    # plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
-    plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
 
     # plt.show()
     return cur_obstacle_world_points, cur_objs_id

robowaiter/robot/robot.py

@@ -96,6 +96,8 @@ class Robot(object):
         # print_tree_from_root(self.bt.root)
         print("行为树扩展完成！")
 
+        self.scene.draw_current_bt()
+
     # 获取所有action的pre,add,del列表
     def collect_action_nodes(self):
         action_list = []

robowaiter/scene/__init__.py

@@ -1,11 +1,11 @@
 
 # from .scene import Scene
-# from robowaiter.scene.tasks.AEM import SceneAEM
+# from robowaiter.scene.tasks_no_ui.AEM import SceneAEM
-# from robowaiter.scene.tasks.GQA import SceneGQA
+# from robowaiter.scene.tasks_no_ui.GQA import SceneGQA
-# from robowaiter.scene.tasks.VLN import SceneVLN
+# from robowaiter.scene.tasks_no_ui.VLN import SceneVLN
-# from robowaiter.scene.tasks.VLM import SceneVLM
+# from robowaiter.scene.tasks_no_ui.VLM import SceneVLM
-# from robowaiter.scene.tasks.Open_tasks import SceneOT
+# from robowaiter.scene.tasks_no_ui.Open_tasks import SceneOT
-# from robowaiter.scene.tasks.Auto_tasks import SceneAT
+# from robowaiter.scene.tasks_no_ui.Auto_tasks import SceneAT
 
 # task_map = {
 #     "AEM": SceneAEM,

robowaiter/scene/scene.py

@@ -1,3 +1,4 @@
+import io
 import pickle
 import sys
 import time
@@ -18,6 +19,9 @@ from robowaiter.utils import get_root_path
 from sklearn.cluster import DBSCAN
 from matplotlib import pyplot as plt
 from robowaiter.algos.navigator.dstar_lite import euclidean_distance
+from PIL import Image
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
+
 plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签
 plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号
 
@@ -30,22 +34,10 @@ channel = grpc.insecure_channel(
         ("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=11):
-    stub.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=mapID))
-    time.sleep(3)  # wait for the map to load
-
-
-def get_camera(part, scene_id=0):
-    # print('------------------get_camera----------------------')
-    action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
-    return stub.Capture(action)
-
 
 def show_image(camera_data):
     print('------------------show_image----------------------')
@@ -56,7 +48,7 @@ def show_image(camera_data):
     # matplotlib中的plt方法 对矩阵d 进行图形绘制，如果 深度相机拍摄的带深度的图片（图片名字中有depth信息），则转换成黑白图即灰度图
     plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None)
     # 图像展示在屏幕上
-    plt.show()
+    # plt.show()
 
     return d
 
@@ -68,6 +60,9 @@ class Scene:
     signal_event_list = []
     # show_bubble = True
     event_signal = "None"
+    # step_interval = 1
+    # camera_interval = 1.5
+    output_path = os.path.join(os.path.dirname(__file__), "outputs")
 
     default_state = {
         "map": {
@@ -78,9 +73,12 @@ class Scene:
         "sub_goal_list": [],  # 子目标列表
         "status": None,  # 仿真器中的观测信息，见下方详细解释
         "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)',
-                          'Holding(Nothing)', 'Exist(Yogurt)', 'Exist(BottledDrink)', 'On(Yogurt,Bar)',
+                          'Holding(Nothing)', 'Exist(Yogurt)', 'Exist(BottledDrink)',
-                          'On(BottledDrink,Bar)',
+                          'Exist(Softdrink)',
+                          # 'On(Yogurt,Bar)','On(BottledDrink,Bar)',
                           # 'Exist(Softdrink)', 'On(Softdrink,Table1)',
+                          'Exist(Chips)', 'Exist(NFCJuice)', 'Exist(Bernachon)', 'Exist(ADMilk)', 'Exist(SpringWater)'
+                          
                           'Exist(VacuumCup)', 'On(VacuumCup,Table2)',
                           'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
                           'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'},
@@ -108,35 +106,48 @@ class Scene:
     """
 
     def __init__(self, robot=None, sceneID=0):
+        self.stub = GrabSim_pb2_grpc.GrabSimStub(channel)
+
+
+        self.robot = robot
+
         self.sceneID = sceneID
         self.use_offset = False
         self.start_time = time.time()
         self.time = 0
         self.sub_task_seq = None
+        os.makedirs(self.output_path,exist_ok=True)
 
         self.show_bubble = True
+        # 是否展示UI
+        self.show_ui = False
         # 图像分割
-        self.take_picture = True
+        self.take_picture = False
         self.map_ratio = 5
         self.map_map = np.zeros((math.ceil(950 / self.map_ratio), math.ceil(1850 / self.map_ratio)))
         self.db = DBSCAN(eps=self.map_ratio, min_samples=int(self.map_ratio / 2))
         self.infoCount = 0
 
-        self.is_nav_walk = True
+        self.is_nav_walk = False
 
         file_name = os.path.join(root_path,'robowaiter/algos/navigator/map_5.pkl')
         if os.path.exists(file_name):
             with open(file_name, 'rb') as file:
                 self.map_map_real = pickle.load(file)
 
-        # init robot
+
-        if robot:
-            robot.set_scene(self)
-            robot.load_BT()
-        self.robot = robot
 
         self.robot_changed = False
         self.last_event_time = 0
+        self.last_camera_time = -99999
+        self.last_step_time = -99999
+
+        self.img_cache = {
+            "img_label_canvas":None,
+            "img_label_seg":None,
+            "img_label_obj":None,
+            "img_label_map":None,
+        }
 
         # 1-7 正常执行, 8-10 控灯操作移动到6, 11-12窗帘操作不需要移动,
         self.op_dialog = ["", "制作咖啡", "倒水", "夹点心", "拖地", "擦桌子", "开筒灯", "搬椅子",  # 1-7
@@ -212,6 +223,23 @@ class Scene:
 
         self.init_algos()  # 初始化各种算法类
 
+
+    def init_world(self,scene_num=1, mapID=11):
+        self.stub.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=mapID))
+        time.sleep(3)  # wait for the map to load
+
+    def get_camera(self,part, scene_id=0):
+        # print('------------------get_camera----------------------')
+        action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
+        return self.stub.Capture(action)
+
+    def init_robot(self):
+        # init robot
+
+        if self.robot:
+            self.robot.set_scene(self)
+            self.robot.load_BT()
+
     def init_algos(self):
         '''
             初始化各种各种算法
@@ -229,18 +257,19 @@ class Scene:
     def reset(self):
         # 基类reset，默认执行仿真器初始化操作
         self.reset_sim()
+        self.init_robot()
+        self.init_algos()
 
         # reset state
         self.state = copy.deepcopy(self.default_state)
 
-        print("场景初始化完成")
         self._reset()
+        print("场景初始化完成")
 
         self.running = True
 
     def run(self):
         # 基类run
-
         self._run()
         # 运行并由robot打印每步信息
         while True:
@@ -250,11 +279,13 @@ class Scene:
         # 基类step，默认执行行为树tick操作
         self.time = time.time() - self.start_time
 
+        # if self.time - self.last_step_time > self.step_interval:
         self.deal_event()
         self.deal_new_event()
         self.deal_signal_event()
         self._step()
         self.robot_changed = self.robot.step()
+        self.last_step_time = self.time
 
     def deal_new_event(self):
         if len(self.new_event_list) > 0:
@@ -306,22 +337,20 @@ class Scene:
 
         return set_sub_task
 
-    def new_set_goal(self, goal):
+
-        g = eval("{'" + goal + "'}")
-        self.state['chat_list'].append(("Goal", g))
     def new_set_goal(self,goal):
         self.state['chat_list'].append(("Goal",goal))
 
 
     @property
     def status(self):
-        return stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
+        return self.stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
 
     def reset_sim(self):
         # reset world
-        stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
+        self.stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
-        init_world()
+        self.init_world()
-        stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID))
+        self.stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID))
 
     def _reset(self):
         # 场景自定义的reset
@@ -350,7 +379,7 @@ class Scene:
         action = GrabSim_pb2.Action(
             scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v
         )
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
 
         return scene
 
@@ -361,7 +390,7 @@ class Scene:
     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(
+        navigation_info = self.stub.Do(
             GrabSim_pb2.Action(
                 scene=self.sceneID,
                 action=GrabSim_pb2.Action.ActionType.WalkTo,
@@ -376,7 +405,7 @@ class Scene:
     def add_walker(self, id, x, y, yaw=0, v=0, scope=100):
         loc = [x, y, yaw, v, scope]
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc)
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
         # print(scene.info)
         walker_list = []
         if (str(scene.info).find('unreachable') > -1):
@@ -384,11 +413,15 @@ class Scene:
         else:
             walker_list.append(
                 GrabSim_pb2.WalkerList.Walker(id=id, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=loc[2])))
-        stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID))
+        self.stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID))
 
         w = self.status.walkers
         num_customer = len(w)
-        self.state["customer_mem"][w[-1].name] = num_customer - 1
+        name = w[-1].name
+        self.state["customer_mem"][name] = num_customer - 1
+
+        self.ui_func(("add_walker",name))
+
 
     def walker_index2mem(self, index):
         for mem, i in self.state["customer_mem"].items():
@@ -417,15 +450,15 @@ class Scene:
                 # 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))
+        self.stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID))
         self.state["customer_mem"] = {}
         w = self.status.walkers
         for i in range(len(w)):
             self.state["customer_mem"][w[i].name] = i
 
     def remove_walkers(self, IDs=[0]):
-        s = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
+        s = self.stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
-        scene = stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=IDs, scene=self.sceneID))
+        scene = self.stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=IDs, scene=self.sceneID))
         time.sleep(2)
         self.state["customer_mem"] = {}
         w = self.status.walkers
@@ -434,7 +467,7 @@ class Scene:
         return
 
     def clean_walkers(self):
-        scene = stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
+        scene = self.stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
         self.state["customer_mem"] = {}
         return scene
 
@@ -444,13 +477,13 @@ class Scene:
             walkerID = self.walker_index2mem(walkerID)
 
         pose = GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw)
-        scene = stub.ControlWalkers(
+        scene = self.stub.ControlWalkers(
             GrabSim_pb2.WalkerControls(
                 controls=[GrabSim_pb2.WalkerControls.WControl(id=walkerID, autowalk=autowalk, speed=speed, pose=pose)],
                 scene=self.sceneID)
         )
         return scene
-        # stub.ControlWalkers(
+        # self.stub.ControlWalkers(
         #     GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID)
         # )
 
@@ -473,7 +506,7 @@ class Scene:
                 self.walker_control_generator(walkerID=control[0], autowalk=control[1], speed=control[2], X=control[3],
                                               Y=control[4], Yaw=control[5]))
         # 收集没有对话的统一控制
-        scene = stub.ControlWalkers(
+        scene = self.stub.ControlWalkers(
             GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID)
         )
         return scene
@@ -488,7 +521,7 @@ class Scene:
             is_autowalk = is_autowalk
             pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180)
             controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=80, pose=pose))
-        scene = stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID))
+        scene = self.stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID))
         return scene
 
     def control_walker_ls(self, walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]]):
@@ -508,12 +541,12 @@ class Scene:
             elif len(walker) == 6:
                 self.control_walker(walker[0], walker[1], walker[2], walker[3], walker[4], walker[5])
         #     self.control_walker()
-        # scene = stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID))
+        # scene = self.stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID))
         # return scene
         return
 
     def control_joints(self, angles):
-        stub.Do(
+        self.stub.Do(
             GrabSim_pb2.Action(
                 scene=self.sceneID,
                 action=GrabSim_pb2.Action.ActionType.RotateJoints,
@@ -524,7 +557,7 @@ class Scene:
     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(
+        self.stub.AddObjects(
             GrabSim_pb2.ObjectList(
                 objects=[
                     GrabSim_pb2.ObjectList.Object(x=X, y=Y, yaw=Yaw, z=Z, type=type)
@@ -540,13 +573,13 @@ class Scene:
         else:
             for objectID in args:
                 remove_list.append(objectID)
-        stub.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID))
+        self.stub.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID))
 
     def clean_object(self):
-        stub.CleanObjects(GrabSim_pb2.SceneID(value=self.sceneID))
+        self.stub.CleanObjects(GrabSim_pb2.SceneID(value=self.sceneID))
 
     def grasp(self, handID, objectID):
-        stub.Do(
+        self.stub.Do(
             GrabSim_pb2.Action(
                 scene=self.sceneID,
                 action=GrabSim_pb2.Action.ActionType.Grasp,
@@ -555,7 +588,7 @@ class Scene:
         )
 
     def release(self, handID):
-        stub.Do(
+        self.stub.Do(
             GrabSim_pb2.Action(
                 scene=self.sceneID,
                 action=GrabSim_pb2.Action.ActionType.Release,
@@ -564,7 +597,7 @@ class Scene:
         )
 
     def get_camera_color(self, image_only=True):
-        camera_data = stub.Capture(
+        camera_data = self.stub.Capture(
             GrabSim_pb2.CameraList(
                 cameras=[GrabSim_pb2.CameraName.Head_Color], scene=self.sceneID
             )
@@ -575,7 +608,7 @@ class Scene:
             return camera_data
 
     def get_camera_depth(self, image_only=True):
-        camera_data = stub.Capture(
+        camera_data = self.stub.Capture(
             GrabSim_pb2.CameraList(
                 cameras=[GrabSim_pb2.CameraName.Head_Depth], scene=self.sceneID
             )
@@ -586,7 +619,7 @@ class Scene:
             return camera_data
 
     def get_camera_segment(self, show=True):
-        camera_data = stub.Capture(
+        camera_data = self.stub.Capture(
             GrabSim_pb2.CameraList(
                 cameras=[GrabSim_pb2.CameraName.Head_Segment], scene=self.sceneID
             )
@@ -597,7 +630,7 @@ class Scene:
         return camera_data
 
     def chat_bubble(self, message):
-        stub.ControlRobot(
+        self.stub.ControlRobot(
             GrabSim_pb2.ControlInfo(
                 scene=self.sceneID, type=0, action=1, content=message.strip()
             )
@@ -621,7 +654,7 @@ class Scene:
 
     # def control_robot_action(self, scene_id=0, type=0, action=0, message="你好"):
     #     print('------------------control_robot_action----------------------')
-    #     scene = stub.ControlRobot(
+    #     scene = self.stub.ControlRobot(
     #         GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message))
     #     if (str(scene.info).find("Action Success") > -1):
     #         print(scene.info)
@@ -632,19 +665,19 @@ class Scene:
 
     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(
+        scene = self.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(
+                self.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))
+        self.stub.ControlRobot(GrabSim_pb2.ControlInfo(scene=self.sceneID, type=0, action=0))
 
     # 手指移动到指定位置
     def ik_control_joints(self, handNum=2, x=30, y=40, z=80):
@@ -654,7 +687,7 @@ class Scene:
             GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=handNum, x=x, y=y, z=z, roll=0, pitch=0, yaw=0),
             # GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=1, x=0, y=0, z=0, roll=0, pitch=0, yaw=0),
         ]
-        temp = stub.GetIKControlInfos(
+        temp = self.stub.GetIKControlInfos(
             GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject))
 
     def move_to_obj(self, obj_id):
@@ -666,7 +699,7 @@ class Scene:
         #     value[i] = self.status.joints[i].angle
         # value[5] = 0
         # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
-        # scene = stub.Do(action)
+        # scene = self.stub.Do(action)
         # time.sleep(1.0)
 
         obj_info = scene.objects[obj_id]
@@ -683,7 +716,7 @@ class Scene:
             self.navigator.navigate(goal=(walk_v[0], walk_v[1]), animation=False)
         else:
             action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
-            scene = stub.Do(action)
+            scene = self.stub.Do(action)
         print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw])
 
     # 移动到进行操作任务的指定地点
@@ -696,7 +729,7 @@ class Scene:
         #     value[i] = self.status.joints[i].angle
         # value[5] = 0
         # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
-        # scene = stub.Do(action)
+        # scene = self.stub.Do(action)
         # time.sleep(1.0)
 
         cur_pos = [scene.location.X, scene.location.Y, scene.rotation.Yaw]
@@ -705,32 +738,38 @@ class Scene:
             return
         print('------------------moveTo_Area----------------------')
         if op_type < 8:  # 动画控制相关任务的移动目标
+            if self.show_ui:
+                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
             walk_v = self.op_v_list[op_type] + [scene.rotation.Yaw, 180, 0]
         if 8 <= op_type <= 10:  # 控灯相关任务的移动目标
+            if self.show_ui:
+                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
             walk_v = self.op_v_list[6] + [scene.rotation.Yaw, 180, 0]
         if op_type in [13, 14, 15]:  # 空调相关任务的移动目标
+            if self.show_ui:
+                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
             walk_v = [240, -140.0] + [0, 180, 0]
         if op_type == 16:  # 抓握物体，移动到物体周围的可达区域
             scene = self.status
             obj_info = scene.objects[obj_id]
             obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
             walk_v = [obj_x + 50, obj_y] + [180, 180, 0]
+            if obj_info.name == 'Plate':
+                walk_v = [obj_x + 51, obj_y] + [180, 180, 0]
             if 820 <= obj_y <= 1200 and 240 <= obj_x <= 500:  # 物品位于斜的抹布桌上 ([240,500],[820,1200])
                 walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0]
-                obj_x += 3
-                obj_y += 2.5
         if op_type == 17:  # 放置物体，移动到物体周围的可达区域
             walk_v = release_pos[:-1] + [180, 180, 0]
             if release_pos == [340.0, 900.0, 99.0]:
                 walk_v[2] = 130
         # 移动到目标位置
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
         print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw])
 
     # 相应的行动，由主办方封装
     def control_robot_action(self, type=0, action=0, message="你好"):
-        scene = stub.ControlRobot(
+        scene = self.stub.ControlRobot(
             GrabSim_pb2.ControlInfo(
                 scene=self.sceneID, type=type, action=action, content=message
             )
@@ -750,10 +789,10 @@ class Scene:
             value[i] = self.status.joints[i].angle
         value[5] = 30
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
         time.sleep(1.0)
 
-        if self.take_picture:
+        if self.show_ui:
             self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio,update_info_count=1)
 
 
@@ -772,21 +811,71 @@ class Scene:
         scene = self.status
         ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z]
         obj_list = [
-            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 40, z=95, roll=0, pitch=0, yaw=0,
+
-                                          type=5),
+            GrabSim_pb2.ObjectList.Object(x=190, y=40, z=87, roll=0, pitch=0, yaw=0,
-            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z=h, roll=0, pitch=0, yaw=0, type=9),
+                                          type=38), #矿泉水
+
+
+            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 75, z=95, roll=0, pitch=0, yaw=0,
+                                          type=48),  # 48是薯片
+            # GrabSim_pb2.ObjectList.Object(x=190, y=40, z=87, roll=0, pitch=0, yaw=0,
+            #                               type=48), #48是薯片
+            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 65, z=95, roll=0, pitch=0, yaw=0,
+                                          type=37), #37是NFC果汁
+            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 55, z=95, roll=0, pitch=0, yaw=0,
+                                          type=8), #8是贝尔纳松
+            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 45, z=95, roll=0, pitch=0, yaw=0,
+                                          type=6), #6是AD钙奶
+            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 35, z=95, roll=0, pitch=0, yaw=0,
+                                          type=9), #9是冰红(瓶)
+            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 25, z=95, roll=0, pitch=0, yaw=0,
+                                          type=5),  # 5是酸奶
+
+            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 30, z=95, roll=0, pitch=0, yaw=0,
+            #                               type=13),
+            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 40, z=95, roll=0, pitch=0, yaw=0,
+            #                               type=48),
             # GrabSim_pb2.ObjectList.Object(x=340, y=960, z=88, roll=0, pitch=0, yaw=90, type=7),
             # GrabSim_pb2.ObjectList.Object(x=340, y=960, z = 88, roll=0, pitch=0, yaw=90, type=9),
-            # GrabSim_pb2.ObjectList.Object(x=340, y=952, z=88, roll=0, pitch=0, yaw=90, type=4),
+
+            GrabSim_pb2.ObjectList.Object(x=320, y=400, z=95, roll=0, pitch=0, yaw=0,
+                                          type=20),
+
+            # 斜桌三瓶冰红茶
+            GrabSim_pb2.ObjectList.Object(x=340, y=965, z=88, roll=0, pitch=0, yaw=90, type=4),
+            GrabSim_pb2.ObjectList.Object(x=320, y=940, z=88, roll=0, pitch=0, yaw=90, type=4),
+            GrabSim_pb2.ObjectList.Object(x=300, y=930, z=88, roll=0, pitch=0, yaw=90, type=4),
+            # GrabSim_pb2.ObjectList.Object(x=300, y=930, z=88, roll=0, pitch=0, yaw=90, type=38), #矿泉水
+
+            GrabSim_pb2.ObjectList.Object(x=370, y=1000, z=88, roll=0, pitch=0, yaw=90, type=1), #香蕉
+            GrabSim_pb2.ObjectList.Object(x=380, y=1000, z=88, roll=0, pitch=0, yaw=90, type=65),  # 番茄
+            GrabSim_pb2.ObjectList.Object(x=380, y=1020, z=88, roll=0, pitch=0, yaw=90, type=42),  # 山竹
+            GrabSim_pb2.ObjectList.Object(x=360, y=1020, z=88, roll=0, pitch=0, yaw=90, type=27),  # 橙子
+
+            # BrightTable2
+            # GrabSim_pb2.ObjectList.Object(x=-30, y=1000, z=35, roll=0, pitch=0, yaw=90, type=64), #西瓜
+            GrabSim_pb2.ObjectList.Object(x=-15, y=1050, z=40, roll=0, pitch=0, yaw=90, type=17), #a午餐盒
+
+            # 保温杯
             GrabSim_pb2.ObjectList.Object(x=-102, y=10, z=90, roll=0, pitch=0, yaw=90, type=7),
-            GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 70, z=95, roll=0, pitch=0, yaw=0,
+            # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 70, z=95, roll=0, pitch=0, yaw=0,
-                                          type=9),
+            #                               type=9),
+
+            # Table3上由两套军旗，一个模仿
             GrabSim_pb2.ObjectList.Object(x=-115, y=200, z=85, roll=0, pitch=0, yaw=90, type=26), # Chess
+            GrabSim_pb2.ObjectList.Object(x=-130, y=225, z=85, roll=0, pitch=0, yaw=90, type=55), # 玩具狗
+            GrabSim_pb2.ObjectList.Object(x=-110, y=225, z=85, roll=0, pitch=0, yaw=90, type=56), #玩具熊
             GrabSim_pb2.ObjectList.Object(x=-115, y=250, z=85, roll=0, pitch=0, yaw=90, type=26),  # Chess
-            GrabSim_pb2.ObjectList.Object(x=-115, y=280, z=85, roll=0, pitch=0, yaw=90, type=35),  # Chess
+            GrabSim_pb2.ObjectList.Object(x=-115, y=280, z=85, roll=0, pitch=0, yaw=90, type=35),  # 魔方
+
+            # 靠窗边的桌子上
+            GrabSim_pb2.ObjectList.Object(x=-400, y=520, z=70, roll=0, pitch=0, yaw=0, type=63),  # 小说
+            GrabSim_pb2.ObjectList.Object(x=-410, y=550, z=70, roll=0, pitch=0, yaw=0, type=59),  # 围巾
+            GrabSim_pb2.ObjectList.Object(x=-395, y=570, z=70, roll=0, pitch=0, yaw=0, type=18), # 手镯
+
 
         ]
-        scene = stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID))
+        scene = self.stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID))
         time.sleep(1.0)
 
     # 实现抓握操作
@@ -800,19 +889,22 @@ class Scene:
             value[i] = self.status.joints[i].angle
         value[5] = 30
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
         time.sleep(1.0)
 
-        if self.take_picture:
+        if self.show_ui:
             self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
 
         obj_info = scene.objects[obj_id]
         obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
+        if 820 <= obj_y <= 1200 and 240 <= obj_x <= 500: # 物品位于斜的抹布桌上 ([240,500],[820,1200])
+            obj_x += 3
+            obj_y += 2.5
         if obj_info.name == "CoffeeCup":
-            # obj_x += 1
+            # obj_x += 2.5
-            # obj_y -= 1
+            # obj_y -= 0.7  # 1.7
-            # values = [0,0,0,0,0, 10,-25,-45,-45,-45]
+            # obj_z -= 6
-            # values= [-6, 0, 0, 0, 0, -6, 0, 45, 45, 45]
+            # values= [0, 0, 0, 0, 0, 15, -6, -6, -6, -6]  # 后5位右手 [-6,45]
             # stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
             pass
         if obj_info.name == "Glass":
@@ -824,7 +916,7 @@ class Scene:
         print('------------------grasp_obj----------------------')
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp,
                                     values=[hand_id, obj_id])
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
         time.sleep(3.0)
         return True
 
@@ -832,12 +924,12 @@ class Scene:
     def robo_recover(self):
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints,  # 恢复原位
                                     values=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
 
     # 恢复手指关节
     def standard_finger(self):
         values = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
-        stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
+        self.stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
         time.sleep(1.0)
 
     # 弯腰以及手掌与放置面平齐
@@ -850,7 +942,7 @@ class Scene:
         angle[20] = -30
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints,  # 弯腰
                                     values=angle)
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
         time.sleep(1.0)
 
     # 实现放置操作
@@ -862,10 +954,10 @@ class Scene:
             value[i] = self.status.joints[i].angle
         value[5] = 30
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
         time.sleep(1.0)
 
-        if self.take_picture:
+        if self.show_ui:
             self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
 
         if release_pos == [340.0, 900.0, 99.0]:
@@ -877,7 +969,7 @@ class Scene:
             self.robo_stoop_parallel()
         print("------------------release_obj----------------------")
         action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Release, values=[1])
-        scene = stub.Do(action)
+        scene = self.stub.Do(action)
         time.sleep(2.0)
         self.robo_recover()  # 恢复肢体关节
         self.standard_finger()  # 恢复手指关节
@@ -885,10 +977,15 @@ class Scene:
 
     # 执行过程: Robot输出"开始(任务名)" -> 按步骤数执行任务 -> Robot输出成功或失败的对话
     def op_task_execute(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]):
+        #id = 196  # Glass = 188+x, Plate = 150+x
         self.control_robot_action(0, 1, "开始" + self.op_dialog[op_type])  # 输出正在执行的任务
         if op_type < 8:
+            if self.show_ui:
+                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
             result = self.control_robot_action(op_type, 1)
         if 8 <= op_type <= 12:
+            if self.show_ui:
+                self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
             result = self.control_robot_action(self.op_typeToAct[op_type][0], self.op_typeToAct[op_type][1])
         if op_type in [13, 14, 15]:  # 调整空调:13代表按开关,14升温,15降温
             result = self.adjust_kongtiao(op_type)
@@ -918,12 +1015,12 @@ class Scene:
             walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100]
             print("walk_v", walk_v)
             action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
-            scene = stub.Do(action)
+            scene = self.stub.Do(action)
             print(scene.info)
 
     def navigation_move(self, plt, cur_objs, cur_obstacle_world_points, v_list, map_ratio, db, scene_id=0, map_id=11):
         print('------------------navigation_move----------------------')
-        scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id))
+        scene = self.stub.Observe(GrabSim_pb2.SceneID(value=scene_id))
         walk_value = [scene.location.X, scene.location.Y]
         print("position:", walk_value)
 
@@ -934,12 +1031,17 @@ class Scene:
             walk_v = walk_value + [yaw, 250, 10]
             print("walk_v", walk_v)
             action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
-            scene = stub.Do(action)
+            scene = self.stub.Do(action)
             # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
             #                                                   objs_name_set)
 
-            cur_obstacle_world_points, cur_objs_id = camera.get_obstacle_point(plt, db, scene,
+            # cur_obstacle_world_points, cur_objs_id = camera.get_obstacle_point(plt, db, scene,
+            #                                                                    cur_obstacle_world_points, map_ratio)
+            cur_obstacle_world_points, cur_objs_id = camera.get_obstacle_point(self, db, scene,
                                                                                cur_obstacle_world_points, map_ratio)
+            # cur_obstacle_world_points, cur_objs_id = self.get_obstacle_point(db, scene, map_ratio)
+            # # self.get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio)
+
 
             # if scene.info == "Unreachable":
             print(scene.info)
@@ -956,7 +1058,7 @@ class Scene:
                 walk_v = walk_v + [yaw, 250, 10]
                 print("walk_v", walk_v)
                 action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
-                scene = stub.Do(action)
+                scene = self.stub.Do(action)
 
                 # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
                 #                                                   objs_name_set)
@@ -984,7 +1086,7 @@ class Scene:
         return x, y
 
     def explore(self, map, explore_range):
-        scene = stub.Observe(GrabSim_pb2.SceneID(value=0))
+        scene = self.stub.Observe(GrabSim_pb2.SceneID(value=0))
         cur_pos = [int(scene.location.X), int(scene.location.Y)]
         for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1):
             for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1):
@@ -1152,6 +1254,12 @@ class Scene:
         # print("物体世界偏移的坐标: ", world_offest_coordinates)
         return world_coordinates
 
+    def ui_func(self,args):
+        plt.show()
+
+    def draw_current_bt(self):
+        pass
+
     def get_obstacle_point(self, db, scene, map_ratio, update_info_count=0):
 
         # if abs(self.last_take_pic_tim - self.time)<
@@ -1161,15 +1269,14 @@ class Scene:
         cur_obstacle_world_points = []
         obj_detect_count = 0
         walker_detect_count = 0
-        fig = plt.figure()
         object_pixels = {}
 
         not_key_objs_id = {255, 254, 253, 107, 81}
 
-        img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
-        img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
-        img_data_color = get_camera([GrabSim_pb2.CameraName.Head_Color])
 
+        img_data_segment,img_data_depth,img_data_color = self.get_cameras()
+        if len(img_data_segment.images) <1:
+            return
         im_segment = img_data_segment.images[0]
         im_depth = img_data_depth.images[0]
         im_color = img_data_color.images[0]
@@ -1181,6 +1288,7 @@ class Scene:
         d_color = np.frombuffer(im_color.data, dtype=im_color.dtype).reshape(
             (im_color.height, im_color.width, im_color.channels))
 
+
         items = img_data_segment.info.split(";")
         objs_id = {}
         for item in items:
@@ -1189,10 +1297,7 @@ class Scene:
         objs_id[251] = "walker"
         # plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None)
         # plt.show()
-        plt.subplot(2, 2, 1)
+        img_segment = d_segment
-        plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
-        plt.axis("off")
-        plt.title("相机分割")
 
 
         d_depth = np.transpose(d_depth, (1, 0, 2))
@@ -1252,10 +1357,24 @@ class Scene:
         #     world_point = self.transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene)
         #     cur_obstacle_world_points.append([world_point[0], world_point[1]])
         # print(f"{pixel}：{[world_point[0], world_point[1]]}")
-        plt.subplot(2, 2, 2)
+        img_obj = d_color
-        plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None)
+
+        # self.ui_func(("draw_img","img_label_obj",d_color))
+
+        # 画分隔图
+        # plt.subplot(2, 2, 1)
+        plt.figure()
+        plt.imshow(img_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
+        plt.axis("off")
+        # plt.title("相机分割")
+        self.send_img("img_label_seg")
+
+        # 画目标检测图
+        # plt.subplot(2, 2, 2)
+        plt.figure()
+        plt.imshow(img_obj, cmap="gray" if "depth" in im_depth.name.lower() else None)
         plt.axis('off')
-        plt.title("目标检测")
+        # plt.title("目标检测")
 
         for key, value in object_pixels.items():
             if key == 0:
@@ -1315,20 +1434,57 @@ class Scene:
                          ha='center',
                          va='center')
                 plt.gca().add_patch(rect)
+
+        self.send_img("img_label_obj")
+
+
         new_map = self.updateMap(cur_obstacle_world_points)
         self.draw_map(plt,new_map)
 
-        plt.subplot(2, 7, 14)
         plt.axis("off")
-        plt.text(0, 0.9, f'检测行人数量：{walker_detect_count}', fontsize=10)
+        self.send_img("img_label_map")
-        plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
+
-        plt.text(0, 0.5, f'新增语义信息：{walker_detect_count}', fontsize=10)
+
-        plt.text(0, 0.3, f'更新语义信息：{update_info_count}', fontsize=10)
+
+        # plt.subplot(2, 7, 14)
+        # plt.text(0, 0.9, f'检测行人数量：{walker_detect_count}', fontsize=10)
+        # plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
+        # plt.text(0, 0.5, f'新增语义信息：{walker_detect_count}', fontsize=10)
+        # plt.text(0, 0.3, f'更新语义信息：{update_info_count}', fontsize=10)
         # plt.text(0, 0.1, f'已存语义信息：{self.infoCount}', fontsize=10)
 
-        plt.show()
+
+        # draw figures
+
+        # output_path = os.path.join(self.output_path,"vision.png")
+
+
+
+
+        # # 转换为numpy数组
+        # image_np = np.asarray(image_pil)
+        # self.ui_func(("draw_from_file","img_label_canvas",output_path))
+        # plt.close()
+        # plt.show()
         # return cur_obstacle_world_points
 
+    def send_img(self,name):
+        # 将图像保存到内存
+        buffer = io.BytesIO()
+        plt.savefig(buffer, bbox_inches='tight', pad_inches=0,format='png')
+        image_data = buffer.getvalue()
+
+        # 关闭当前图像
+        plt.close()
+
+        if not self.img_cache[name] == image_data:
+            self.img_cache[name] = image_data
+            self.ui_func(("draw_img",name,image_data))
+
+
+
+
+
     def updateMap(self, points):
         # map = copy.deepcopy(self.map_map)
         map = copy.deepcopy(self.map_map_real)
@@ -1350,19 +1506,21 @@ class Scene:
         return map
 
     def draw_map(self,plt, map):
-        plt.subplot(2, 1, 2)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
+        # plt.subplot(2, 1, 2)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
         plt.imshow(map, cmap='binary', alpha=0.5, origin='lower',
                    extent=(-400 / self.map_ratio, 1450 / self.map_ratio,
                            -350 / self.map_ratio, 600 / self.map_ratio))
-        plt.title('可达性地图')
+        # plt.title('可达性地图')
 
     def get_id_object_world(self, id, scene):
         pixels = []
         world_points = []
-        img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
+
+        img_data_segment,img_data_depth,_ = self.get_cameras()
+        # img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
         im_segment = img_data_segment.images[0]
 
-        img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
+        # img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
         im_depth = img_data_depth.images[0]
 
         d_segment = np.frombuffer(im_segment.data, dtype=im_segment.dtype).reshape(
@@ -1380,3 +1538,14 @@ class Scene:
         for pixel in pixels:
             world_points.append(self.transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0]))
         return world_points
+
+    def get_cameras(self):
+        # if self.time - self.last_camera_time > self.camera_interval:
+        self.img_data_segment = self.get_camera([GrabSim_pb2.CameraName.Head_Segment])
+        time.sleep(0.2)
+        self.img_data_depth = self.get_camera([GrabSim_pb2.CameraName.Head_Depth])
+        time.sleep(0.2)
+        self.img_data_color = self.get_camera([GrabSim_pb2.CameraName.Head_Color])
+        # self.last_camera_time = self.time
+
+        return self.img_data_segment,self.img_data_depth,self.img_data_color

robowaiter/scene/tasks/CafeDailyOperations/CafeOneDay.py

@@ -1,93 +0,0 @@
-"""
-视觉语言操作
-机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
-"""
-
-import time
-from robowaiter.scene.scene import Scene
-
-
-class SceneVLM(Scene):
-
-    def __init__(self, robot):
-        super().__init__(robot)
-        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
-
-        self.scene_flag = 1
-        self.st1 = 3
-        # self.st2 = self.st1 + 30
-        # self.st3 = self.st2 + 65
-        self.st2 = 3
-        self.st3 = 3
-        self.st4 = 3
-
-        self.signal_event_list = [
-
-            # 场景1：带小女孩找阳光下的空位
-            (3, self.add_walker, (5, 230, 1200)),  # 0号"Girl02_C_3"
-            (1, self.control_walker, (0, False, 200, 60, 520, 0)),
-            (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
-            (-1, self.customer_say, (0, "可以带我过去嘛？")),
-            (0, self.control_walker, (0, False, 50, 140, 1200, 180)),  # 小女孩站在了 BrightTable1 旁边就餐啦
-
-            # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
-            # (3, self.add_walker, (5, 230, 1200)), # 小女孩
-            # # # 上述准备
-            (10, self.add_walker, (26, -28, -150, 90)),
-            (0, self.add_walker, (10, -70, -200, -45)),
-            (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
-            (8, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])),
-            # 20 胖胖男到了 BrightTable6
-            (2, self.customer_say, (1, "咖啡有哪些呢？")),  # 10
-            (2, self.customer_say, (1, "来杯卡布奇诺吧。")),  # 15
-
-
-
-        ]
-
-    def _reset(self):
-        self.gen_obj()
-        # self.add_walkers([[47, 920]])
-        pass
-
-    def _run(self, op_type=10):
-        # 一个行人从门口走到 吧台
-        # 打招呼需要什么
-        # 行人说 哪里有位置，想晒个太阳
-        # 带领行人去有太阳的地方
-        # 行人说 有点热
-        # 好的，这就去开空调
-        self.walker_followed = False
-        pass
-
-    def _step(self):
-
-        if self.scene_flag == 1:
-            # 如果机器人不在 吧台
-            if self.walker_followed:
-                return
-            end = [self.status.location.X, self.status.location.Y]
-            if end[1] >= 600 or end[1] <= 450 or end[0] >= 250:
-                # if int(self.status.location.X)!=247 or  int(self.status.location.X)!=520:
-                self.walker_followed = True
-                self.control_walkers_and_say([[0, False, 150, end[0], end[1], 90, "谢谢！"]])
-                self.scene_flag += 1
-
-        # 获得所有顾客的名字
-        # print("=================")
-        # for cus in self.status.walkers:
-        #     print(cus)
-        # print("=================")
-        pass
-
-
-if __name__ == '__main__':
-    import os
-    from robowaiter.robot.robot import Robot
-
-    robot = Robot()
-
-    # create task
-    task = SceneVLM(robot)
-    task.reset()
-    task.run()

robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py

@@ -1,150 +0,0 @@
-"""
-视觉语言操作
-机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
-"""
-
-import time
-from robowaiter.scene.scene import Scene
-
-
-
-class SceneVLM(Scene):
-
-    def __init__(self, robot):
-        super().__init__(robot)
-        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
-
-        self.scene_flag = 1
-        self.st1 = 3
-        # self.st2 = self.st1 + 30
-        # self.st3 = self.st2 + 65
-        self.st2 = 3
-        self.st3 = 3
-        self.st4 = 3
-
-        self.signal_event_list = [
-
-            # 场景1：带小女孩找阳光下的空位
-            (3, self.add_walker, (5, 230, 1200)), # 0号"Girl02_C_3"
-            (1, self.control_walker, (0, False, 200, 60, 520, 0)),
-            (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
-            (-1, self.customer_say, (0,"可以带我过去嘛？")),
-            (0, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦
-
-
-            # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
-            # (3, self.add_walker, (5, 230, 1200)), # 小女孩
-            # # # 上述准备
-            (10, self.add_walker, (26, -28, -150, 90)),
-            (0, self.add_walker, (10, -70, -200, -45)),
-            (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
-            (8, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), # 20 胖胖男到了 BrightTable6
-            (2, self.customer_say, (1, "咖啡有哪些呢？")),# 10
-            (2, self.customer_say, (1,"来杯卡布奇诺吧。")), # 15
-
-            # # 场景3：有位女士要杯水和冰红茶
-            (30 ,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])),
-            (0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走
-            (0, self.add_walkers, ([[[48, 60, 520, 0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])),
-
-            (5, self.customer_say, (6, "哎呦，今天这么多人，还有空位吗？")),# 女士问
-            (15, self.customer_say, (6, "我带着孩子呢，想要宽敞亮堂的地方。")),  # 女士问
-            # 好的，我明白了，那么您可以选择我们的家庭亲子座，这样可以容纳您的孩子，并且更加宽敞舒适。
-            # 这里可以加一下自主导航和探索，找到一个位置
-            # 好的，我明白了，那么我们推荐您到大厅的桌子，那里的空间比较宽敞，环境也比较明亮，适合带着孩子一起用餐。
-            (8, self.customer_say, (6, "大厅的桌子好啊，快带我去呀！")),
-            (15, self.control_walker, (6, False, 50,-250, 480, 0)), #  #290, 400
-            (3, self.customer_say, (6, "我想来杯水，帮我孩子拿个酸奶吧。")),
-            # ### 9号灰色男 排队排着排着，不排了
-            (0, self.control_walker, (9, False, 100, 100, 760, 180)),
-            (0, self.control_walker, (9, True, 100, 0, 0, 180)),
-            # # ### 增加场景，孩子说热要开空调 或者9号随机游走和说
-            (90, self.customer_say, (6, "谢谢！不用了。")), #倒水+取放酸奶 90s
-
-
-
-            # # # 场景4：三人排队点单，女士要保温杯
-            (6, self.control_walkers_and_say, ([[[7, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
-            (5, self.customer_say, (7,"你可以帮我拿来吗，我在前门的桌子前等你。")),
-            (1, self.control_walker,(7, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步
-            (1, self.control_walker, (7, False, 80, 240, 1000, -45)), # 红女士走到Table1前
-            (1, self.control_walker, (8, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
-            (2, self.control_walker, (9, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
-            (6, self.customer_say, (7,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
-            (5, self.customer_say, (7, "不用了。")),  # 红女士在Table1前
-
-
-
-            # # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯
-            # # 场景6：大胖男点了外卖，离开啦
-            (9, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), #原来写了26s
-            (0, self.animation_reset,()), #重置任务
-            (6, self.customer_say, (8, "打包吧，快点！")),
-            (2, self.control_walker, (9, False, 100, 60, 620, -90)),  # 男灰黑色排队往前走一步
-            (100, self.customer_say, (8, "谢啦，我赶时间！")), #100这个时间很合适
-            (2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦
-            (2, self.control_walker, (8, False, 250, 240, -150, -90)),
-            (5, self.remove_walkers, ([[0,7,8]])),
-
-
-            # 场景7：最后排队那个随机游走 9号变为6号，随机游走。
-            # 机器人自主发现任务，走一圈去擦桌子/拖地，碰到灰色男问好，灰色男说“太阳大，要关窗帘和空调调低”
-            # 开了空调
-            (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
-            (2, self.customer_say, (6, "好热呀！太阳也好大！")),
-            (10, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
-
-            # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
-            (3, self.clean_walkers, ()),
-            (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
-            (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
-            (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "不用了。"]]])),
-
-        ]
-
-    def _reset(self):
-        self.gen_obj()
-        # self.add_walkers([[47, 920]])
-        pass
-
-    def _run(self, op_type=10):
-        # 一个行人从门口走到 吧台
-        # 打招呼需要什么
-        # 行人说 哪里有位置，想晒个太阳
-        # 带领行人去有太阳的地方
-        # 行人说 有点热
-        # 好的，这就去开空调
-        self.walker_followed = False
-        pass
-    
-    def _step(self):
-
-
-        if self.scene_flag == 1:
-            # 如果机器人不在 吧台
-            if self.walker_followed:
-                return
-            end = [self.status.location.X, self.status.location.Y]
-            if end[1]>=600 or end[1]<=450 or end[0]>=250:
-            # if int(self.status.location.X)!=247 or  int(self.status.location.X)!=520:
-                self.walker_followed = True
-                self.control_walkers_and_say([[0,False,150,end[0],end[1],90,"谢谢！"]])
-                self.scene_flag += 1
-
-        # 获得所有顾客的名字
-        # print("=================")
-        # for cus in self.status.walkers:
-        #     print(cus)
-        # print("=================")
-        pass
-
-if __name__ == '__main__':
-    import os
-    from robowaiter.robot.robot import Robot
-
-    robot = Robot()
-
-    # create task
-    task = SceneVLM(robot)
-    task.reset()
-    task.run()

robowaiter/scene/tasks/VLN.py

@@ -1,106 +0,0 @@
-"""
-视觉语言导航
-识别顾客（NPC）靠近、打招呼、对话、领位导航到适合人数的空闲餐桌
-开始条件：监测到顾客靠近
-结束条件：完成领位，语音：“请问您想喝点什么？”，并等待下一步指令
-"""
-import os
-import pickle
-import time
-import random
-
-import matplotlib.pyplot as plt
-import numpy as np
-
-from robowaiter.scene.scene import Scene,init_world  # TODO: 文件名改成Scene.py
-
-from robowaiter.scene.scene import Scene
-from robowaiter.utils import get_root_path
-from robowaiter.algos.navigator.navigate import Navigator
-from robowaiter.algos.navigator import test
-
-class SceneVLN(Scene):
-    def __init__(self, robot):
-        super().__init__(robot)
-        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
-        self.event_list = [
-            (5, self.create_chat_event("测试VLN：前往2号桌")),
-        ]
-
-    def _reset(self):
-        root_path = get_root_path()
-        file_name = os.path.join(root_path,'robowaiter/algos/navigator/map_5.pkl')
-        with open(file_name, 'rb') as file:
-            map = pickle.load(file)
-
-        self.state['map']['2d'] = map
-        self.state['map']['obj_pos']['Table'] = np.array((-100, 700))
-
-    def _run(self):
-        file_name = '../../algos/navigator/map_5.pkl'
-        if os.path.exists(file_name):
-            with open(file_name, 'rb') as file:
-                map = pickle.load(file)
-
-        # self.init_world(1, 11)
-        # scene = self.status
-
-        navigator = Navigator(scene=self, area_range=[-350, 600, -400, 1450], map=map)
-
-        '''场景1: 无行人环境 robot到达指定目标'''
-        # goal = np.array((-100, 700))
-
-        '''场景2: 静止行人环境 robot到达指定目标'''
-        # scene.clean_walker()
-        # scene.add_walker(50, 300, 0)
-        # scene.add_walker(-50, 500, 0)
-        # goal = np.array((-100, 700))
-
-        '''场景3: 移动行人环境 robot到达指定目标'''
-        self.walk_to(100, 0, -90, dis_limit=10)
-        self.clean_walker()
-        self.add_walkers([[50, 300], [-50, 500], [0, 700]])
-        # scene.add_walker(50, 300, 0)
-        # scene.add_walker(-50, 500, 0)
-        # scene.add_walker(0, 700, 0)
-        self.control_walker(
-            [self.walker_control_generator(walkerID=0, autowalk=False, speed=50, X=-50, Y=600, Yaw=0)])
-        self.control_walker(
-            [self.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)])
-        self.control_walker([self.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)])
-
-        # goal = (-100, 700)
-        # goal = (-300)
-        # goal = (340.0, 900.0)
-
-        # goal = (240.0, 1000.0)
-        # goal = (340.0, 900.0)
-        goal = (240.0, 1160.0)
-
-        '''场景4: 行人自由移动 robot到达指定目标'''
-        # # TODO: autowalk=True仿真器会闪退 ???
-        # scene.clean_walker()
-        # scene.add_walker(50, 300, 0)
-        # scene.add_walker(-50, 500, 0)
-        # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=True, speed=20, X=0, Y=0, Yaw=0)])
-        # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=True, speed=20, X=0, Y=0, Yaw=0)])
-        # time.sleep(5)
-        # goal = np.array((-100, 700))
-
-        navigator.navigate(goal, animation=False)
-
-        self.clean_walker()
-        print(self.status.collision)  # TODO: 不显示碰撞信息 ???
-        pass
-
-
-if __name__ == '__main__':
-    import os
-    from robowaiter.robot.robot import Robot
-
-    robot = Robot()
-
-    # create task
-    task = SceneVLN(robot)
-    task.reset()
-    task.run()

robowaiter/scene/ui/pyqt5.py

@@ -0,0 +1,262 @@
+import importlib
+import os
+
+from PyQt5.QtWidgets import QApplication, QMainWindow, QListWidgetItem
+from PyQt5.QtCore import QTimer
+import sys
+
+from robowaiter.scene.ui.window import Ui_MainWindow
+from robowaiter.utils.basic import get_root_path
+from PyQt5.QtCore import QThread
+import queue
+import numpy as np
+from PyQt5.QtGui import QImage, QPixmap
+from PyQt5.QtCore import Qt
+from PyQt5.QtCore import QThread, pyqtSignal
+from robowaiter.scene.ui.scene_ui import SceneUI
+from  robowaiter.scene.ui import scene_ui
+
+root_path = get_root_path()
+
+class TaskThread(QThread):
+    stop_signal = pyqtSignal()
+
+    def __init__(self, task, scene_cls,robot_cls,scene_queue,ui_queue, *args, **kwargs):
+        super(TaskThread, self).__init__(*args, **kwargs)
+        self.task = task
+        self.scene_cls = scene_cls
+        self.robot_cls = robot_cls
+        self.scene_queue = scene_queue
+        self.ui_queue = ui_queue
+        self.stoped = True
+
+    def run(self):
+        self.scene = self.task(self.scene_cls,self.robot_cls,self.scene_queue,self.ui_queue)
+        self.scene._run()
+        # scene._run()
+
+        while not self.isInterruptionRequested():
+            self.scene.step()
+
+    # def stop(self):
+    #     del self.scene
+
+def run_scene(scene_cls,robot_cls,scene_queue,ui_queue):
+    scene = scene_cls(robot_cls(),scene_queue,ui_queue)
+
+    scene.reset()
+    # scene.run()
+    return scene
+    # try:
+    #     # 机器人系统的主循环
+    #
+    # except Exception as e:
+    #     # 打印异常信息到命令行
+    #     print("Robot system error:", str(e))
+
+example_list = ("AEM","VLN","VLM",'GQA',"OT","AT","reset")
+
+class UI(QMainWindow, Ui_MainWindow):
+    scene = None
+    history_dict = {
+        "System": ""
+    }
+
+    def __init__(self,robot_cls):
+        app = QApplication(sys.argv)
+        MainWindow = QMainWindow()
+        super(UI, self).__init__(MainWindow)
+        # 创建线程安全的队列
+        self.scene_queue = queue.Queue()
+        self.ui_queue = queue.Queue()
+
+        self.scene_cls = SceneUI
+        self.robot_cls = robot_cls
+
+        self.setupUi(MainWindow)  # 初始化UI
+
+        # 绑定说话按钮
+        self.btn_say.clicked.connect(self.btn_say_on_click)
+        # 绑定任务演示按钮
+        for example in example_list:
+            btn = getattr(self,f"btn_{example}")
+            btn.clicked.connect(self.create_example_click(example))
+
+        # 设置一个定时器，每隔100ms检查一次队列
+        timer = QTimer()
+        timer.setInterval(100)  # 设置检查队列的间隔时间
+        timer.timeout.connect(self.handle_queue_messages)  # 当定时器超时时，处理队列中的消息
+        timer.start()  # 启动定时器
+
+        MainWindow.show()
+
+        self.thread = TaskThread(run_scene,self.scene_cls,self.robot_cls,self.scene_queue,self.ui_queue)
+        self.thread.start()
+        self.list_customer.itemClicked.connect(self.show_customer_history)
+
+        sys.exit(app.exec_())
+
+    def new_history(self,customer_name,chat):
+        role = chat["role"]
+        content = chat["content"].strip()
+        function_call = chat.get("function_call",None)
+        if function_call:
+            return
+        if role == "function":
+            new_chat = f'Robot:\n 工具调用-{chat["name"]}\n'
+        elif role == "user":
+            new_chat = f'{customer_name}:\n {content}\n'
+        else:
+            new_chat = f'Robot:\n {content}\n'
+
+        self.edit_global_history.append(f'{new_chat}')
+        self.history_dict[customer_name] += new_chat + "\n"
+        items = self.list_customer.findItems(customer_name, Qt.MatchExactly)
+        if items:
+            index = self.list_customer.indexFromItem(items[0])
+            self.list_customer.setCurrentIndex(index)
+        self.edit_local_history.clear()
+        self.edit_local_history.append(self.history_dict[customer_name])
+
+    def reset(self):
+        self.history_dict = {
+            "System": ""
+        }
+        self.edit_local_history.clear()
+        self.edit_global_history.clear()
+        self.list_customer.clear()
+        item = QListWidgetItem("System")
+        self.list_customer.addItem(item)
+
+
+
+
+    def create_example_click(self,name):
+        def btn_example_on_click():
+            self.reset()
+
+            self.thread.requestInterruption()
+            self.thread.wait()  # 等待线程安全退出
+
+            self.scene_queue = queue.Queue()
+            self.ui_queue = queue.Queue()
+            self.thread = TaskThread(run_scene, self.scene_cls, self.robot_cls,self.scene_queue,self.ui_queue)
+            self.thread.start()
+
+            self.scene_func((f"run_{name}",))
+        return btn_example_on_click
+
+    def add_walker(self,name):
+        self.history_dict[name] = ""
+        item = QListWidgetItem(name)
+        self.list_customer.addItem(item)
+
+
+    def show_customer_history(self, item):
+        # 此处为示例，实际上你可能需要从数据库或其他地方获取聊天记录
+        name = item.text()
+        self.edit_local_history.clear()
+        self.edit_local_history.append(self.history_dict[name])
+
+    def btn_say_on_click(self):
+        question = self.edit_say.text()
+        if question[-1] == ")":
+            print(f"设置目标:{question}")
+            self.scene_func(("new_set_goal",question))
+            # self.scene.new_set_goal(question)
+        else:
+            self.scene_func(("customer_say","System",question))
+            # self.scene.customer_say("System", question)
+
+    def scene_func(self,args):
+        self.scene_queue.put(args)
+
+    def handle_queue_messages(self):
+        while not self.ui_queue.empty():
+            message = self.ui_queue.get()
+            function_name = message[0]
+            function = getattr(self, function_name, None)
+
+            args = []
+            if len(message)>1:
+                args = message[1:]
+
+            result = function(*args)
+
+    def setupUi(self, MainWindow):
+        Ui_MainWindow.setupUi(self,MainWindow)
+
+        # dir_path = os.path.dirname(__file__)
+
+        # 加载并显示图片
+        # img_path = os.path.join(root_path, "robowaiter/utils/draw_bt/test.png")
+        # self.draw_from_file("img_label_bt",img_path)
+        # self.label.setAlignment(Qt.AlignCenter)  # 图片居中显示
+
+    def draw_from_file(self,control_name,file_name):
+        control = getattr(self,control_name,None)
+        # 加载并显示图片
+        pixmap = QPixmap(file_name) # 替换为你的图片路径
+        # self.label.setPixmap(pixmap)
+        control.setPixmap(self.scale_pixmap_to_label(pixmap, control))
+
+    def draw_img(self,control_name,img):
+        control = getattr(self,control_name,None)
+        # # 加载并显示图片
+        # print(img)
+        # img = self.ndarray_to_qimage(img)
+        # print(img)
+
+        # image = Image.open(io.BytesIO(img))
+        # print(image)
+        pixmap = QPixmap.fromImage(QImage.fromData(img))
+
+        # self.label.setPixmap(pixmap)
+        control.setPixmap(self.scale_pixmap_to_label(pixmap, control))
+        # control.setPixmap(pixmap)
+
+    def draw_canvas(self,control_name,canvas):
+        control = getattr(self,control_name,None)
+        # 加载并显示图片
+
+        pixmap = QApplication.grabWidget(canvas)
+
+        # self.label.setPixmap(pixmap)
+        control.setPixmap(self.scale_pixmap_to_label(pixmap, control))
+
+
+    def ndarray_to_qimage(self,array: np.ndarray) -> QImage:
+        # 假设你的ndarray是uint8类型的，且是RGB格式（三个通道）
+        height, width, channels = array.shape
+        if channels ==3:
+            bytes_per_line = channels * width
+            image = QImage(array.data, width, height, bytes_per_line, QImage.Format_RGB888)
+        else:
+            # 对于灰度图像
+            image = QImage(array.data, width, height, QImage.Format_Grayscale8)
+
+        return image
+
+    def scale_pixmap_to_label(self, pixmap, label):
+        # 获取QLabel的大小
+        label_width = label.width()
+        label_height = label.height()
+
+        # 保持图片的长宽比，同时确保图片的一个维度适应QLabel的大小
+        scaled_pixmap = pixmap.scaledToWidth(label_width, Qt.SmoothTransformation)
+        if scaled_pixmap.height() > label_height:
+            scaled_pixmap = pixmap.scaledToHeight(label_height, Qt.SmoothTransformation)
+
+        return scaled_pixmap
+
+    def set_scene(self, scene):
+        self.scene = scene
+
+
+if __name__ == "__main__":
+    # app = QApplication(sys.argv)
+    # MainWindow = QMainWindow()
+    ui = UI()
+    # ui.setupUi(MainWindow)
+    # MainWindow.show()
+    # sys.exit(app.exec_())

robowaiter/scene/ui/scene_ui.py

@@ -0,0 +1,274 @@
+"""
+UI场景
+"""
+import sys
+import json
+import math
+from matplotlib import pyplot as plt
+from sklearn.cluster import DBSCAN
+import pickle
+import time
+import os
+plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签
+plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号
+
+from robowaiter.utils import get_root_path
+root_path = get_root_path()
+
+from robowaiter.scene.scene import Scene
+from robowaiter.utils.bt.draw import render_dot_tree
+
+
+class SceneUI(Scene):
+    scene_queue = None
+    ui_queue = None
+    # camera_interval = 4
+    def __init__(self, robot,scene_queue,ui_queue):
+        self.scene_queue = scene_queue
+        self.ui_queue = ui_queue
+
+        super().__init__(robot)
+        # 在这里加入场景中发生的事件
+        self.show_ui = True
+
+        # while True:
+        #     if not self.scene_queue.empty():
+        #         param = self.scene_queue.get()
+        #         # 处理参数...
+
+        # self.ui_queue.put(('say',"test"))
+        self.stoped = False
+
+    def run(self):
+        # 基类run
+        self._run()
+        # 运行并由robot打印每步信息
+        while not self.stoped:
+            self.step()
+
+    def _run(self):
+        pass
+
+    def run_AEM(self):
+        print(len(self.status.objects))
+        # 创建一个从白色（1）到灰色（0）的 colormap
+        objs = self.status.objects
+        cur_objs = []
+        cur_obstacle_world_points = []
+        visited_obstacle = set()
+        obj_json_data = []
+        obj_count = 0
+        added_info = 0
+        map_ratio = self.map_ratio
+        db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2))
+        file_name = os.path.join(root_path, 'robowaiter/proto/map_1.pkl')
+        if os.path.exists(file_name):
+            with open(file_name, 'rb') as file:
+                map = pickle.load(file)
+        print('------------ 自主探索 ------------')
+        while True:
+            walker_count = 0
+            fig = plt.figure()
+            goal = self.explore(map, 120)
+            if goal is None:
+                break
+            # cur_obstacle_world_points, cur_objs_id = self.navigation_move(plt, cur_objs, cur_obstacle_world_points,
+            #                                                               [[goal[0], goal[1]]], map_ratio, db, 0, 11)
+            cur_obstacle_world_points, cur_objs_id = self.navigation_move(self, cur_objs, cur_obstacle_world_points,
+                                                                          [[goal[0], goal[1]]], map_ratio, db, 0, 11)
+            for point in cur_obstacle_world_points:
+                if point[0] < -350 or point[0] > 600 or point[1] < -400 or point[1] > 1450:
+                    continue
+                self.map_map[math.floor((point[0] + 350) / map_ratio), math.floor((point[1] + 400) / map_ratio)] = 1
+                visited_obstacle.add(
+                    (math.floor((point[0] + 350) / map_ratio), math.floor((point[1] + 400) / map_ratio)))
+            for i in range(len(cur_objs_id)):
+                if cur_objs_id[i] == "walker":
+                    walker_count += 1
+                for obj in objs:
+                    if obj.name == cur_objs_id[i] and obj not in cur_objs:
+                        cur_objs.append(obj)
+                        break
+            # plt.subplot(2, 1, 2)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
+            # plt.imshow(self.map_map, cmap='binary', alpha=0.5, origin='lower',
+            #            extent=(-400 / map_ratio, 1450 / map_ratio,
+            #                    -350 / map_ratio, 600 / map_ratio))
+            new_map = self.updateMap(cur_obstacle_world_points)
+            self.draw_map(plt, new_map)
+            plt.axis("off")
+            self.send_img("img_label_map")
+            # plt.title("地图构建过程")
+            # self.send_img("img_label_map")
+            # plt.subplot(2, 7, 14)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
+            new_add_info = len(cur_objs) - added_info + walker_count
+            # plt.text(0, 0.5, f'新增语义信息：{new_add_info}', fontsize=10)  # 在图中添加文字，x和y坐标是在这个图片大小内的相对位置，fontsize是字体大小
+            added_info += new_add_info
+            # plt.text(0, 0.3, f'已存语义信息：{added_info}', fontsize=10)  # 在图中添加文字，x和y坐标是在这个图片大小内的相对位置，fontsize是字体大小
+            self.infoCount = added_info
+            plt.axis("off")
+            # plt.show()
+            print("------------当前检测到的物品信息--------------")
+            print(cur_objs)
+            time.sleep(1)
+
+        for i in range(len(cur_objs)):
+            if cur_objs[i].name == "Desk" or cur_objs[i].name == "Chair":
+                obj_json_data.append(
+                    {"id": f"{i}", "name": f"{cur_objs[i].name}", "location": f"{cur_objs[i].location}",
+                     "height": f"{cur_objs[i].location.Z * 2}"})
+
+            else:
+                obj_json_data.append(
+                    {"id": f"{i}", "name": f"{cur_objs[i].name}", "location": f"{cur_objs[i].location}",
+                     "height": f"{cur_objs[i].location.Z}"})
+
+        with open('../../robowaiter/proto/objs.json', 'w') as file:
+            json.dump(obj_json_data, file)
+
+        print("已绘制完成地图！！！")
+        print("------------检测到的所有物品信息--------------")
+        print(obj_json_data)
+
+    def run_VLN(self):
+        self.gen_obj()
+        self.add_walkers([
+            [29, 60, 520],  # 顾客 0
+            [23, 0, 220],  # 秃头老头子  1
+            [0, -55, 150],  # 小男孩d走来走去 2
+            [10, -55, 750],  # 3
+            [19, 70, -200],  # 后门站着不动的 4
+            [21, 65, 1000, -90],  # 大胖男占了一号桌 5
+            [5, 230, 1200],  # 小女孩 6
+            [26, -28, -10, 90],
+            # [26, 60, 0, 90],
+            # [26, -28, 0, 90] , #在设置一个在后门随机游走的 7
+            # 设置为 26, 60, 0, 90]
+            [31, 280, 1200, -45]  # 8
+        ])
+        self.control_walker(2, True, 200, -55, 155, 90)  # 飞速奔跑的小男孩
+        # self.control_walker(7, True, 80, -25, -150, 90)
+        self.control_walker(5, True, 65, 995, 520, 90)
+        self.control_walker(4, True, 65, 70, -200, 90)
+
+        self.new_event_list = [
+            (5, self.customer_say, (0, "请问哪里有空位啊？")),
+            (13, self.customer_say, (0, "我想坐高凳子。")),
+            (3, self.customer_say, (0, "你带我去吧。")),
+            (45, self.control_walker, (0, False, 100, -250, 480, -90)),
+            (-1, self.customer_say, (0, "谢谢你！这儿还不错！")),
+        ]
+
+    def run_VLM(self):
+        self.gen_obj()
+        self.add_walkers([[4,1, 880], [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
+        self. control_walkers(walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True)
+        self.signal_event_list = [
+            (3, self.add_walker,  (20,0,700)),
+            (1, self.control_walker, (6, False,100, 60, 520,0)),
+            (1, self.customer_say, (6, "给我来份薯片和果汁，我坐在对面的桌子那儿。")),
+            (5, self.control_walker, (6, False, 100, -250, 480, 0)),
+        ]
+        pass
+
+    def run_GQA(self):
+        self.gen_obj()
+        self.add_walkers([ [16,250, 1200],[6,-55, 750],[10,70, -200],[47,-290, 400, 180],[26, 60,-320,90]])
+        self.control_walker(1, True, 100, 60, 720, 0)
+        self.control_walker(4, True, 100, 60, -120, 0)
+        self.add_walkers([[31, 60,500,0], [15,60,550,0]])
+        self.signal_event_list = [
+            (5, self.customer_say, (6, "你好呀，你们这有啥好吃的？")), # 男
+            (8, self.customer_say, (6, "听起来都好甜呀，我女朋友爱吃水果。")),
+            (15, self.customer_say, (6, "你们这人可真多。")),
+            (15, self.customer_say, (6, "我女朋友怕晒，有空余的阴凉位置嘛？")),
+            (20, self.customer_say, (6, "那还不错。")),
+            (15, self.customer_say, (5, "请问洗手间在哪呢？")),
+            (20, self.customer_say, (5, "我们还想一起下下棋,切磋切磋。")),
+            (20, self.customer_say, (6, "太棒啦，亲爱的。")),
+            (15, self.customer_say, (5, "那你知道附近最近的电影院在哪吗?")),
+            (20, self.customer_say, (6, "谢啦，那我们先去阴凉位置下个棋，等电影开始了就去看呢!")),
+        ]
+        pass
+
+    def run_OT(self):
+        self.gen_obj()
+        self.add_walkers([ [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
+        self.control_walker(1, True, 100, 60, 720, 0)
+        self.control_walker(4, True, 100, 60, -120, 0)
+        self.add_walkers([[16,60, 520], [47,-40, 520]])
+        self.signal_event_list = [
+            (8, self.customer_say, (5, "给我来杯咖啡，哦对，再倒一杯水。")),
+            (1, self.control_walker_ls,([[[5, False, 100, -250, 480, 0],[6, False, 100, 60, 520, 0]]])),
+            (-1, self.customer_say, (5, "感谢，这些够啦，你去忙吧。")),
+            (10, self.customer_say, (6, "我想来份点心和酸奶。")),
+            (-1, self.customer_say, (6, "真美味啊！")),
+        ]
+        pass
+
+    def run_AT(self):
+        self.add_walker(23, 60, 520, 0)
+        self.signal_event_list = [
+            (2, self.customer_say, (0,"可以关筒灯和关窗帘吗？")),
+        ]
+        pass
+
+    def run_reset(self):
+        pass
+
+    def init_robot(self):
+        # init robot
+
+        if self.robot:
+            self.robot.set_scene(self)
+            self.robot.load_BT()
+            self.draw_current_bt()
+
+    def draw_current_bt(self):
+        render_dot_tree(self.robot.bt.root,target_directory=self.output_path,name="current_bt")
+        self.ui_queue.put(('draw_from_file',"img_label_bt", f"{self.output_path}/current_bt.png"))
+
+    def ui_func(self,args):
+        # _,_,output_path = args
+        # plt.savefig(output_path)
+
+        self.ui_queue.put(args)
+
+    def _reset(self):
+        pass
+
+    def _step(self):
+        # print("已运行")
+        self.handle_queue_messages()
+        # if len(self.sub_task_seq.children) == 0:
+        #     question = input("请输入指令：")
+        #     if question[-1] == ")":
+        #         print(f"设置目标:{question}")
+        #         self.new_set_goal(question)
+        #     else:
+        #         self.customer_say("System",question)
+
+
+    def handle_queue_messages(self):
+        while not self.scene_queue.empty():
+            message = self.scene_queue.get()
+            function_name = message[0]
+            function = getattr(self, function_name, None)
+
+            args = []
+            if len(message)>1:
+                args = message[1:]
+
+            result = function(*args)
+
+    def stop(self):
+        self.stoped = True
+
+if __name__ == '__main__':
+    from robowaiter.robot.robot import Robot
+
+    robot = Robot()
+    ui = UI( Robot)
+
+    # create task
+    # task = SceneUI(robot,ui)
+

robowaiter/scene/ui/ui.py

@@ -0,0 +1,110 @@
+import tkinter as tk
+from robowaiter.utils.basic import get_root_path
+from tkinter import PhotoImage
+import os
+root_path = get_root_path()
+
+class Robot:
+    def __init__(self):
+        self.is_running = False
+
+    def start(self):
+        if not self.is_running:
+            self.is_running = True
+            # 在这里添加启动机器人的逻辑代码
+            print("机器人正在启动...")
+
+    def stop(self):
+        if self.is_running:
+            self.is_running = False
+            # 在这里添加停止机器人的逻辑代码
+            print("机器人正在停止...")
+
+    def get_status(self):
+        if self.is_running:
+            return "运行中"
+        else:
+            return "已停止"
+
+
+class RobotInterface:
+    def __init__(self, master):
+        self.master = master
+        # self.robot = Robot()  # 假设这里有一个名为Robot的类，表示机器人系统
+
+        # 创建启动机器人按钮
+        self.start_button = tk.Button(master, text="启动机器人", command=self.start_robot)
+        self.start_button.pack()
+
+        # 创建停止机器人按钮
+        self.stop_button = tk.Button(master, text="停止机器人", command=self.stop_robot)
+        self.stop_button.pack()
+
+        # 创建显示机器人状态的标签
+        self.status_label = tk.Label(master, text="机器人状态：")
+        self.status_label.pack()
+
+        # 创建用于显示图片的三个标签
+        self.image_label1 = tk.Label(master)
+        self.image_label1.pack()
+        self.image_label2 = tk.Label(master)
+        self.image_label2.pack()
+        self.image_label3 = tk.Label(master)
+        self.image_label3.pack()
+
+        # dir_path = os.path.dirname(__file__)
+        # # 加载图片
+        # self.image1 = PhotoImage(file=os.path.join(dir_path,"1.png"))  # 替换为实际图片路径
+        # self.image2 = PhotoImage(file=os.path.join(dir_path,"2.png"))  # 替换为实际图片路径
+        # self.image3 = PhotoImage(file=os.path.join(dir_path,"3.png"))  # 替换为实际图片路径
+        #
+        # # 显示第一张图片
+        # self.show_image(1)
+
+    def display_binary_image(self,data):
+        # 创建一个Tkinter窗口
+
+        # 创建一个像素画布
+        canvas = tk.Canvas(self.master, width=len(data[0]), height=len(data), bg='white')
+        canvas.pack()
+
+        # 在画布上绘制像素点
+        for i in range(len(data)):
+            for j in range(len(data[0])):
+                if data[i][j] == 1:
+                    canvas.create_rectangle(j, i, j + 1, i + 1, fill='black')
+
+    def show_image(self, image_number):
+        if image_number == 1:
+            self.image_label1.config(image=self.image1)
+        elif image_number == 2:
+            self.image_label2.config(image=self.image2)
+        elif image_number == 3:
+            self.image_label3.config(image=self.image3)
+        else:
+            print("无效的图片编号")
+
+    def start_robot(self):
+        self.robot.start()  # 假设Robot类有一个start方法来启动机器人
+        self.update_status("机器人已启动。")
+
+    def stop_robot(self):
+        self.robot.stop()  # 假设Robot类有一个stop方法来停止机器人
+        self.update_status("机器人已停止。")
+
+    def update_status(self, status):
+        self.status_label.config(text="机器人状态： " + status)
+
+
+if __name__ == '__main__':
+
+    root = tk.Tk()
+    interface = RobotInterface(root)
+    binary_image = [
+        [0, 1, 0],
+        [1, 1, 1],
+        [0, 1, 0]
+    ]
+    interface.display_binary_image(binary_image)
+
+    root.mainloop()

robowaiter/scene/ui/ui2py.bat

@@ -0,0 +1 @@
+pyuic5 -x window.ui -o window.py

robowaiter/scene/ui/window.py

@@ -0,0 +1,230 @@
+# -*- coding: utf-8 -*-
+
+# Form implementation generated from reading ui file 'window.ui'
+#
+# Created by: PyQt5 UI code generator 5.15.10
+#
+# WARNING: Any manual changes made to this file will be lost when pyuic5 is
+# run again.  Do not edit this file unless you know what you are doing.
+
+
+from PyQt5 import QtCore, QtGui, QtWidgets
+
+
+class Ui_MainWindow(object):
+    def setupUi(self, MainWindow):
+        MainWindow.setObjectName("MainWindow")
+        MainWindow.resize(936, 1080)
+        MainWindow.setAutoFillBackground(False)
+        self.centralwidget = QtWidgets.QWidget(MainWindow)
+        self.centralwidget.setObjectName("centralwidget")
+        self.edit_say = QtWidgets.QLineEdit(self.centralwidget)
+        self.edit_say.setGeometry(QtCore.QRect(520, 390, 221, 31))
+        font = QtGui.QFont()
+        font.setFamily("Times New Roman")
+        font.setPointSize(14)
+        self.edit_say.setFont(font)
+        self.edit_say.setObjectName("edit_say")
+        self.btn_say = QtWidgets.QPushButton(self.centralwidget)
+        self.btn_say.setGeometry(QtCore.QRect(760, 390, 81, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.btn_say.setFont(font)
+        self.btn_say.setObjectName("btn_say")
+        self.img_label_seg = QtWidgets.QLabel(self.centralwidget)
+        self.img_label_seg.setGeometry(QtCore.QRect(80, 540, 321, 231))
+        self.img_label_seg.setStyleSheet("border: 2px solid black;")
+        self.img_label_seg.setText("")
+        self.img_label_seg.setObjectName("img_label_seg")
+        self.img_label_bt = QtWidgets.QLabel(self.centralwidget)
+        self.img_label_bt.setGeometry(QtCore.QRect(40, 830, 831, 161))
+        self.img_label_bt.setStyleSheet("border: 2px solid black;")
+        self.img_label_bt.setText("")
+        self.img_label_bt.setObjectName("img_label_bt")
+        self.label_5 = QtWidgets.QLabel(self.centralwidget)
+        self.label_5.setGeometry(QtCore.QRect(410, 800, 121, 21))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.label_5.setFont(font)
+        self.label_5.setObjectName("label_5")
+        self.img_label_obj = QtWidgets.QLabel(self.centralwidget)
+        self.img_label_obj.setGeometry(QtCore.QRect(520, 540, 321, 231))
+        self.img_label_obj.setStyleSheet("border: 2px solid black;")
+        self.img_label_obj.setText("")
+        self.img_label_obj.setObjectName("img_label_obj")
+        self.img_label_map = QtWidgets.QLabel(self.centralwidget)
+        self.img_label_map.setGeometry(QtCore.QRect(80, 320, 321, 161))
+        self.img_label_map.setStyleSheet("border: 2px solid black;")
+        self.img_label_map.setText("")
+        self.img_label_map.setObjectName("img_label_map")
+        self.label_6 = QtWidgets.QLabel(self.centralwidget)
+        self.label_6.setGeometry(QtCore.QRect(190, 290, 121, 21))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.label_6.setFont(font)
+        self.label_6.setObjectName("label_6")
+        self.label_7 = QtWidgets.QLabel(self.centralwidget)
+        self.label_7.setGeometry(QtCore.QRect(200, 500, 111, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.label_7.setFont(font)
+        self.label_7.setObjectName("label_7")
+        self.label_8 = QtWidgets.QLabel(self.centralwidget)
+        self.label_8.setGeometry(QtCore.QRect(630, 510, 121, 21))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.label_8.setFont(font)
+        self.label_8.setObjectName("label_8")
+        self.btn_AEM = QtWidgets.QPushButton(self.centralwidget)
+        self.btn_AEM.setGeometry(QtCore.QRect(30, 70, 151, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.btn_AEM.setFont(font)
+        self.btn_AEM.setObjectName("btn_AEM")
+        self.label_9 = QtWidgets.QLabel(self.centralwidget)
+        self.label_9.setGeometry(QtCore.QRect(10, 20, 371, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(10)
+        self.label_9.setFont(font)
+        self.label_9.setObjectName("label_9")
+        self.btn_VLN = QtWidgets.QPushButton(self.centralwidget)
+        self.btn_VLN.setGeometry(QtCore.QRect(30, 120, 151, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.btn_VLN.setFont(font)
+        self.btn_VLN.setObjectName("btn_VLN")
+        self.btn_VLM = QtWidgets.QPushButton(self.centralwidget)
+        self.btn_VLM.setGeometry(QtCore.QRect(30, 170, 151, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.btn_VLM.setFont(font)
+        self.btn_VLM.setObjectName("btn_VLM")
+        self.btn_GQA = QtWidgets.QPushButton(self.centralwidget)
+        self.btn_GQA.setGeometry(QtCore.QRect(200, 70, 141, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.btn_GQA.setFont(font)
+        self.btn_GQA.setObjectName("btn_GQA")
+        self.btn_OT = QtWidgets.QPushButton(self.centralwidget)
+        self.btn_OT.setGeometry(QtCore.QRect(200, 120, 141, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.btn_OT.setFont(font)
+        self.btn_OT.setObjectName("btn_OT")
+        self.btn_AT = QtWidgets.QPushButton(self.centralwidget)
+        self.btn_AT.setGeometry(QtCore.QRect(200, 170, 141, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.btn_AT.setFont(font)
+        self.btn_AT.setObjectName("btn_AT")
+        self.btn_reset = QtWidgets.QPushButton(self.centralwidget)
+        self.btn_reset.setGeometry(QtCore.QRect(30, 220, 311, 31))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.btn_reset.setFont(font)
+        self.btn_reset.setObjectName("btn_reset")
+        self.list_customer = QtWidgets.QListWidget(self.centralwidget)
+        self.list_customer.setGeometry(QtCore.QRect(380, 50, 151, 201))
+        font = QtGui.QFont()
+        font.setPointSize(11)
+        self.list_customer.setFont(font)
+        self.list_customer.setObjectName("list_customer")
+        item = QtWidgets.QListWidgetItem()
+        self.list_customer.addItem(item)
+        self.label_10 = QtWidgets.QLabel(self.centralwidget)
+        self.label_10.setGeometry(QtCore.QRect(390, 20, 121, 16))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.label_10.setFont(font)
+        self.label_10.setObjectName("label_10")
+        self.label_11 = QtWidgets.QLabel(self.centralwidget)
+        self.label_11.setGeometry(QtCore.QRect(560, 20, 141, 16))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.label_11.setFont(font)
+        self.label_11.setObjectName("label_11")
+        self.label_12 = QtWidgets.QLabel(self.centralwidget)
+        self.label_12.setGeometry(QtCore.QRect(740, 20, 131, 16))
+        font = QtGui.QFont()
+        font.setFamily("黑体")
+        font.setPointSize(14)
+        self.label_12.setFont(font)
+        self.label_12.setObjectName("label_12")
+        self.edit_local_history = QtWidgets.QTextEdit(self.centralwidget)
+        self.edit_local_history.setEnabled(True)
+        self.edit_local_history.setGeometry(QtCore.QRect(550, 50, 171, 261))
+        font = QtGui.QFont()
+        font.setPointSize(11)
+        self.edit_local_history.setFont(font)
+        self.edit_local_history.setReadOnly(True)
+        self.edit_local_history.setObjectName("edit_local_history")
+        self.edit_global_history = QtWidgets.QTextEdit(self.centralwidget)
+        self.edit_global_history.setEnabled(True)
+        self.edit_global_history.setGeometry(QtCore.QRect(740, 50, 161, 261))
+        font = QtGui.QFont()
+        font.setPointSize(11)
+        self.edit_global_history.setFont(font)
+        self.edit_global_history.setReadOnly(True)
+        self.edit_global_history.setObjectName("edit_global_history")
+        MainWindow.setCentralWidget(self.centralwidget)
+        self.menubar = QtWidgets.QMenuBar(MainWindow)
+        self.menubar.setGeometry(QtCore.QRect(0, 0, 936, 23))
+        self.menubar.setObjectName("menubar")
+        MainWindow.setMenuBar(self.menubar)
+        self.statusbar = QtWidgets.QStatusBar(MainWindow)
+        self.statusbar.setObjectName("statusbar")
+        MainWindow.setStatusBar(self.statusbar)
+
+        self.retranslateUi(MainWindow)
+        QtCore.QMetaObject.connectSlotsByName(MainWindow)
+
+    def retranslateUi(self, MainWindow):
+        _translate = QtCore.QCoreApplication.translate
+        MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow"))
+        self.edit_say.setText(_translate("MainWindow", "Is(AC,On)"))
+        self.btn_say.setText(_translate("MainWindow", "说话"))
+        self.label_5.setText(_translate("MainWindow", "当前行为树"))
+        self.label_6.setText(_translate("MainWindow", "可达性地图"))
+        self.label_7.setText(_translate("MainWindow", "实例分割"))
+        self.label_8.setText(_translate("MainWindow", "目标检测"))
+        self.btn_AEM.setText(_translate("MainWindow", "环境主动探索"))
+        self.label_9.setText(_translate("MainWindow", "任务演示：（播放动画时需等待动画播放完毕才会重置场景）"))
+        self.btn_VLN.setText(_translate("MainWindow", "视觉语言导航"))
+        self.btn_VLM.setText(_translate("MainWindow", "视觉语言操作"))
+        self.btn_GQA.setText(_translate("MainWindow", "具身多轮对话"))
+        self.btn_OT.setText(_translate("MainWindow", "开放具身任务"))
+        self.btn_AT.setText(_translate("MainWindow", "自主具身任务"))
+        self.btn_reset.setText(_translate("MainWindow", "重置"))
+        __sortingEnabled = self.list_customer.isSortingEnabled()
+        self.list_customer.setSortingEnabled(False)
+        item = self.list_customer.item(0)
+        item.setText(_translate("MainWindow", "System"))
+        self.list_customer.setSortingEnabled(__sortingEnabled)
+        self.label_10.setText(_translate("MainWindow", "顾客列表："))
+        self.label_11.setText(_translate("MainWindow", "顾客历史对话："))
+        self.label_12.setText(_translate("MainWindow", "全局历史对话："))
+
+
+if __name__ == "__main__":
+    import sys
+    app = QtWidgets.QApplication(sys.argv)
+    MainWindow = QtWidgets.QMainWindow()
+    ui = Ui_MainWindow()
+    ui.setupUi(MainWindow)
+    MainWindow.show()
+    sys.exit(app.exec_())

robowaiter/scene/ui/window.ui

@@ -0,0 +1,484 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>936</width>
+    <height>1080</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <property name="autoFillBackground">
+   <bool>false</bool>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <widget class="QLineEdit" name="edit_say">
+    <property name="geometry">
+     <rect>
+      <x>520</x>
+      <y>390</y>
+      <width>221</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>Times New Roman</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>Is(AC,On)</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="btn_say">
+    <property name="geometry">
+     <rect>
+      <x>760</x>
+      <y>390</y>
+      <width>81</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>说话</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="img_label_seg">
+    <property name="geometry">
+     <rect>
+      <x>80</x>
+      <y>540</y>
+      <width>321</width>
+      <height>231</height>
+     </rect>
+    </property>
+    <property name="styleSheet">
+     <string notr="true">border: 2px solid black;</string>
+    </property>
+    <property name="text">
+     <string/>
+    </property>
+   </widget>
+   <widget class="QLabel" name="img_label_bt">
+    <property name="geometry">
+     <rect>
+      <x>40</x>
+      <y>830</y>
+      <width>831</width>
+      <height>161</height>
+     </rect>
+    </property>
+    <property name="styleSheet">
+     <string notr="true">border: 2px solid black;</string>
+    </property>
+    <property name="text">
+     <string/>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_5">
+    <property name="geometry">
+     <rect>
+      <x>410</x>
+      <y>800</y>
+      <width>121</width>
+      <height>21</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>当前行为树</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="img_label_obj">
+    <property name="geometry">
+     <rect>
+      <x>520</x>
+      <y>540</y>
+      <width>321</width>
+      <height>231</height>
+     </rect>
+    </property>
+    <property name="styleSheet">
+     <string notr="true">border: 2px solid black;</string>
+    </property>
+    <property name="text">
+     <string/>
+    </property>
+   </widget>
+   <widget class="QLabel" name="img_label_map">
+    <property name="geometry">
+     <rect>
+      <x>80</x>
+      <y>320</y>
+      <width>321</width>
+      <height>161</height>
+     </rect>
+    </property>
+    <property name="styleSheet">
+     <string notr="true">border: 2px solid black;</string>
+    </property>
+    <property name="text">
+     <string/>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_6">
+    <property name="geometry">
+     <rect>
+      <x>190</x>
+      <y>290</y>
+      <width>121</width>
+      <height>21</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>可达性地图</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_7">
+    <property name="geometry">
+     <rect>
+      <x>200</x>
+      <y>500</y>
+      <width>111</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>实例分割</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_8">
+    <property name="geometry">
+     <rect>
+      <x>630</x>
+      <y>510</y>
+      <width>121</width>
+      <height>21</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>目标检测</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="btn_AEM">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>70</y>
+      <width>151</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>环境主动探索</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_9">
+    <property name="geometry">
+     <rect>
+      <x>10</x>
+      <y>20</y>
+      <width>371</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>10</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>任务演示：（播放动画时需等待动画播放完毕才会重置场景）</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="btn_VLN">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>120</y>
+      <width>151</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>视觉语言导航</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="btn_VLM">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>170</y>
+      <width>151</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>视觉语言操作</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="btn_GQA">
+    <property name="geometry">
+     <rect>
+      <x>200</x>
+      <y>70</y>
+      <width>141</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>具身多轮对话</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="btn_OT">
+    <property name="geometry">
+     <rect>
+      <x>200</x>
+      <y>120</y>
+      <width>141</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>开放具身任务</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="btn_AT">
+    <property name="geometry">
+     <rect>
+      <x>200</x>
+      <y>170</y>
+      <width>141</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>自主具身任务</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="btn_reset">
+    <property name="geometry">
+     <rect>
+      <x>30</x>
+      <y>220</y>
+      <width>311</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>重置</string>
+    </property>
+   </widget>
+   <widget class="QListWidget" name="list_customer">
+    <property name="geometry">
+     <rect>
+      <x>380</x>
+      <y>50</y>
+      <width>151</width>
+      <height>201</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>11</pointsize>
+     </font>
+    </property>
+    <item>
+     <property name="text">
+      <string>System</string>
+     </property>
+    </item>
+   </widget>
+   <widget class="QLabel" name="label_10">
+    <property name="geometry">
+     <rect>
+      <x>390</x>
+      <y>20</y>
+      <width>121</width>
+      <height>16</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>顾客列表：</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_11">
+    <property name="geometry">
+     <rect>
+      <x>560</x>
+      <y>20</y>
+      <width>141</width>
+      <height>16</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>顾客历史对话：</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_12">
+    <property name="geometry">
+     <rect>
+      <x>740</x>
+      <y>20</y>
+      <width>131</width>
+      <height>16</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <family>黑体</family>
+      <pointsize>14</pointsize>
+     </font>
+    </property>
+    <property name="text">
+     <string>全局历史对话：</string>
+    </property>
+   </widget>
+   <widget class="QTextEdit" name="edit_local_history">
+    <property name="enabled">
+     <bool>true</bool>
+    </property>
+    <property name="geometry">
+     <rect>
+      <x>550</x>
+      <y>50</y>
+      <width>171</width>
+      <height>261</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>11</pointsize>
+     </font>
+    </property>
+    <property name="readOnly">
+     <bool>true</bool>
+    </property>
+   </widget>
+   <widget class="QTextEdit" name="edit_global_history">
+    <property name="enabled">
+     <bool>true</bool>
+    </property>
+    <property name="geometry">
+     <rect>
+      <x>740</x>
+      <y>50</y>
+      <width>161</width>
+      <height>261</height>
+     </rect>
+    </property>
+    <property name="font">
+     <font>
+      <pointsize>11</pointsize>
+     </font>
+    </property>
+    <property name="readOnly">
+     <bool>true</bool>
+    </property>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menubar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>936</width>
+     <height>23</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QStatusBar" name="statusbar"/>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

robowaiter/utils/bt/draw.py

@@ -279,7 +279,8 @@ def render_dot_tree(root: behaviour.Behaviour,
                     name: str = None,
                     target_directory: str = os.getcwd(),
                     with_blackboard_variables: bool = False,
-                    with_qualified_names: bool = False):
+                    with_qualified_names: bool = False,
+                    png_only = True):
     """
     Render the dot tree to .dot, .svg, .png. files in the current
     working directory. These will be named with the root behaviour name.
@@ -322,10 +323,16 @@ def render_dot_tree(root: behaviour.Behaviour,
     filename_wo_extension_to_convert = root.ins_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():
+
+    if png_only:
+        write_dict = {"png": graph.write_png}
+    else:
+        write_dict = {"dot": graph.write, "png": graph.write_png, "svg": graph.write_svg}
+
+    for extension, writer in write_dict.items():
         filename = filename_wo_extension + '.' + extension
         pathname = os.path.join(target_directory, filename)
         print("Writing {}".format(pathname))
         writer(pathname)
         filenames[extension] = pathname
-    return filenames
+    return filenames["png"]

robowaiter/utils/draw_bt/Default.ptml

@@ -1,17 +1,10 @@
-selector
+sequence{
-    sequence
+cond Holding(Nothing)
-        cond CustomerChatting()
+sequence{
-        act DealChat()
+cond Is(Curtain,Off)
-    sequence
+act Turn(Curtain,On)
-        cond HasSubTask()
+}
-        sequence
+sequence{
-            act SubTaskPlaceHolder()
+cond Is(AC,Off)
-    sequence
+act Turn(AC,On)
-        cond FocusingCustomer()
+}}
-        act ServeCustomer()
-    sequence
-        cond NewCustomerComing()
-        selector
-            cond At(Robot,Bar)
-            act MoveTo(Bar)
-        act GreetCustomer()

robowaiter/utils/draw_bt/Default_bracket.ptml

@@ -2,27 +2,28 @@ selector
 {
     sequence
 {
-        cond CustomerChatting()
+        cond Chatting()
         act DealChat()
-
+    }    sequence
-}    sequence
 {
         cond HasSubTask()
         sequence
 {
             act SubTaskPlaceHolder()
-
+        }    }    sequence
-}    sequence
+{
         cond FocusingCustomer()
         act ServeCustomer()
-
+    }    sequence
-}    sequence
 {
-        cond NewCustomerComing()
+        cond NewCustomer()
         selector
 {
             cond At(Robot,Bar)
             act MoveTo(Bar)
-
+        }        act GreetCustomer()
-}        act GreetCustomer()
+    }    sequence
-}}
+{
+        cond AnomalyDetected()
+        act ResolveAnomaly()
+    }}

robowaiter/utils/draw_bt/test.dot

@@ -3,37 +3,19 @@ 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];
+"c9a8671e-b0b8-4587-a4de-a094fd678334" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=octagon, 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];
+"bc597ff6-b3cb-4573-9dea-944fdb38d045" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Holding(Nothing)", shape=ellipse, style=filled];
-"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "717a1290-56bc-41e1-9b4b-e6df93d40e64";
+"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "bc597ff6-b3cb-4573-9dea-944fdb38d045";
-"b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2" [fillcolor=yellow, fontcolor=black, fontsize=20, label="CustomerChatting()", shape=ellipse, style=filled];
+"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
-"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2";
+"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "a8e26121-f5cc-4ba5-b98e-86bd84de06d6";
-"28015225-b7a4-4f38-b26f-4dbe5eea4154" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="DealChat()", shape=box, style=filled];
+"676d8bda-2c30-44a5-bca2-ade68761403a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(Curtain,Off)", shape=ellipse, style=filled];
-"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "28015225-b7a4-4f38-b26f-4dbe5eea4154";
+"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "676d8bda-2c30-44a5-bca2-ade68761403a";
-"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
+"c3d096cf-f848-46a3-9497-5398614798e8" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(Curtain,On)", shape=box, style=filled];
-"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e";
+"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "c3d096cf-f848-46a3-9497-5398614798e8";
-"2f77f976-499c-4601-9d8c-86dd80d66dfa" [fillcolor=yellow, fontcolor=black, fontsize=20, label="HasSubTask()", shape=ellipse, style=filled];
+"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
-"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2f77f976-499c-4601-9d8c-86dd80d66dfa";
+"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "400b4104-ff96-4d64-89b2-49dc6e4e3cbd";
-"2428792c-3896-443d-8744-f5e286644fad" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01];
+"3fc48afc-1cef-45f5-826d-2571d7979a2f" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(AC,Off)", shape=ellipse, style=filled];
-"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2428792c-3896-443d-8744-f5e286644fad";
+"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "3fc48afc-1cef-45f5-826d-2571d7979a2f";
-"4bb7a9c3-521b-408f-b403-1fd25d54b192" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="SubTaskPlaceHolder()", shape=box, style=filled];
+"47af4df0-8811-45a5-95e2-f0fc9458ddfd" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(AC,On)", shape=box, style=filled];
-"2428792c-3896-443d-8744-f5e286644fad" -> "4bb7a9c3-521b-408f-b403-1fd25d54b192";
+"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "47af4df0-8811-45a5-95e2-f0fc9458ddfd";
-"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";
 }

robowaiter/utils/draw_bt/test.svg

@@ -4,208 +4,100 @@
 <!-- Generated by graphviz version 9.0.0 (20230911.1827)
  -->
 <!-- Title: pastafarianism Pages: 1 -->
-<svg width="1699pt" height="417pt"
+<svg width="716pt" height="220pt"
- viewBox="0.00 0.00 1699.49 416.87" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
+ viewBox="0.00 0.00 715.70 219.87" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
-<g id="graph0" class="graph" transform="scale(1 1) rotate(0) translate(4 412.87)">
+<g id="graph0" class="graph" transform="scale(1 1) rotate(0) translate(4 215.87)">
 <title>pastafarianism</title>
-<polygon fill="white" stroke="none" points="-4,4 -4,-412.87 1695.49,-412.87 1695.49,4 -4,4"/>
+<polygon fill="white" stroke="none" points="-4,4 -4,-215.87 711.7,-215.87 711.7,4 -4,4"/>
-<!-- a3ac082f&#45;ad46&#45;4cc9&#45;86b4&#45;3e76c3c564d3 -->
+<!-- c9a8671e&#45;b0b8&#45;4587&#45;a4de&#45;a094fd678334 -->
 <g id="node1" class="node">
-<title>a3ac082f&#45;ad46&#45;4cc9&#45;86b4&#45;3e76c3c564d3</title>
+<title>c9a8671e&#45;b0b8&#45;4587&#45;a4de&#45;a094fd678334</title>
-<polygon fill="cyan" stroke="black" points="495.87,-408.87 412.37,-377.62 495.87,-346.37 579.37,-377.62 495.87,-408.87"/>
+<polygon fill="orange" stroke="black" points="383.11,-178.04 383.11,-197.85 345.67,-211.87 292.73,-211.87 255.29,-197.85 255.29,-178.04 292.73,-164.03 345.67,-164.03 383.11,-178.04"/>
-<text text-anchor="middle" x="495.87" y="-370.24" font-family="Times New Roman,serif" font-size="20.00">Selector</text>
+<text text-anchor="middle" x="319.2" y="-180.57" font-family="Times New Roman,serif" font-size="20.00">Selector</text>
 </g>
-<!-- 717a1290&#45;56bc&#45;41e1&#45;9b4b&#45;e6df93d40e64 -->
+<!-- bc597ff6&#45;b3cb&#45;4573&#45;9dea&#45;944fdb38d045 -->
 <g id="node2" class="node">
-<title>717a1290&#45;56bc&#45;41e1&#45;9b4b&#45;e6df93d40e64</title>
+<title>bc597ff6&#45;b3cb&#45;4573&#45;9dea&#45;944fdb38d045</title>
-<polygon fill="orange" stroke="black" points="395.38,-276.54 395.38,-296.35 352.9,-310.37 292.83,-310.37 250.35,-296.35 250.35,-276.54 292.83,-262.53 352.9,-262.53 395.38,-276.54"/>
+<ellipse fill="yellow" stroke="black" cx="114.2" cy="-104.11" rx="114.2" ry="22.1"/>
-<text text-anchor="middle" x="322.87" y="-279.07" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
+<text text-anchor="middle" x="114.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Holding(Nothing)</text>
 </g>
-<!-- a3ac082f&#45;ad46&#45;4cc9&#45;86b4&#45;3e76c3c564d3&#45;&gt;717a1290&#45;56bc&#45;41e1&#45;9b4b&#45;e6df93d40e64 -->
+<!-- c9a8671e&#45;b0b8&#45;4587&#45;a4de&#45;a094fd678334&#45;&gt;bc597ff6&#45;b3cb&#45;4573&#45;9dea&#45;944fdb38d045 -->
 <g id="edge1" class="edge">
-<title>a3ac082f&#45;ad46&#45;4cc9&#45;86b4&#45;3e76c3c564d3&#45;&gt;717a1290&#45;56bc&#45;41e1&#45;9b4b&#45;e6df93d40e64</title>
+<title>c9a8671e&#45;b0b8&#45;4587&#45;a4de&#45;a094fd678334&#45;&gt;bc597ff6&#45;b3cb&#45;4573&#45;9dea&#45;944fdb38d045</title>
-<path fill="none" stroke="black" d="M461.27,-358.78C435.8,-345.66 400.81,-327.62 372.38,-312.97"/>
+<path fill="none" stroke="black" d="M276.22,-169.79C246.35,-157.87 206.15,-141.82 173.26,-128.69"/>
-<polygon fill="black" stroke="black" points="374.07,-309.9 363.58,-308.43 370.87,-316.13 374.07,-309.9"/>
+<polygon fill="black" stroke="black" points="174.86,-125.56 164.27,-125.1 172.26,-132.06 174.86,-125.56"/>
 </g>
-<!-- ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e -->
+<!-- a8e26121&#45;f5cc&#45;4ba5&#45;b98e&#45;86bd84de06d6 -->
-<g id="node5" class="node">
-<title>ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e</title>
-<polygon fill="orange" stroke="black" points="742.38,-276.54 742.38,-296.35 699.9,-310.37 639.83,-310.37 597.35,-296.35 597.35,-276.54 639.83,-262.53 699.9,-262.53 742.38,-276.54"/>
-<text text-anchor="middle" x="669.87" y="-279.07" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
-</g>
-<!-- a3ac082f&#45;ad46&#45;4cc9&#45;86b4&#45;3e76c3c564d3&#45;&gt;ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e -->
-<g id="edge4" class="edge">
-<title>a3ac082f&#45;ad46&#45;4cc9&#45;86b4&#45;3e76c3c564d3&#45;&gt;ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e</title>
-<path fill="none" stroke="black" d="M530.24,-359C556.02,-345.79 591.68,-327.51 620.51,-312.74"/>
-<polygon fill="black" stroke="black" points="621.74,-316.04 629.04,-308.37 618.55,-309.81 621.74,-316.04"/>
-</g>
-<!-- b3236b13&#45;d5f2&#45;4643&#45;ae7c&#45;6f27bd4ab6f2 -->
 <g id="node3" class="node">
-<title>b3236b13&#45;d5f2&#45;4643&#45;ae7c&#45;6f27bd4ab6f2</title>
+<title>a8e26121&#45;f5cc&#45;4ba5&#45;b98e&#45;86bd84de06d6</title>
-<ellipse fill="yellow" stroke="black" cx="125.87" cy="-202.61" rx="125.87" ry="22.1"/>
+<polygon fill="orange" stroke="black" points="391.72,-94.2 391.72,-114.02 349.24,-128.03 289.16,-128.03 246.68,-114.02 246.68,-94.2 289.16,-80.19 349.24,-80.19 391.72,-94.2"/>
-<text text-anchor="middle" x="125.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">CustomerChatting()</text>
+<text text-anchor="middle" x="319.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
 </g>
-<!-- 717a1290&#45;56bc&#45;41e1&#45;9b4b&#45;e6df93d40e64&#45;&gt;b3236b13&#45;d5f2&#45;4643&#45;ae7c&#45;6f27bd4ab6f2 -->
+<!-- c9a8671e&#45;b0b8&#45;4587&#45;a4de&#45;a094fd678334&#45;&gt;a8e26121&#45;f5cc&#45;4ba5&#45;b98e&#45;86bd84de06d6 -->
 <g id="edge2" class="edge">
-<title>717a1290&#45;56bc&#45;41e1&#45;9b4b&#45;e6df93d40e64&#45;&gt;b3236b13&#45;d5f2&#45;4643&#45;ae7c&#45;6f27bd4ab6f2</title>
+<title>c9a8671e&#45;b0b8&#45;4587&#45;a4de&#45;a094fd678334&#45;&gt;a8e26121&#45;f5cc&#45;4ba5&#45;b98e&#45;86bd84de06d6</title>
-<path fill="none" stroke="black" d="M278.16,-266.88C250.24,-255.28 213.99,-240.22 183.86,-227.7"/>
+<path fill="none" stroke="black" d="M319.2,-163.72C319.2,-156.29 319.2,-147.89 319.2,-139.83"/>
-<polygon fill="black" stroke="black" points="185.46,-224.58 174.88,-223.97 182.77,-231.04 185.46,-224.58"/>
+<polygon fill="black" stroke="black" points="322.7,-139.92 319.2,-129.92 315.7,-139.92 322.7,-139.92"/>
 </g>
-<!-- 28015225&#45;b7a4&#45;4f38&#45;b26f&#45;4dbe5eea4154 -->
+<!-- 400b4104&#45;ff96&#45;4d64&#45;89b2&#45;49dc6e4e3cbd -->
-<g id="node4" class="node">
-<title>28015225&#45;b7a4&#45;4f38&#45;b26f&#45;4dbe5eea4154</title>
-<polygon fill="lawngreen" stroke="black" points="376.24,-220.61 269.49,-220.61 269.49,-184.61 376.24,-184.61 376.24,-220.61"/>
-<text text-anchor="middle" x="322.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">DealChat()</text>
-</g>
-<!-- 717a1290&#45;56bc&#45;41e1&#45;9b4b&#45;e6df93d40e64&#45;&gt;28015225&#45;b7a4&#45;4f38&#45;b26f&#45;4dbe5eea4154 -->
-<g id="edge3" class="edge">
-<title>717a1290&#45;56bc&#45;41e1&#45;9b4b&#45;e6df93d40e64&#45;&gt;28015225&#45;b7a4&#45;4f38&#45;b26f&#45;4dbe5eea4154</title>
-<path fill="none" stroke="black" d="M322.87,-262.22C322.87,-252.88 322.87,-242.01 322.87,-232.2"/>
-<polygon fill="black" stroke="black" points="326.37,-232.43 322.87,-222.43 319.37,-232.43 326.37,-232.43"/>
-</g>
-<!-- 2f77f976&#45;499c&#45;4601&#45;9d8c&#45;86dd80d66dfa -->
 <g id="node6" class="node">
-<title>2f77f976&#45;499c&#45;4601&#45;9d8c&#45;86dd80d66dfa</title>
+<title>400b4104&#45;ff96&#45;4d64&#45;89b2&#45;49dc6e4e3cbd</title>
-<ellipse fill="yellow" stroke="black" cx="486.87" cy="-202.61" rx="92.45" ry="22.1"/>
+<polygon fill="orange" stroke="black" points="562.72,-94.2 562.72,-114.02 520.24,-128.03 460.16,-128.03 417.68,-114.02 417.68,-94.2 460.16,-80.19 520.24,-80.19 562.72,-94.2"/>
-<text text-anchor="middle" x="486.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">HasSubTask()</text>
+<text text-anchor="middle" x="490.2" y="-96.74" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
 </g>
-<!-- ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e&#45;&gt;2f77f976&#45;499c&#45;4601&#45;9d8c&#45;86dd80d66dfa -->
+<!-- c9a8671e&#45;b0b8&#45;4587&#45;a4de&#45;a094fd678334&#45;&gt;400b4104&#45;ff96&#45;4d64&#45;89b2&#45;49dc6e4e3cbd -->
 <g id="edge5" class="edge">
-<title>ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e&#45;&gt;2f77f976&#45;499c&#45;4601&#45;9d8c&#45;86dd80d66dfa</title>
+<title>c9a8671e&#45;b0b8&#45;4587&#45;a4de&#45;a094fd678334&#45;&gt;400b4104&#45;ff96&#45;4d64&#45;89b2&#45;49dc6e4e3cbd</title>
-<path fill="none" stroke="black" d="M626.96,-266.26C600.92,-254.61 567.48,-239.66 539.8,-227.28"/>
+<path fill="none" stroke="black" d="M358,-168.38C382.01,-156.89 413.11,-142.01 439.11,-129.56"/>
-<polygon fill="black" stroke="black" points="541.44,-224.18 530.88,-223.3 538.58,-230.57 541.44,-224.18"/>
+<polygon fill="black" stroke="black" points="440.29,-132.88 447.8,-125.4 437.27,-126.56 440.29,-132.88"/>
 </g>
-<!-- 2428792c&#45;3896&#45;443d&#45;8744&#45;f5e286644fad -->
+<!-- 676d8bda&#45;2c30&#45;44a5&#45;bca2&#45;ade68761403a -->
+<g id="node4" class="node">
+<title>676d8bda&#45;2c30&#45;44a5&#45;bca2&#45;ade68761403a</title>
+<ellipse fill="yellow" stroke="black" cx="124.2" cy="-22.1" rx="96.17" ry="22.1"/>
+<text text-anchor="middle" x="124.2" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">Is(Curtain,Off)</text>
+</g>
+<!-- a8e26121&#45;f5cc&#45;4ba5&#45;b98e&#45;86bd84de06d6&#45;&gt;676d8bda&#45;2c30&#45;44a5&#45;bca2&#45;ade68761403a -->
+<g id="edge3" class="edge">
+<title>a8e26121&#45;f5cc&#45;4ba5&#45;b98e&#45;86bd84de06d6&#45;&gt;676d8bda&#45;2c30&#45;44a5&#45;bca2&#45;ade68761403a</title>
+<path fill="none" stroke="black" d="M274.46,-84.75C246.5,-73.28 210.25,-58.41 180.36,-46.14"/>
+<polygon fill="black" stroke="black" points="182.06,-43.06 171.48,-42.5 179.4,-49.53 182.06,-43.06"/>
+</g>
+<!-- c3d096cf&#45;f848&#45;46a3&#45;9497&#45;5398614798e8 -->
+<g id="node5" class="node">
+<title>c3d096cf&#45;f848&#45;46a3&#45;9497&#45;5398614798e8</title>
+<polygon fill="lawngreen" stroke="black" points="399.57,-40.1 238.82,-40.1 238.82,-4.1 399.57,-4.1 399.57,-40.1"/>
+<text text-anchor="middle" x="319.2" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">Turn(Curtain,On)</text>
+</g>
+<!-- a8e26121&#45;f5cc&#45;4ba5&#45;b98e&#45;86bd84de06d6&#45;&gt;c3d096cf&#45;f848&#45;46a3&#45;9497&#45;5398614798e8 -->
+<g id="edge4" class="edge">
+<title>a8e26121&#45;f5cc&#45;4ba5&#45;b98e&#45;86bd84de06d6&#45;&gt;c3d096cf&#45;f848&#45;46a3&#45;9497&#45;5398614798e8</title>
+<path fill="none" stroke="black" d="M319.2,-79.97C319.2,-71.24 319.2,-61.2 319.2,-52.01"/>
+<polygon fill="black" stroke="black" points="322.7,-52.11 319.2,-42.11 315.7,-52.11 322.7,-52.11"/>
+</g>
+<!-- 3fc48afc&#45;1cef&#45;45f5&#45;826d&#45;2571d7979a2f -->
 <g id="node7" class="node">
-<title>2428792c&#45;3896&#45;443d&#45;8744&#45;f5e286644fad</title>
+<title>3fc48afc&#45;1cef&#45;45f5&#45;826d&#45;2571d7979a2f</title>
-<polygon fill="orange" stroke="black" points="742.38,-192.7 742.38,-212.52 699.9,-226.53 639.83,-226.53 597.35,-212.52 597.35,-192.7 639.83,-178.69 699.9,-178.69 742.38,-192.7"/>
+<ellipse fill="yellow" stroke="black" cx="490.2" cy="-22.1" rx="72.3" ry="22.1"/>
-<text text-anchor="middle" x="669.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
+<text text-anchor="middle" x="490.2" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">Is(AC,Off)</text>
 </g>
-<!-- ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e&#45;&gt;2428792c&#45;3896&#45;443d&#45;8744&#45;f5e286644fad -->
+<!-- 400b4104&#45;ff96&#45;4d64&#45;89b2&#45;49dc6e4e3cbd&#45;&gt;3fc48afc&#45;1cef&#45;45f5&#45;826d&#45;2571d7979a2f -->
 <g id="edge6" class="edge">
-<title>ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e&#45;&gt;2428792c&#45;3896&#45;443d&#45;8744&#45;f5e286644fad</title>
+<title>400b4104&#45;ff96&#45;4d64&#45;89b2&#45;49dc6e4e3cbd&#45;&gt;3fc48afc&#45;1cef&#45;45f5&#45;826d&#45;2571d7979a2f</title>
-<path fill="none" stroke="black" d="M669.87,-262.22C669.87,-254.79 669.87,-246.39 669.87,-238.33"/>
+<path fill="none" stroke="black" d="M490.2,-79.97C490.2,-72.46 490.2,-63.99 490.2,-55.92"/>
-<polygon fill="black" stroke="black" points="673.37,-238.42 669.87,-228.42 666.37,-238.42 673.37,-238.42"/>
+<polygon fill="black" stroke="black" points="493.7,-56.06 490.2,-46.06 486.7,-56.06 493.7,-56.06"/>
 </g>
-<!-- 64b2362a&#45;d99e&#45;4e99&#45;8772&#45;ad1419e53a2e -->
+<!-- 47af4df0&#45;8811&#45;45a5&#45;95e2&#45;f0fc9458ddfd -->
-<g id="node9" class="node">
-<title>64b2362a&#45;d99e&#45;4e99&#45;8772&#45;ad1419e53a2e</title>
-<polygon fill="orange" stroke="black" points="905.38,-192.7 905.38,-212.52 862.9,-226.53 802.83,-226.53 760.35,-212.52 760.35,-192.7 802.83,-178.69 862.9,-178.69 905.38,-192.7"/>
-<text text-anchor="middle" x="832.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
-</g>
-<!-- ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e&#45;&gt;64b2362a&#45;d99e&#45;4e99&#45;8772&#45;ad1419e53a2e -->
-<g id="edge8" class="edge">
-<title>ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e&#45;&gt;64b2362a&#45;d99e&#45;4e99&#45;8772&#45;ad1419e53a2e</title>
-<path fill="none" stroke="black" d="M709.74,-265.43C731.74,-254.38 759.28,-240.56 782.73,-228.78"/>
-<polygon fill="black" stroke="black" points="784.12,-232 791.48,-224.39 780.98,-225.75 784.12,-232"/>
-</g>
-<!-- bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5 -->
-<g id="node12" class="node">
-<title>bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5</title>
-<polygon fill="orange" stroke="black" points="1384.38,-192.7 1384.38,-212.52 1341.9,-226.53 1281.83,-226.53 1239.35,-212.52 1239.35,-192.7 1281.83,-178.69 1341.9,-178.69 1384.38,-192.7"/>
-<text text-anchor="middle" x="1311.87" y="-195.24" font-family="Times New Roman,serif" font-size="20.00">Sequence</text>
-</g>
-<!-- ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e&#45;&gt;bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5 -->
-<g id="edge11" class="edge">
-<title>ff83c742&#45;a2ba&#45;4aa5&#45;8b3a&#45;39ecd7c03b0e&#45;&gt;bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5</title>
-<path fill="none" stroke="black" d="M742.57,-276.18C862.55,-260.89 1101.31,-230.45 1228.15,-214.28"/>
-<polygon fill="black" stroke="black" points="1228.34,-217.79 1237.82,-213.05 1227.46,-210.84 1228.34,-217.79"/>
-</g>
-<!-- 4bb7a9c3&#45;521b&#45;408f&#45;b403&#45;1fd25d54b192 -->
 <g id="node8" class="node">
-<title>4bb7a9c3&#45;521b&#45;408f&#45;b403&#45;1fd25d54b192</title>
+<title>47af4df0&#45;8811&#45;45a5&#45;95e2&#45;f0fc9458ddfd</title>
-<polygon fill="lawngreen" stroke="black" points="574.49,-129.44 373.24,-129.44 373.24,-93.44 574.49,-93.44 574.49,-129.44"/>
+<polygon fill="lawngreen" stroke="black" points="707.7,-40.1 580.7,-40.1 580.7,-4.1 707.7,-4.1 707.7,-40.1"/>
-<text text-anchor="middle" x="473.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">SubTaskPlaceHolder()</text>
+<text text-anchor="middle" x="644.2" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">Turn(AC,On)</text>
 </g>
-<!-- 2428792c&#45;3896&#45;443d&#45;8744&#45;f5e286644fad&#45;&gt;4bb7a9c3&#45;521b&#45;408f&#45;b403&#45;1fd25d54b192 -->
+<!-- 400b4104&#45;ff96&#45;4d64&#45;89b2&#45;49dc6e4e3cbd&#45;&gt;47af4df0&#45;8811&#45;45a5&#45;95e2&#45;f0fc9458ddfd -->
 <g id="edge7" class="edge">
-<title>2428792c&#45;3896&#45;443d&#45;8744&#45;f5e286644fad&#45;&gt;4bb7a9c3&#45;521b&#45;408f&#45;b403&#45;1fd25d54b192</title>
+<title>400b4104&#45;ff96&#45;4d64&#45;89b2&#45;49dc6e4e3cbd&#45;&gt;47af4df0&#45;8811&#45;45a5&#45;95e2&#45;f0fc9458ddfd</title>
-<path fill="none" stroke="black" d="M627.82,-182.48C596.86,-168.4 554.7,-149.22 522.33,-134.49"/>
+<path fill="none" stroke="black" d="M529.46,-82.71C551.25,-71.39 578.4,-57.28 600.82,-45.64"/>
-<polygon fill="black" stroke="black" points="524.12,-131.46 513.57,-130.51 521.22,-137.83 524.12,-131.46"/>
+<polygon fill="black" stroke="black" points="602.15,-48.89 609.41,-41.17 598.92,-42.68 602.15,-48.89"/>
-</g>
-<!-- fd7124ba&#45;c9d1&#45;4fde&#45;8063&#45;e8337d335121 -->
-<g id="node10" class="node">
-<title>fd7124ba&#45;c9d1&#45;4fde&#45;8063&#45;e8337d335121</title>
-<ellipse fill="yellow" stroke="black" cx="720.87" cy="-111.44" rx="128.52" ry="22.1"/>
-<text text-anchor="middle" x="720.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">FocusingCustomer()</text>
-</g>
-<!-- 64b2362a&#45;d99e&#45;4e99&#45;8772&#45;ad1419e53a2e&#45;&gt;fd7124ba&#45;c9d1&#45;4fde&#45;8063&#45;e8337d335121 -->
-<g id="edge9" class="edge">
-<title>64b2362a&#45;d99e&#45;4e99&#45;8772&#45;ad1419e53a2e&#45;&gt;fd7124ba&#45;c9d1&#45;4fde&#45;8063&#45;e8337d335121</title>
-<path fill="none" stroke="black" d="M803.73,-178.42C789.1,-166.77 771.27,-152.57 755.9,-140.33"/>
-<polygon fill="black" stroke="black" points="758.36,-137.82 748.36,-134.33 754,-143.3 758.36,-137.82"/>
-</g>
-<!-- 169ebec9&#45;3645&#45;4fbb&#45;a533&#45;5186a8e5967b -->
-<g id="node11" class="node">
-<title>169ebec9&#45;3645&#45;4fbb&#45;a533&#45;5186a8e5967b</title>
-<polygon fill="lawngreen" stroke="black" points="1020.49,-129.44 867.24,-129.44 867.24,-93.44 1020.49,-93.44 1020.49,-129.44"/>
-<text text-anchor="middle" x="943.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">ServeCustomer()</text>
-</g>
-<!-- 64b2362a&#45;d99e&#45;4e99&#45;8772&#45;ad1419e53a2e&#45;&gt;169ebec9&#45;3645&#45;4fbb&#45;a533&#45;5186a8e5967b -->
-<g id="edge10" class="edge">
-<title>64b2362a&#45;d99e&#45;4e99&#45;8772&#45;ad1419e53a2e&#45;&gt;169ebec9&#45;3645&#45;4fbb&#45;a533&#45;5186a8e5967b</title>
-<path fill="none" stroke="black" d="M861.74,-178.42C877.67,-165.62 897.45,-149.73 913.6,-136.76"/>
-<polygon fill="black" stroke="black" points="915.36,-139.83 920.97,-130.84 910.98,-134.37 915.36,-139.83"/>
-</g>
-<!-- a847a0cc&#45;c5af&#45;4757&#45;aa8c&#45;8baef72788dc -->
-<g id="node13" class="node">
-<title>a847a0cc&#45;c5af&#45;4757&#45;aa8c&#45;8baef72788dc</title>
-<ellipse fill="yellow" stroke="black" cx="1186.87" cy="-111.44" rx="148.14" ry="22.1"/>
-<text text-anchor="middle" x="1186.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">NewCustomerComing()</text>
-</g>
-<!-- bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5&#45;&gt;a847a0cc&#45;c5af&#45;4757&#45;aa8c&#45;8baef72788dc -->
-<g id="edge12" class="edge">
-<title>bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5&#45;&gt;a847a0cc&#45;c5af&#45;4757&#45;aa8c&#45;8baef72788dc</title>
-<path fill="none" stroke="black" d="M1280,-178.88C1263.4,-167.04 1242.97,-152.46 1225.48,-139.99"/>
-<polygon fill="black" stroke="black" points="1227.51,-137.14 1217.34,-134.18 1223.44,-142.84 1227.51,-137.14"/>
-</g>
-<!-- 99354f05&#45;1716&#45;46b2&#45;9151&#45;d88eac0a5b27 -->
-<g id="node14" class="node">
-<title>99354f05&#45;1716&#45;46b2&#45;9151&#45;d88eac0a5b27</title>
-<polygon fill="cyan" stroke="black" points="1436.87,-142.69 1353.37,-111.44 1436.87,-80.19 1520.37,-111.44 1436.87,-142.69"/>
-<text text-anchor="middle" x="1436.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">Selector</text>
-</g>
-<!-- bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5&#45;&gt;99354f05&#45;1716&#45;46b2&#45;9151&#45;d88eac0a5b27 -->
-<g id="edge13" class="edge">
-<title>bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5&#45;&gt;99354f05&#45;1716&#45;46b2&#45;9151&#45;d88eac0a5b27</title>
-<path fill="none" stroke="black" d="M1343.73,-178.88C1360.85,-166.67 1382.05,-151.54 1399.9,-138.81"/>
-<polygon fill="black" stroke="black" points="1401.55,-141.94 1407.65,-133.28 1397.48,-136.24 1401.55,-141.94"/>
-</g>
-<!-- a3766a34&#45;5152&#45;4f82&#45;8fba&#45;8bf1f6b3830b -->
-<g id="node17" class="node">
-<title>a3766a34&#45;5152&#45;4f82&#45;8fba&#45;8bf1f6b3830b</title>
-<polygon fill="lawngreen" stroke="black" points="1691.49,-129.44 1538.24,-129.44 1538.24,-93.44 1691.49,-93.44 1691.49,-129.44"/>
-<text text-anchor="middle" x="1614.87" y="-104.07" font-family="Times New Roman,serif" font-size="20.00">GreetCustomer()</text>
-</g>
-<!-- bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5&#45;&gt;a3766a34&#45;5152&#45;4f82&#45;8fba&#45;8bf1f6b3830b -->
-<g id="edge16" class="edge">
-<title>bea12066&#45;ecbd&#45;49b0&#45;8934&#45;efcb2c38b5f5&#45;&gt;a3766a34&#45;5152&#45;4f82&#45;8fba&#45;8bf1f6b3830b</title>
-<path fill="none" stroke="black" d="M1369.59,-187.32C1413.74,-176.07 1475.98,-159.54 1529.87,-142.69 1538.78,-139.91 1548.16,-136.78 1557.3,-133.61"/>
-<polygon fill="black" stroke="black" points="1558.17,-137.02 1566.45,-130.4 1555.85,-130.41 1558.17,-137.02"/>
-</g>
-<!-- 19de10a3&#45;7554&#45;43a3&#45;b892&#45;34cb2e32ab9a -->
-<g id="node15" class="node">
-<title>19de10a3&#45;7554&#45;43a3&#45;b892&#45;34cb2e32ab9a</title>
-<ellipse fill="yellow" stroke="black" cx="1347.87" cy="-22.1" rx="95.64" ry="22.1"/>
-<text text-anchor="middle" x="1347.87" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">At(Robot,Bar)</text>
-</g>
-<!-- 99354f05&#45;1716&#45;46b2&#45;9151&#45;d88eac0a5b27&#45;&gt;19de10a3&#45;7554&#45;43a3&#45;b892&#45;34cb2e32ab9a -->
-<g id="edge14" class="edge">
-<title>99354f05&#45;1716&#45;46b2&#45;9151&#45;d88eac0a5b27&#45;&gt;19de10a3&#45;7554&#45;43a3&#45;b892&#45;34cb2e32ab9a</title>
-<path fill="none" stroke="black" d="M1414.41,-88.4C1403.11,-77.32 1389.28,-63.74 1377.16,-51.84"/>
-<polygon fill="black" stroke="black" points="1379.76,-49.5 1370.17,-44.99 1374.86,-54.49 1379.76,-49.5"/>
-</g>
-<!-- 4286d652&#45;c4ef&#45;4522&#45;a7f1&#45;b5c865dcc4c9 -->
-<g id="node16" class="node">
-<title>4286d652&#45;c4ef&#45;4522&#45;a7f1&#45;b5c865dcc4c9</title>
-<polygon fill="lawngreen" stroke="black" points="1589.74,-40.1 1461.99,-40.1 1461.99,-4.1 1589.74,-4.1 1589.74,-40.1"/>
-<text text-anchor="middle" x="1525.87" y="-14.72" font-family="Times New Roman,serif" font-size="20.00">MoveTo(Bar)</text>
-</g>
-<!-- 99354f05&#45;1716&#45;46b2&#45;9151&#45;d88eac0a5b27&#45;&gt;4286d652&#45;c4ef&#45;4522&#45;a7f1&#45;b5c865dcc4c9 -->
-<g id="edge15" class="edge">
-<title>99354f05&#45;1716&#45;46b2&#45;9151&#45;d88eac0a5b27&#45;&gt;4286d652&#45;c4ef&#45;4522&#45;a7f1&#45;b5c865dcc4c9</title>
-<path fill="none" stroke="black" d="M1459.32,-88.4C1471.74,-76.21 1487.23,-61.02 1500.13,-48.35"/>
-<polygon fill="black" stroke="black" points="1502.42,-51.01 1507.1,-41.51 1497.51,-46.02 1502.42,-51.01"/>
 </g>
 </g>
 </svg>

run_robowaiter.py

@@ -1,16 +0,0 @@
-import os
-from robowaiter import Robot, task_map
-
-TASK_NAME = 'VLM'
-
-# create robot
-project_path = "./robowaiter"
-ptml_path = os.path.join(project_path, 'robot/Default.ptml')
-behavior_lib_path = os.path.join(project_path, 'behavior_lib')
-
-robot = Robot(ptml_path, behavior_lib_path)
-
-# create task
-task = task_map[TASK_NAME](robot)
-task.reset()
-task.run()

run_ui.py

@@ -0,0 +1,5 @@
+from robowaiter.scene.ui.pyqt5 import UI
+from robowaiter.robot.robot import Robot
+
+robot = Robot()
+ui = UI(Robot)

sub_task.ptml

@@ -1,31 +0,0 @@
-selector{
-
-sequence{
-    cond On(Coffee,CoffeeTable)
-    cond On(Coffee,CoffeeTable)
-    }
-
-sequence{
-cond Holding(Nothing)
-act Make(Coffee)
-}
-sequence{
-cond At(Robot,CoffeeTable)
- cond Holding(Coffee)
-act PutDown(Coffee,CoffeeTable)
-}
-sequence{
-cond At(Robot,Bar)
- cond Holding(Coffee)
-act PutDown(Coffee,Bar)
-}
-sequence{
-cond At(Robot,WaterTable)
- cond Holding(Coffee)
-act PutDown(Coffee,WaterTable)
-}
-sequence{
-cond Holding(Coffee)
-act MoveTo(CoffeeTable)
-}
-}

tasks_no_ui/AEM/AEM.py

@@ -40,7 +40,7 @@ class SceneAEM(Scene):
         map_ratio = self.map_ratio
         db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2))
 
-        file_name = '../../proto/map_1.pkl'
+        file_name = '../../robowaiter/proto/map_1.pkl'
         if os.path.exists(file_name):
             with open(file_name, 'rb') as file:
                 map = pickle.load(file)
@@ -117,7 +117,7 @@ class SceneAEM(Scene):
                     {"id": f"{i}", "name": f"{cur_objs[i].name}", "location": f"{cur_objs[i].location}",
                      "height": f"{cur_objs[i].location.Z}"})
 
-        with open('../../proto/objs.json', 'w') as file:
+        with open('../../robowaiter/proto/objs.json', 'w') as file:
             json.dump(obj_json_data, file)
 
         # for i in range(-350, 600):

tasks_no_ui/CafeDailyOperations/CafeOneDay.py

@@ -0,0 +1,129 @@
+"""
+视觉语言操作
+机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
+"""
+
+import time
+from robowaiter.scene.scene import Scene
+
+
+class SceneVLM(Scene):
+
+    def __init__(self, robot):
+        super().__init__(robot)
+        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
+
+        self.scene_flag = 1
+        self.st1 = 3
+        # self.st2 = self.st1 + 30
+        # self.st3 = self.st2 + 65
+        self.st2 = 3
+        self.st3 = 3
+        self.st4 = 3
+
+        self.signal_event_list = [
+
+            # 场景1：带小女孩找阳光下的空位
+            (3, self.add_walker, (5, 230, 1200)),  # 0号"Girl02_C_3"
+            (1, self.control_walker, (0, False, 200, 60, 520, 0)),
+            (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
+            (1, self.customer_say, (0, "可以带我过去嘛？")),
+            (13, self.control_walker, (0, False, 50, 140, 1200, 180)),  # 小女孩站在了 BrightTable1 旁边就餐啦
+
+            # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
+            # (3, self.add_walker, (5, 230, 1200)), # 小女孩
+            # # # 上述准备
+            (10, self.add_walker, (26, -28, -150, 90)),
+            (0, self.add_walker, (10, -70, -200, -45)),
+            (5, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
+            (10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), #6
+            # 20 胖胖男到了 BrightTable6
+            (2, self.customer_say, (1, "咖啡有哪些呢？")),  # 10
+            (2, self.customer_say, (1, "来杯卡布奇诺吧。")),  # 15
+
+            # # 场景3：有位女士要杯水和冰红茶
+            (3, self.add_walkers,
+             ([[[21, 65, 1000, -90], [32, -80, 850, 135], [1, 60, 420, 135], [29, -290, 400, 180]]])),
+            (0, self.control_walker, (5, True, 50, 250, 1200, 180)),  # 设置id=4 的2小男孩随机游走红随机游走
+            (0, self.add_walker, (48, 60, 520, 0)),  # 生成他妈妈
+            (0, self.add_walkers, ([[[48, 60, 520, 0], [31, 60, 600, -90], [20, 60, 680, -90], [9, 60, 760, -90]]])),
+            (38, self.customer_say, (7, "哎呦，今天这么多人，还有空位吗？")),  # 女士问 38
+            (10, self.customer_say, (7, "我带着孩子呢，想要宽敞亮堂的地方。")),  # 女士问
+            (8, self.customer_say, (7, "大厅的桌子好啊，快带我去呀！")),
+            (15, self.control_walker, (7, False, 50, -250, 480, 0)),  # #290, 400
+            (3, self.customer_say, (7, "我想来杯水，帮我孩子拿个酸奶吧。")),
+            # # ### 9号灰色男 排队排着排着，不排了
+            (0, self.control_walker, (10, False, 100, 100, 760, 180)),
+            (0, self.control_walker, (10, True, 100, 0, 0, 180)),
+            (90, self.customer_say, (7, "谢谢你的水和酸奶！")),  # 倒水+取放酸奶 90s
+
+
+            (10, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
+            (5, self.customer_say, (8,"你可以帮我拿来吗，我在前门的桌子前等你。")),
+            (1, self.control_walker,(8, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步
+            (1, self.control_walker, (8, False, 80, 240, 1000, -45)), # 红女士走到Table1前
+            (1, self.control_walker, (9, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
+            (2, self.control_walker, (10, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
+            (6, self.customer_say, (8,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
+            (5, self.customer_say, (8, "不用了。")),  # 红女士在Table1前
+
+
+            (8, self.remove_walkers, ([[0, 7, 8]])),
+            (3, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
+            (2, self.customer_say, (6, "好热呀！太阳也好大！")),
+            (20, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
+
+
+            # # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
+            (24, self.clean_walkers, ()),
+            (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
+            (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
+
+        ]
+
+    def _reset(self):
+        self.gen_obj()
+        # self.add_walkers([[47, 920]])
+        pass
+
+    def _run(self, op_type=10):
+        # 一个行人从门口走到 吧台
+        # 打招呼需要什么
+        # 行人说 哪里有位置，想晒个太阳
+        # 带领行人去有太阳的地方
+        # 行人说 有点热
+        # 好的，这就去开空调
+        self.walker_followed = False
+        pass
+
+    def _step(self):
+
+        if self.scene_flag == 1:
+            # 如果机器人不在 吧台
+            if self.walker_followed:
+                return
+            end = [self.status.location.X, self.status.location.Y]
+            if end[1] >= 600 or end[1] <= 450 or end[0] >= 250:
+                # if int(self.status.location.X)!=247 or  int(self.status.location.X)!=520:
+                self.walker_followed = True
+                self.control_walkers_and_say([[0, False, 150, end[0], end[1], 90, "谢谢！"]])
+                self.scene_flag += 1
+
+        # 获得所有顾客的名字
+        # print("=================")
+        # for cus in self.status.walkers:
+        #     print(cus)
+        # print("=================")
+        pass
+
+
+if __name__ == '__main__':
+    import os
+    from robowaiter.robot.robot import Robot
+
+    robot = Robot()
+
+    # create task
+    task = SceneVLM(robot)
+    task.reset()
+    task.run()

tasks_no_ui/CafeDailyOperations/VLN_all.py

@@ -0,0 +1,129 @@
+"""
+视觉语言操作
+机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
+"""
+
+import time
+from robowaiter.scene.scene import Scene
+
+
+class SceneVLM(Scene):
+
+    def __init__(self, robot):
+        super().__init__(robot)
+        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
+
+        self.scene_flag = 2
+        self.st1 = 3
+        # self.st2 = self.st1 + 30
+        # self.st3 = self.st2 + 65
+        self.st2 = 3
+        self.st3 = 3
+        self.st4 = 3
+
+        self.signal_event_list = [
+
+            # 场景1：带小女孩找阳光下的空位
+            # (3, self.add_walker, (5, 230, 1200)),  # 0号"Girl02_C_3"
+            # (1, self.control_walker, (0, False, 200, 60, 520, 0)),
+            # (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
+            # (1, self.customer_say, (0, "可以带我过去嘛？")),
+            # (13, self.control_walker, (0, False, 50, 140, 1200, 180)),  # 小女孩站在了 BrightTable1 旁边就餐啦
+            #
+            # # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
+            # # (3, self.add_walker, (5, 230, 1200)), # 小女孩
+            # # # # 上述准备
+            # (10, self.add_walker, (26, -28, -150, 90)),
+            # (0, self.add_walker, (10, -70, -200, -45)),
+            # (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
+            # (10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])),
+            # # 20 胖胖男到了 BrightTable6
+            # (2, self.customer_say, (1, "咖啡有哪些呢？")),  # 10
+            # (2, self.customer_say, (1, "来杯卡布奇诺吧。")),  # 15
+            #
+            # # # 场景3：有位女士要杯水和冰红茶
+            # (3, self.add_walkers,
+            #  ([[[21, 65, 1000, -90], [32, -80, 850, 135], [1, 60, 420, 135], [29, -290, 400, 180]]])),
+            # (0, self.control_walker, (5, True, 50, 250, 1200, 180)),  # 设置id=4 的2小男孩随机游走红随机游走
+            # (0, self.add_walker, (48, 60, 520, 0)),  # 生成他妈妈
+            # (0, self.add_walkers, ([[[48, 60, 520, 0], [31, 60, 600, -90], [20, 60, 680, -90], [9, 60, 760, -90]]])),
+            # (38, self.customer_say, (7, "哎呦，今天这么多人，还有空位吗？")),  # 女士问
+            # (10, self.customer_say, (7, "我带着孩子呢，想要宽敞亮堂的地方。")),  # 女士问
+            # (8, self.customer_say, (7, "大厅的桌子好啊，快带我去呀！")),
+            # (15, self.control_walker, (7, False, 50, -250, 480, 0)),  # #290, 400
+            # (3, self.customer_say, (7, "我想来杯水，帮我孩子拿个酸奶吧。")),
+            # # # ### 9号灰色男 排队排着排着，不排了
+            # (0, self.control_walker, (10, False, 100, 100, 760, 180)),
+            # (0, self.control_walker, (10, True, 100, 0, 0, 180)),
+            # (90, self.customer_say, (7, "谢谢你的水和酸奶！")),  # 倒水+取放酸奶 90s
+            #
+            #
+            # (10, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
+            # (5, self.customer_say, (8,"你可以帮我拿来吗，我在前门的桌子前等你。")),
+            # (1, self.control_walker,(8, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步
+            # (1, self.control_walker, (8, False, 80, 240, 1000, -45)), # 红女士走到Table1前
+            # (1, self.control_walker, (9, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
+            # (2, self.control_walker, (10, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
+            # (6, self.customer_say, (8,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
+            # (5, self.customer_say, (8, "不用了。")),  # 红女士在Table1前
+            #
+            #
+            # (8, self.remove_walkers, ([[0, 7, 8]])),
+            # (3, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
+            # (2, self.customer_say, (6, "好热呀！太阳也好大！")),
+            # (20, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
+
+
+            # # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
+            (3, self.clean_walkers, ()),
+            (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
+            (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
+
+        ]
+
+    def _reset(self):
+        self.gen_obj()
+        # self.add_walkers([[47, 920]])
+        pass
+
+    def _run(self, op_type=10):
+        # 一个行人从门口走到 吧台
+        # 打招呼需要什么
+        # 行人说 哪里有位置，想晒个太阳
+        # 带领行人去有太阳的地方
+        # 行人说 有点热
+        # 好的，这就去开空调
+        self.walker_followed = False
+        pass
+
+    def _step(self):
+
+        if self.scene_flag == 1:
+            # 如果机器人不在 吧台
+            if self.walker_followed:
+                return
+            end = [self.status.location.X, self.status.location.Y]
+            if end[1] >= 600 or end[1] <= 450 or end[0] >= 250:
+                # if int(self.status.location.X)!=247 or  int(self.status.location.X)!=520:
+                self.walker_followed = True
+                self.control_walkers_and_say([[0, False, 150, end[0], end[1], 90, "谢谢！"]])
+                self.scene_flag += 1
+
+        # 获得所有顾客的名字
+        # print("=================")
+        # for cus in self.status.walkers:
+        #     print(cus)
+        # print("=================")
+        pass
+
+
+if __name__ == '__main__':
+    import os
+    from robowaiter.robot.robot import Robot
+
+    robot = Robot()
+
+    # create task
+    task = SceneVLM(robot)
+    task.reset()
+    task.run()

tasks_no_ui/CafeDailyOperations/VLN_greet_and_order.py

@@ -73,7 +73,7 @@ class SceneVLM(Scene):
             (0, self.control_walker, (10, False, 100, 100, 760, 180)),
             (0, self.control_walker, (10, True, 100, 0, 0, 180)),
             # # # ### 增加场景，孩子说热要开空调 或者9号随机游走和说
-            # # (90, self.customer_say, (7, "谢谢！")), #倒水+取放酸奶 90s
+            (90, self.customer_say, (7, "谢谢！")), #倒水+取放酸奶 90s
             # (3, self.customer_say, (7, "谢谢！")),
 
 

tasks_no_ui/GQA/GQA_1_ask_order_place.py

@@ -26,6 +26,14 @@ class SceneGQA(Scene):
             (8, self.customer_say, (6, "太棒啦，亲爱的。")),
             (8, self.customer_say, (5, "那你知道附近最近的电影院在哪吗?")),
             (8, self.customer_say, (6, "谢啦，那我们先去阴凉位置下个棋，等电影开始了就去看呢!")),
+            (15, self.customer_say, (6, "你们这人可真多。")),
+            (15, self.customer_say, (6, "我女朋友怕晒，有空余的阴凉位置嘛？")),
+            (15, self.customer_say, (6, "那还不错。")),
+            (15, self.customer_say, (5, "请问洗手间在哪呢？")),
+            (15, self.customer_say, (5, "我们还想一起下下棋,切磋切磋。")),
+            (15, self.customer_say, (6, "太棒啦，亲爱的。")),
+            (15, self.customer_say, (5, "那你知道附近最近的电影院在哪吗?")),
+            (15, self.customer_say, (6, "谢啦，那我们先去阴凉位置下个棋，等电影开始了就去看呢!")),
 
             # (6, self.customer_say, (5, "你好呀，这位可是我女朋友呢！")),
             # (5, self.customer_say, (6, "你们这有什么饮料嘛？")),

tasks_no_ui/Interact/system.py

@@ -1,10 +1,5 @@
 """
-人提出请求，机器人完成任务
+交互式场景，输入
-1. 做咖啡（固定动画）：接收到做咖啡指令、走到咖啡机、拿杯子、操作咖啡机、取杯子、送到客人桌子上
-2. 倒水
-3. 夹点心
-
-具体描述：设计一套点单规则（如菜单包含咖啡、水、点心等），按照规则拟造随机的订单。在收到订单后，通过大模型让机器人输出合理的备餐计划，并尝试在模拟环境中按照这个规划实现任务。
 
 """
 
@@ -27,9 +22,9 @@ class SubScene(Scene):
             question = input("请输入指令：")
             if question[-1] == ")":
                 print(f"设置目标:{question}")
-                self.set_goal(question)()
+                self.new_set_goal(question)
             else:
-                self.state['chat_list'].append(question)
+                self.customer_say("System",question)
 
 
 

tasks_no_ui/OT/OT_coffee_water_dessert.py

@@ -11,7 +11,7 @@ class SceneVLM(Scene):
         super().__init__(robot)
         # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
         self.signal_event_list = [
-            (5, self.customer_say, (5, "给我来杯咖啡，哦对，再倒一杯水。")),
+            (8, self.customer_say, (5, "给我来杯咖啡，哦对，再倒一杯水。")),
             (1, self.control_walker_ls,([[[5, False, 100, -250, 480, 0],[6, False, 100, 60, 520, 0]]])),
             (10, self.customer_say, (5, "感谢，这些够啦，你去忙吧。")),
             (2, self.customer_say, (6, "我想来份点心和酸奶。")),

tasks_no_ui/OT/Open_tasks_test.py

@@ -18,13 +18,16 @@ class SceneOT(Scene):
         super().__init__(robot)
         # 在这里加入场景中发生的事件
         self.signal_event_list = [
+            (3, self.customer_say, ("System", "冰红茶")),
+            # (3, self.customer_say, ("System", "酸奶。")),
             # (3, self.customer_say, ("System","来一号桌")),
             # (-1, self.customer_say, ("System","回去吧")),
             # (5, self.set_goal("At(Robot,BrightTable4)"))
         ]
-        self.event_list = [
+        # self.event_list = [
-            (3, self.set_goal("On(VacuumCup,Bar)"))
+        #     # (3, self.set_goal("On(VacuumCup,Bar)"))
-        ]
+        #     (3, self.set_goal("On(Yogurt,Bar)"))
+        # ]
 
     def _reset(self):
         # self.add_walkers([[0, 880], [250, 1200]])

tasks_no_ui/VLM/VLM.py

@@ -83,18 +83,19 @@ class SceneVLM(Scene):
         # "抓握物体","放置物体"                                       # 16-17
 
         # self.gen_obj()
-        if op_type <=15:
+        # if op_type <=15:
-            self.move_task_area(op_type)
+        #     self.move_task_area(op_type)
-            self.op_task_execute(op_type)
+        #     self.op_task_execute(op_type)
-        if op_type == 16:   # 16: 抓操作需要传入物品id
+        # if op_type == 16:   # 16: 抓操作需要传入物品id
-            self.move_task_area(op_type, obj_id=1)
+        #     self.move_task_area(op_type, obj_id=281)
-            self.op_task_execute(op_type, obj_id=1)
+        #     self.op_task_execute(op_type, obj_id=281)
-        # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 100.0], 水杯桌:[-70.0, 500.0, 107]
+        #     op_type = 17
-        # 桌子1:[-55.0, 0.0, 107],抹布桌:[340.0, 900.0, 99.0]   # 桌子2:[-55.0, 150.0, 107],
+        # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 100.0], 水杯桌:[-70.0, 500.0, 107]
-        if op_type == 17:   # 17: 放操作需要传入放置位置周围的可达区域
+        # # 桌子1:[-55.0, 0.0, 107],抹布桌:[340.0, 900.0, 99.0]   # 桌子2:[-55.0, 150.0, 107],
-            pos = [240.0, 40.0, 100.0]
+        # if op_type == 17:   # 17: 放操作需要传入放置位置周围的可达区域
-            self.move_task_area(op_type, release_pos=pos)
+        #     pos = [240.0, 40.0, 100.0]
-            self.op_task_execute(op_type, release_pos=pos)   # [325.0, 860.0, 100]
+        #     self.move_task_area(op_type, release_pos=pos)
+        #     self.op_task_execute(op_type, release_pos=pos)   # [325.0, 860.0, 100]
 
 
 
@@ -111,13 +112,14 @@ class SceneVLM(Scene):
         # self.move_task_area(1)
         # self.op_task_execute(1)
         #
-        # self.find_obj("CoffeeCup")
+        # self.find_obj("Plate")
-        #
+        # #
-        # self.move_task_area(16, obj_id=275)
+        id = 0
-        # self.op_task_execute(16, obj_id=275)
+        self.move_task_area(16, obj_id=id)
-        # pos = [-70.0, 500.0, 107]
+        self.op_task_execute(16, obj_id=id)
-        # self.move_task_area(17, release_pos=pos)
+        pos = [-70.0, 500.0, 111]   # 107  98
-        # self.op_task_execute(17, release_pos=pos)
+        self.move_task_area(17, release_pos=pos)
+        self.op_task_execute(17, release_pos=pos)
         #
         # # 倒水:倒完的水放到旁边桌子上
         # self.move_task_area(2)

tasks_no_ui/VLM/VLM_1_order.py

@@ -13,7 +13,7 @@ class SceneVLM(Scene):
         self.signal_event_list = [
             (3, self.add_walker,  (20,0,700)),
             (1, self.control_walker, (6, False,100, 60, 520,0)),
-            (1, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
+            (1, self.customer_say, (6, "给我来份薯片和果汁，我坐在对面的桌子那儿。")), #给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。
             (5, self.control_walker, (6, False, 100, -250, 480, 0)),
         ]
 

tasks_no_ui/VLN/VLN_edge_map.py

@@ -3,8 +3,6 @@
 
 测试导航算法的动态避障(Obstacle Detection and Avoidance, ODA)能力
 
-
-
 """
 import os
 import pickle

zoo/behavior_lib/Behavior.py

@@ -1,27 +0,0 @@
-import py_trees as ptree
-from typing import Any
-
-# _base Behavior
-class Bahavior(ptree.behaviour.Behaviour):
-    scene = None
-    def __init__(self, name: str, scene):
-        super().__init__(name)
-        self.scene = 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("this is just a _base behavior node.")
-
-
-    def update(self) -> ptree.common.Status:
-        re = self._update()
-        print(f"{self.__class__.__name__}: {re.value}")
-        return re
-
-    def terminate(self, new_status: ptree.common.Status) -> None:
-        return super().terminate(new_status)

zoo/behavior_lib/Chatting.py

@@ -1,23 +0,0 @@
-import py_trees as ptree
-from typing import Any
-from robowaiter.behavior_lib._base.Behavior import Bahavior
-
-class Chatting(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:
-        # if self.scene.status?
-        if self.scene.state['chat_list'] == []:
-            return ptree.common.Status.FAILURE
-        else:
-            return ptree.common.Status.SUCCESS
-
-    def terminate(self, new_status: ptree.common.Status) -> None:
-        return super().terminate(new_status)

zoo/behavior_lib/CoffeeCupFound.py

@@ -1,20 +0,0 @@
-import py_trees as ptree
-from typing import Any
-from robowaiter.behavior_lib._base.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)

zoo/behavior_lib/CoffeeCupGrasped.py

@@ -1,22 +0,0 @@
-import py_trees as ptree
-from typing import Any
-from robowaiter.behavior_lib._base.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)
-    

zoo/behavior_lib/CoffeeCupPlaced.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/DealChat.py

@@ -1,26 +0,0 @@
-import py_trees as ptree
-from typing import Any
-from robowaiter.behavior_lib._base.Behavior import Bahavior
-from robowaiter.llm_client.ask_llm import ask_llm
-
-class DealChat(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:
-        # if self.scene.status?
-        chat = self.scene.state['chat_list'].pop()
-        answer = ask_llm(chat)
-        print(f"机器人回答：{answer}")
-        self.scene.chat_bubble(f"机器人回答：{answer}")
-
-        return ptree.common.Status.RUNNING
-
-    def terminate(self, new_status: ptree.common.Status) -> None:
-        return super().terminate(new_status)

zoo/behavior_lib/DestinationAReached.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/FindCoffeeCup.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/Grasp.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/GraspCoffeeCup.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/Istask.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/MoveTo.py

@@ -1,21 +0,0 @@
-import py_trees as ptree
-from typing import Any
-
-class MoveTo(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)
-    

zoo/behavior_lib/PlaceCoffeeCup.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/ReachDestinationA.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/SeqTest.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_lib/TestTask.py

@@ -1,21 +0,0 @@
-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)
-    

zoo/behavior_tree/behavior_library.py

@@ -1,124 +0,0 @@
-"""
-    顶层行为树中的动作与条件节点
-"""
-
-from typing import *
-import py_trees
-from py_trees import common
-from py_trees.common import Status
-
-
-##############################################################
-#                       条件节点 
-##############################################################
-
-class IsChatting(py_trees.behaviour.Behaviour):
-    def __init__(self, name: str = ""):
-        super().__init__(name)
-
-    def setup(self, **kwargs: Any) -> None:
-        return super().setup(**kwargs)
-    
-    def initialise(self) -> None:
-        return super().initialise()
-    
-    def update(self) -> Status:
-        print('Start checking IsChatting...')
-        return common.Status.SUCCESS
-    
-    def terminate(self, new_status: Status) -> None:
-        return super().terminate(new_status)
-    
-
-class IsTakingAction(py_trees.behaviour.Behaviour):
-    def __init__(self, name: str = ""):
-        super().__init__(name)
-
-    def setup(self, **kwargs: Any) -> None:
-        return super().setup(**kwargs)
-    
-    def initialise(self) -> None:
-        return super().initialise()
-    
-    def update(self) -> Status:
-        print('Start checking IsTakingAction...')
-        return common.Status.SUCCESS
-    
-    def terminate(self, new_status: Status) -> None:
-        return super().terminate(new_status)
-    
-
-class IsSomethingMore(py_trees.behaviour.Behaviour):
-    def __init__(self, name: str = ""):
-        super().__init__(name)
-
-    def setup(self, **kwargs: Any) -> None:
-        return super().setup(**kwargs)
-    
-    def initialise(self) -> None:
-        return super().initialise()
-    
-    def update(self) -> Status:
-        print('Start checking IsSomethingMore...')
-        return common.Status.SUCCESS
-    
-    def terminate(self, new_status: Status) -> None:
-        return super().terminate(new_status)
-    
-
-##############################################################
-#                       动作节点 
-##############################################################
-
-class Chatting(py_trees.behaviour.Behaviour):
-    def __init__(self, name: str = ""):
-        super().__init__(name)
-
-    def setup(self, **kwargs: Any) -> None:
-        return super().setup(**kwargs)
-    
-    def initialise(self) -> None:
-        return super().initialise()
-    
-    def update(self) -> Status:
-        print('Start executing Chatting...')
-        return common.Status.SUCCESS
-    
-    def terminate(self, new_status: Status) -> None:
-        return super().terminate(new_status)
-    
-
-class TakingAction(py_trees.behaviour.Behaviour):
-    def __init__(self, name: str = ""):
-        super().__init__(name)
-
-    def setup(self, **kwargs: Any) -> None:
-        return super().setup(**kwargs)
-    
-    def initialise(self) -> None:
-        return super().initialise()
-    
-    def update(self) -> Status:
-        print('Start executing TakingAction...')
-        return common.Status.SUCCESS
-    
-    def terminate(self, new_status: Status) -> None:
-        return super().terminate(new_status)
-    
-
-class TakingMoreAction(py_trees.behaviour.Behaviour):
-    def __init__(self, name: str = ""):
-        super().__init__(name)
-
-    def setup(self, **kwargs: Any) -> None:
-        return super().setup(**kwargs)
-    
-    def initialise(self) -> None:
-        return super().initialise()
-    
-    def update(self) -> Status:
-        print('Start executing TakingMoreAction...')
-        return common.Status.SUCCESS
-    
-    def terminate(self, new_status: Status) -> None:
-        return super().terminate(new_status)

zoo/behavior_tree/main.py

@@ -1,66 +0,0 @@
-import py_trees
-from behavior_library import *
-
-
-def LoadMainTree() -> py_trees.trees.BehaviourTree:
-    """
-        此方法用于加载固定的顶层行为树（不包括实际执行）
-
-        Args: None
-    """
-
-    seq_subtree_0 = py_trees.composites.Sequence(
-        name='seq_subtree_0',
-        memory=False,
-        children=[IsChatting(), Chatting()]
-    )
-
-    seq_subtree_1 = py_trees.composites.Sequence(
-        name='seq_subtree_1',
-        memory=False,
-        children=[IsTakingAction(), TakingAction()]
-    )
-
-    seq_subtree_2 = py_trees.composites.Sequence(
-        name='seq_subtree_2',
-        memory=False,
-        children=[IsSomethingMore(), TakingMoreAction()]
-    )
-
-    root = py_trees.composites.Selector(
-        name='selector_root',
-        memory=False,
-        children=[seq_subtree_0, seq_subtree_1, seq_subtree_2]
-    )
-
-    return py_trees.trees.BehaviourTree(root)
-
-
-def LoadSubTree(path: str) -> py_trees.behaviour.Behaviour:
-    """
-        此方法用于从ptml文件中加载行为树（不包括实际执行）
-
-        Args:
-            -- path: ptml文件的路径
-    """
-    # TODO
-    pass
-
-
-if __name__ == '__main__':
-    btree = LoadMainTree()
-
-
-    def print_tree(tree):
-        print(py_trees.display.unicode_tree(root=tree.root, show_status=True))
-
-
-    try:
-        btree.tick_tock(
-            period_ms=500,
-            number_of_iterations=py_trees.trees.CONTINUOUS_TICK_TOCK,
-            pre_tick_handler=None,
-            post_tick_handler=print_tree
-        )
-    except KeyboardInterrupt:
-        btree.interrupt()

zoo/demo/关节控制.py

@@ -1,59 +0,0 @@
-#!/usr/bin/env python3
-# -*- encoding: utf-8 -*-
-import sys
-import time
-import grpc
-
-import matplotlib.pyplot as plt
-import numpy as np
-from mpl_toolkits.axes_grid1 import make_axes_locatable
-
-from proto import GrabSim_pb2
-from 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)
-        ])
-
-sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
-
-def map_test(map_id=0, scene_num=1):
-    initworld = sim_client.Init(GrabSim_pb2.NUL())
-    print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
-    initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
-
-def joint_test(scene_id=0):
-    print('------------------joint_test----------------------')
-    action_list = [[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.37, 37.2, -92.4, 4.13, -0.62, 0.4],
-                   [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.62, 34.75, -94.80, 3.22, -0.26, 0.85],
-                   [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 32.63, -32.80, 15.15, -110.70, 6.86, 2.36, 0.40],
-                   [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 28.18, -27.92, 6.75, -115.02, 9.46, 4.28, 1.35],
-                   [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 4.09, -13.15, -11.97, -107.35, 13.08, 8.58, 3.33]]
-
-    for value in action_list:
-        action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
-        scene = sim_client.Do(action)
-
-        for i in range(8, 21):  # arm
-            print(
-                f"{scene.joints[i].name}:{scene.joints[i].angle} location:{scene.joints[i].location.X},{scene.joints[i].location.Y},{scene.joints[i].location.Z}"
-            )
-        print('')
-        for i in range(5, 10):  # Right hand
-            print(
-                f"{scene.fingers[i].name} angle:{scene.fingers[i].angle} location:{scene.fingers[i].location[0].X},{scene.fingers[i].location[0].Y},{scene.fingers[i].location[0].Z}"
-            )
-        print('----------------------------------------')
-        time.sleep(0.03)
-    time.sleep(1)
-
-if __name__ == '__main__':
-    map_id = 3                      # 地图编号: 3: 咖啡厅
-    scene_num = 1                   # 场景数量
-    map_test(map_id, scene_num)     # 场景加载测试
-    time.sleep(5)
-
-    for i in range(scene_num):
-        print("------------------", i, "----------------------")
-        joint_test(i)               # 关节控制测试

zoo/demo/动画控制.py

@@ -1,94 +0,0 @@
-#!/usr/bin/env python3
-# -*- encoding: utf-8 -*-
-# enconding = utf8
-import sys
-import time
-import grpc
-
-import matplotlib.pyplot as plt
-import numpy as np
-from mpl_toolkits.axes_grid1 import make_axes_locatable
-
-from proto import GrabSim_pb2
-from 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)
-])
-
-sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
-
-
-def map_test(map_id=0, scene_num=1):
-    initworld = sim_client.Init(GrabSim_pb2.NUL())
-    print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
-    initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
-
-
-def control_robot_action(scene_id=0, type=0, action=0, message="你好"):
-    scene = sim_client.ControlRobot(GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message))
-    if (scene.info == "action success"):
-        return True
-    else:
-        return False
-
-
-if __name__ == '__main__':
-    map_id = 3  # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室
-    scene_num = 1  # 场景数量
-    map_test(map_id, scene_num)  # 场景加载测试
-    time.sleep(5)
-
-    # 制作咖啡
-    control_robot_action(0, 0, 1, "开始制作咖啡")
-    result = control_robot_action(0, 1, 1)
-    if (result):
-        control_robot_action(0, 1, 2)
-        control_robot_action(0, 1, 3)
-        control_robot_action(0, 1, 4)
-    else:
-        control_robot_action(0, 0, 1, "制作咖啡失败")
-
-    # 倒水
-    control_robot_action(0, 0, 1, "开始倒水")
-    result = control_robot_action(0, 2, 1)
-    if (result):
-        control_robot_action(0, 2, 2)
-        control_robot_action(0, 2, 3)
-        control_robot_action(0, 2, 4)
-        control_robot_action(0, 2, 5)
-    else:
-        control_robot_action(0, 0, 1, "倒水失败")
-
-    # 夹点心
-    control_robot_action(0, 0, 1, "开始夹点心")
-    result = control_robot_action(0, 3, 1)
-    if (result):
-        control_robot_action(0, 3, 2)
-        control_robot_action(0, 3, 3)
-        control_robot_action(0, 3, 4)
-        control_robot_action(0, 3, 5)
-        control_robot_action(0, 3, 6)
-        control_robot_action(0, 3, 7)
-    else:
-        control_robot_action(0, 0, 1, "夹点心失败")
-
-        # 拖地
-    control_robot_action(0, 0, 1, "开始拖地")
-    result = control_robot_action(0, 4, 1)
-    if (result):
-        control_robot_action(0, 4, 2)
-        control_robot_action(0, 4, 3)
-        control_robot_action(0, 4, 4)
-    else:
-        control_robot_action(0, 0, 1, "拖地失败")
-
-        # 擦桌子
-    control_robot_action(0, 0, 1, "开始擦桌子")
-    result = control_robot_action(0, 5, 1)
-    if (result):
-        control_robot_action(0, 5, 2)
-        control_robot_action(0, 5, 3)
-    else:
-        control_robot_action(0, 0, 1, "擦桌子失败")