diff --git a/README.md b/README.md index 0a3eb56..62ec602 100644 --- a/README.md +++ b/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 -task_map是任务字典，通过任务缩写来返回相应的场景类。 +## tasks +tasks文件夹中存放的场景定义及运行代码。 -| 缩写 | 任务 | -|----|---------| -| AEM | 主动探索和记忆 | -| GQA | 具身多轮对话 | -| VLN | 视觉语言导航 | -| VLM | 视觉语言操作 | -| OT | 复杂开放任务 | -| AT | 自主任务 | +| 缩写 | 任务 | +|---------------------|-------------| +| AEM | 主动探索和记忆 | +| GQA | 具身多轮对话 | +| VLN | 视觉语言导航 | +| VLM | 视觉语言操作 | +| OT | 复杂开放任务 | +| 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表示对话结束。 +输入字符即可等待回答 diff --git a/requirements.txt b/requirements.txt index c43674f..31cae50 100644 --- a/requirements.txt +++ b/requirements.txt @@ -13,4 +13,5 @@ matplotlib~=3.8.0 numpy~=1.26.0 setuptools~=68.0.0 pydot~=1.4.2 -colorama~=0.4.6 \ No newline at end of file +colorama~=0.4.6 +pyqt5 \ No newline at end of file diff --git a/robowaiter/algos/navigator/navigate.py b/robowaiter/algos/navigator/navigate.py index a06c147..ddad6bc 100644 --- a/robowaiter/algos/navigator/navigate.py +++ b/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:] # 去除已走过的路径 diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index c60ff51..7051e10 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/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', - 'VacuumCup'} + # all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', + # '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 diff --git a/robowaiter/behavior_lib/act/DealChat.py b/robowaiter/behavior_lib/act/DealChat.py index 9b66db8..4b074bd 100644 --- a/robowaiter/behavior_lib/act/DealChat.py +++ b/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'] diff --git a/robowaiter/behavior_lib/act/DelSubTree.py b/robowaiter/behavior_lib/act/DelSubTree.py index 24f9e07..f5551b0 100644 --- a/robowaiter/behavior_lib/act/DelSubTree.py +++ b/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 \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/FreeHands.py b/robowaiter/behavior_lib/act/FreeHands.py new file mode 100644 index 0000000..c391bc3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 5b954ca..5362413 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/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"]) diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index 7d16d25..240c360 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/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) diff --git a/robowaiter/behavior_lib/act/PickUp.py b/robowaiter/behavior_lib/act/PickUp.py index 2380c3d..ace0035 100644 --- a/robowaiter/behavior_lib/act/PickUp.py +++ b/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"]) diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py index 670785c..abe4650 100644 --- a/robowaiter/behavior_lib/act/PutDown.py +++ b/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"]) diff --git a/robowaiter/behavior_lib/act/Turn.py b/robowaiter/behavior_lib/act/Turn.py index 538d549..5a7166d 100644 --- a/robowaiter/behavior_lib/act/Turn.py +++ b/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"] diff --git a/robowaiter/behavior_lib/cond/FocusingCustomer.py b/robowaiter/behavior_lib/cond/FocusingCustomer.py index 1685a85..4a07198 100644 --- a/robowaiter/behavior_lib/cond/FocusingCustomer.py +++ b/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: - self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio) + # if self.scene.take_picture: + # 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'] diff --git a/robowaiter/behavior_lib/cond/NewCustomer.py b/robowaiter/behavior_lib/cond/NewCustomer.py index 8d46ca6..358ba02 100644 --- a/robowaiter/behavior_lib/cond/NewCustomer.py +++ b/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 diff --git a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py index 11bab3d..f23a6d6 100644 --- a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py +++ b/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: # 剪枝操作 diff --git a/robowaiter/llm_client/data/fix_questions.txt b/robowaiter/llm_client/data/fix_questions.txt index 5ceeb0a..372673f 100644 --- a/robowaiter/llm_client/data/fix_questions.txt +++ b/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)"} diff --git a/robowaiter/proto/camera.py b/robowaiter/proto/camera.py index cb73d19..8161f06 100644 --- a/robowaiter/proto/camera.py +++ b/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') - - plt.subplot(2, 7, 14) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图 - - plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10) + sence.send_img("img_label_obj") + # plt.subplot(2, 7, 14) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图 + # plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10) # plt.show() return cur_obstacle_world_points, cur_objs_id diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index bc8a0bc..f9ed11d 100644 --- a/robowaiter/robot/robot.py +++ b/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 = [] diff --git a/robowaiter/scene/__init__.py b/robowaiter/scene/__init__.py index 4a2d484..eca84a2 100644 --- a/robowaiter/scene/__init__.py +++ b/robowaiter/scene/__init__.py @@ -1,11 +1,11 @@ # from .scene import Scene -# from robowaiter.scene.tasks.AEM import SceneAEM -# from robowaiter.scene.tasks.GQA import SceneGQA -# from robowaiter.scene.tasks.VLN import SceneVLN -# from robowaiter.scene.tasks.VLM import SceneVLM -# from robowaiter.scene.tasks.Open_tasks import SceneOT -# from robowaiter.scene.tasks.Auto_tasks import SceneAT +# from robowaiter.scene.tasks_no_ui.AEM import SceneAEM +# from robowaiter.scene.tasks_no_ui.GQA import SceneGQA +# from robowaiter.scene.tasks_no_ui.VLN import SceneVLN +# from robowaiter.scene.tasks_no_ui.VLM import SceneVLM +# from robowaiter.scene.tasks_no_ui.Open_tasks import SceneOT +# from robowaiter.scene.tasks_no_ui.Auto_tasks import SceneAT # task_map = { # "AEM": SceneAEM, diff --git a/robowaiter/scene/outputs/current_bt.png b/robowaiter/scene/outputs/current_bt.png new file mode 100644 index 0000000..550746e Binary files /dev/null and b/robowaiter/scene/outputs/current_bt.png differ diff --git a/robowaiter/scene/outputs/vision.png b/robowaiter/scene/outputs/vision.png new file mode 100644 index 0000000..9b5a0d2 Binary files /dev/null and b/robowaiter/scene/outputs/vision.png differ diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index a6f61c8..bb3294b 100644 --- a/robowaiter/scene/scene.py +++ b/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)', - 'On(BottledDrink,Bar)', + 'Holding(Nothing)', 'Exist(Yogurt)', 'Exist(BottledDrink)', + '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)) - init_world() - stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) + self.stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) + self.init_world() + 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)) - scene = stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=IDs, scene=self.sceneID)) + s = self.stub.Observe(GrabSim_pb2.SceneID(value=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=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z=h, roll=0, pitch=0, yaw=0, type=9), + + GrabSim_pb2.ObjectList.Object(x=190, y=40, z=87, roll=0, pitch=0, yaw=0, + 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, - type=9), + # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 70, z=95, roll=0, pitch=0, yaw=0, + # 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_y -= 1 - # values = [0,0,0,0,0, 10,-25,-45,-45,-45] - # values= [-6, 0, 0, 0, 0, -6, 0, 45, 45, 45] + # obj_x += 2.5 + # obj_y -= 0.7 # 1.7 + # obj_z -= 6 + # 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) - plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None) - plt.axis("off") - plt.title("相机分割") + img_segment = d_segment 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) - plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None) + img_obj = d_color + + # 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) - 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) + self.send_img("img_label_map") + + + + # 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 \ No newline at end of file diff --git a/robowaiter/scene/tasks/CafeDailyOperations/CafeOneDay.py b/robowaiter/scene/tasks/CafeDailyOperations/CafeOneDay.py deleted file mode 100644 index 34de4cc..0000000 --- a/robowaiter/scene/tasks/CafeDailyOperations/CafeOneDay.py +++ /dev/null @@ -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() diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py b/robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py deleted file mode 100644 index 8926794..0000000 --- a/robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py +++ /dev/null @@ -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() diff --git a/robowaiter/scene/tasks/VLN.py b/robowaiter/scene/tasks/VLN.py deleted file mode 100644 index ce59d79..0000000 --- a/robowaiter/scene/tasks/VLN.py +++ /dev/null @@ -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() diff --git a/robowaiter/scene/interact/__init__.py b/robowaiter/scene/ui/__init__.py similarity index 100% rename from robowaiter/scene/interact/__init__.py rename to robowaiter/scene/ui/__init__.py diff --git a/robowaiter/scene/ui/pyqt5.py b/robowaiter/scene/ui/pyqt5.py new file mode 100644 index 0000000..4abb830 --- /dev/null +++ b/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_()) \ No newline at end of file diff --git a/robowaiter/scene/ui/scene_ui.py b/robowaiter/scene/ui/scene_ui.py new file mode 100644 index 0000000..765cdec --- /dev/null +++ b/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) + diff --git a/robowaiter/scene/ui/ui.py b/robowaiter/scene/ui/ui.py new file mode 100644 index 0000000..310a1d4 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/robowaiter/scene/ui/ui2py.bat b/robowaiter/scene/ui/ui2py.bat new file mode 100644 index 0000000..d383f1c --- /dev/null +++ b/robowaiter/scene/ui/ui2py.bat @@ -0,0 +1 @@ +pyuic5 -x window.ui -o window.py \ No newline at end of file diff --git a/robowaiter/scene/ui/window.py b/robowaiter/scene/ui/window.py new file mode 100644 index 0000000..a2093e7 --- /dev/null +++ b/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_()) diff --git a/robowaiter/scene/ui/window.ui b/robowaiter/scene/ui/window.ui new file mode 100644 index 0000000..e03d462 --- /dev/null +++ b/robowaiter/scene/ui/window.ui @@ -0,0 +1,484 @@ + + + MainWindow + + + + 0 + 0 + 936 + 1080 + + + + MainWindow + + + false + + + + + + 520 + 390 + 221 + 31 + + + + + Times New Roman + 14 + + + + Is(AC,On) + + + + + + 760 + 390 + 81 + 31 + + + + + 黑体 + 14 + + + + 说话 + + + + + + 80 + 540 + 321 + 231 + + + + border: 2px solid black; + + + + + + + + + 40 + 830 + 831 + 161 + + + + border: 2px solid black; + + + + + + + + + 410 + 800 + 121 + 21 + + + + + 黑体 + 14 + + + + 当前行为树 + + + + + + 520 + 540 + 321 + 231 + + + + border: 2px solid black; + + + + + + + + + 80 + 320 + 321 + 161 + + + + border: 2px solid black; + + + + + + + + + 190 + 290 + 121 + 21 + + + + + 黑体 + 14 + + + + 可达性地图 + + + + + + 200 + 500 + 111 + 31 + + + + + 黑体 + 14 + + + + 实例分割 + + + + + + 630 + 510 + 121 + 21 + + + + + 黑体 + 14 + + + + 目标检测 + + + + + + 30 + 70 + 151 + 31 + + + + + 黑体 + 14 + + + + 环境主动探索 + + + + + + 10 + 20 + 371 + 31 + + + + + 黑体 + 10 + + + + 任务演示：（播放动画时需等待动画播放完毕才会重置场景） + + + + + + 30 + 120 + 151 + 31 + + + + + 黑体 + 14 + + + + 视觉语言导航 + + + + + + 30 + 170 + 151 + 31 + + + + + 黑体 + 14 + + + + 视觉语言操作 + + + + + + 200 + 70 + 141 + 31 + + + + + 黑体 + 14 + + + + 具身多轮对话 + + + + + + 200 + 120 + 141 + 31 + + + + + 黑体 + 14 + + + + 开放具身任务 + + + + + + 200 + 170 + 141 + 31 + + + + + 黑体 + 14 + + + + 自主具身任务 + + + + + + 30 + 220 + 311 + 31 + + + + + 黑体 + 14 + + + + 重置 + + + + + + 380 + 50 + 151 + 201 + + + + + 11 + + + + + System + + + + + + + 390 + 20 + 121 + 16 + + + + + 黑体 + 14 + + + + 顾客列表： + + + + + + 560 + 20 + 141 + 16 + + + + + 黑体 + 14 + + + + 顾客历史对话： + + + + + + 740 + 20 + 131 + 16 + + + + + 黑体 + 14 + + + + 全局历史对话： + + + + + true + + + + 550 + 50 + 171 + 261 + + + + + 11 + + + + true + + + + + true + + + + 740 + 50 + 161 + 261 + + + + + 11 + + + + true + + + + + + + 0 + 0 + 936 + 23 + + + + + + + + diff --git a/robowaiter/utils/bt/draw.py b/robowaiter/utils/bt/draw.py index 8139bee..b0e8f55 100644 --- a/robowaiter/utils/bt/draw.py +++ b/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"] diff --git a/robowaiter/utils/draw_bt/Default.ptml b/robowaiter/utils/draw_bt/Default.ptml index 03745e6..55a6f88 100644 --- a/robowaiter/utils/draw_bt/Default.ptml +++ b/robowaiter/utils/draw_bt/Default.ptml @@ -1,17 +1,10 @@ -selector - sequence - cond CustomerChatting() - act DealChat() - sequence - cond HasSubTask() - sequence - act SubTaskPlaceHolder() - sequence - cond FocusingCustomer() - act ServeCustomer() - sequence - cond NewCustomerComing() - selector - cond At(Robot,Bar) - act MoveTo(Bar) - act GreetCustomer() +sequence{ +cond Holding(Nothing) +sequence{ +cond Is(Curtain,Off) +act Turn(Curtain,On) +} +sequence{ +cond Is(AC,Off) +act Turn(AC,On) +}} \ No newline at end of file diff --git a/robowaiter/utils/draw_bt/Default_bracket.ptml b/robowaiter/utils/draw_bt/Default_bracket.ptml index e18182b..844db49 100644 --- a/robowaiter/utils/draw_bt/Default_bracket.ptml +++ b/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() -}} \ No newline at end of file + } act GreetCustomer() + } sequence +{ + cond AnomalyDetected() + act ResolveAnomaly() + }} \ No newline at end of file diff --git a/robowaiter/utils/draw_bt/test.dot b/robowaiter/utils/draw_bt/test.dot index bd79eac..cf2b2d7 100644 --- a/robowaiter/utils/draw_bt/test.dot +++ b/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]; -"717a1290-56bc-41e1-9b4b-e6df93d40e64" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "717a1290-56bc-41e1-9b4b-e6df93d40e64"; -"b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2" [fillcolor=yellow, fontcolor=black, fontsize=20, label="CustomerChatting()", shape=ellipse, style=filled]; -"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2"; -"28015225-b7a4-4f38-b26f-4dbe5eea4154" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="DealChat()", shape=box, style=filled]; -"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "28015225-b7a4-4f38-b26f-4dbe5eea4154"; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e"; -"2f77f976-499c-4601-9d8c-86dd80d66dfa" [fillcolor=yellow, fontcolor=black, fontsize=20, label="HasSubTask()", shape=ellipse, style=filled]; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2f77f976-499c-4601-9d8c-86dd80d66dfa"; -"2428792c-3896-443d-8744-f5e286644fad" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2428792c-3896-443d-8744-f5e286644fad"; -"4bb7a9c3-521b-408f-b403-1fd25d54b192" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="SubTaskPlaceHolder()", shape=box, style=filled]; -"2428792c-3896-443d-8744-f5e286644fad" -> "4bb7a9c3-521b-408f-b403-1fd25d54b192"; -"64b2362a-d99e-4e99-8772-ad1419e53a2e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "64b2362a-d99e-4e99-8772-ad1419e53a2e"; -"fd7124ba-c9d1-4fde-8063-e8337d335121" [fillcolor=yellow, fontcolor=black, fontsize=20, label="FocusingCustomer()", shape=ellipse, style=filled]; -"64b2362a-d99e-4e99-8772-ad1419e53a2e" -> "fd7124ba-c9d1-4fde-8063-e8337d335121"; -"169ebec9-3645-4fbb-a533-5186a8e5967b" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="ServeCustomer()", shape=box, style=filled]; -"64b2362a-d99e-4e99-8772-ad1419e53a2e" -> "169ebec9-3645-4fbb-a533-5186a8e5967b"; -"bea12066-ecbd-49b0-8934-efcb2c38b5f5" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "bea12066-ecbd-49b0-8934-efcb2c38b5f5"; -"a847a0cc-c5af-4757-aa8c-8baef72788dc" [fillcolor=yellow, fontcolor=black, fontsize=20, label="NewCustomerComing()", shape=ellipse, style=filled]; -"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "a847a0cc-c5af-4757-aa8c-8baef72788dc"; -"99354f05-1716-46b2-9151-d88eac0a5b27" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=diamond, style=filled, width=0.01]; -"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "99354f05-1716-46b2-9151-d88eac0a5b27"; -"19de10a3-7554-43a3-b892-34cb2e32ab9a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="At(Robot,Bar)", shape=ellipse, style=filled]; -"99354f05-1716-46b2-9151-d88eac0a5b27" -> "19de10a3-7554-43a3-b892-34cb2e32ab9a"; -"4286d652-c4ef-4522-a7f1-b5c865dcc4c9" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="MoveTo(Bar)", shape=box, style=filled]; -"99354f05-1716-46b2-9151-d88eac0a5b27" -> "4286d652-c4ef-4522-a7f1-b5c865dcc4c9"; -"a3766a34-5152-4f82-8fba-8bf1f6b3830b" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="GreetCustomer()", shape=box, style=filled]; -"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "a3766a34-5152-4f82-8fba-8bf1f6b3830b"; +"c9a8671e-b0b8-4587-a4de-a094fd678334" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Selector, 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]; +"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "bc597ff6-b3cb-4573-9dea-944fdb38d045"; +"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "a8e26121-f5cc-4ba5-b98e-86bd84de06d6"; +"676d8bda-2c30-44a5-bca2-ade68761403a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(Curtain,Off)", shape=ellipse, style=filled]; +"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "676d8bda-2c30-44a5-bca2-ade68761403a"; +"c3d096cf-f848-46a3-9497-5398614798e8" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(Curtain,On)", shape=box, style=filled]; +"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "c3d096cf-f848-46a3-9497-5398614798e8"; +"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "400b4104-ff96-4d64-89b2-49dc6e4e3cbd"; +"3fc48afc-1cef-45f5-826d-2571d7979a2f" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(AC,Off)", shape=ellipse, style=filled]; +"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "3fc48afc-1cef-45f5-826d-2571d7979a2f"; +"47af4df0-8811-45a5-95e2-f0fc9458ddfd" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(AC,On)", shape=box, style=filled]; +"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "47af4df0-8811-45a5-95e2-f0fc9458ddfd"; } diff --git a/robowaiter/utils/draw_bt/test.png b/robowaiter/utils/draw_bt/test.png index 773238d..15a3183 100644 Binary files a/robowaiter/utils/draw_bt/test.png and b/robowaiter/utils/draw_bt/test.png differ diff --git a/robowaiter/utils/draw_bt/test.svg b/robowaiter/utils/draw_bt/test.svg index 2ed2acf..29211a1 100644 --- a/robowaiter/utils/draw_bt/test.svg +++ b/robowaiter/utils/draw_bt/test.svg @@ -4,208 +4,100 @@ - - + + pastafarianism - - + + -a3ac082f-ad46-4cc9-86b4-3e76c3c564d3 - -Selector +c9a8671e-b0b8-4587-a4de-a094fd678334 + +Selector - + -717a1290-56bc-41e1-9b4b-e6df93d40e64 - -Sequence +bc597ff6-b3cb-4573-9dea-944fdb38d045 + +Holding(Nothing) - + -a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->717a1290-56bc-41e1-9b4b-e6df93d40e64 - - +c9a8671e-b0b8-4587-a4de-a094fd678334->bc597ff6-b3cb-4573-9dea-944fdb38d045 + + - - -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e - -Sequence - - - -a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e - - - - + -b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2 - -CustomerChatting() +a8e26121-f5cc-4ba5-b98e-86bd84de06d6 + +Sequence - + -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 - -DealChat() - - - -717a1290-56bc-41e1-9b4b-e6df93d40e64->28015225-b7a4-4f38-b26f-4dbe5eea4154 - - - - + -2f77f976-499c-4601-9d8c-86dd80d66dfa - -HasSubTask() +400b4104-ff96-4d64-89b2-49dc6e4e3cbd + +Sequence - + -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2f77f976-499c-4601-9d8c-86dd80d66dfa - - +c9a8671e-b0b8-4587-a4de-a094fd678334->400b4104-ff96-4d64-89b2-49dc6e4e3cbd + + - + + +676d8bda-2c30-44a5-bca2-ade68761403a + +Is(Curtain,Off) + + + +a8e26121-f5cc-4ba5-b98e-86bd84de06d6->676d8bda-2c30-44a5-bca2-ade68761403a + + + + + +c3d096cf-f848-46a3-9497-5398614798e8 + +Turn(Curtain,On) + + + +a8e26121-f5cc-4ba5-b98e-86bd84de06d6->c3d096cf-f848-46a3-9497-5398614798e8 + + + + -2428792c-3896-443d-8744-f5e286644fad - -Sequence +3fc48afc-1cef-45f5-826d-2571d7979a2f + +Is(AC,Off) - + -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2428792c-3896-443d-8744-f5e286644fad - - +400b4104-ff96-4d64-89b2-49dc6e4e3cbd->3fc48afc-1cef-45f5-826d-2571d7979a2f + + - - -64b2362a-d99e-4e99-8772-ad1419e53a2e - -Sequence - - - -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->64b2362a-d99e-4e99-8772-ad1419e53a2e - - - - - -bea12066-ecbd-49b0-8934-efcb2c38b5f5 - -Sequence - - - -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->bea12066-ecbd-49b0-8934-efcb2c38b5f5 - - - - + -4bb7a9c3-521b-408f-b403-1fd25d54b192 - -SubTaskPlaceHolder() +47af4df0-8811-45a5-95e2-f0fc9458ddfd + +Turn(AC,On) - + -2428792c-3896-443d-8744-f5e286644fad->4bb7a9c3-521b-408f-b403-1fd25d54b192 - - - - - -fd7124ba-c9d1-4fde-8063-e8337d335121 - -FocusingCustomer() - - - -64b2362a-d99e-4e99-8772-ad1419e53a2e->fd7124ba-c9d1-4fde-8063-e8337d335121 - - - - - -169ebec9-3645-4fbb-a533-5186a8e5967b - -ServeCustomer() - - - -64b2362a-d99e-4e99-8772-ad1419e53a2e->169ebec9-3645-4fbb-a533-5186a8e5967b - - - - - -a847a0cc-c5af-4757-aa8c-8baef72788dc - -NewCustomerComing() - - - -bea12066-ecbd-49b0-8934-efcb2c38b5f5->a847a0cc-c5af-4757-aa8c-8baef72788dc - - - - - -99354f05-1716-46b2-9151-d88eac0a5b27 - -Selector - - - -bea12066-ecbd-49b0-8934-efcb2c38b5f5->99354f05-1716-46b2-9151-d88eac0a5b27 - - - - - -a3766a34-5152-4f82-8fba-8bf1f6b3830b - -GreetCustomer() - - - -bea12066-ecbd-49b0-8934-efcb2c38b5f5->a3766a34-5152-4f82-8fba-8bf1f6b3830b - - - - - -19de10a3-7554-43a3-b892-34cb2e32ab9a - -At(Robot,Bar) - - - -99354f05-1716-46b2-9151-d88eac0a5b27->19de10a3-7554-43a3-b892-34cb2e32ab9a - - - - - -4286d652-c4ef-4522-a7f1-b5c865dcc4c9 - -MoveTo(Bar) - - - -99354f05-1716-46b2-9151-d88eac0a5b27->4286d652-c4ef-4522-a7f1-b5c865dcc4c9 - - +400b4104-ff96-4d64-89b2-49dc6e4e3cbd->47af4df0-8811-45a5-95e2-f0fc9458ddfd + + diff --git a/run_robowaiter.py b/run_robowaiter.py deleted file mode 100644 index dda629e..0000000 --- a/run_robowaiter.py +++ /dev/null @@ -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() diff --git a/run_ui.py b/run_ui.py new file mode 100644 index 0000000..55e6d51 --- /dev/null +++ b/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) diff --git a/sub_task.ptml b/sub_task.ptml deleted file mode 100644 index 33895cd..0000000 --- a/sub_task.ptml +++ /dev/null @@ -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) -} -} diff --git a/robowaiter/scene/tasks/AEM.py b/tasks_no_ui/AEM/AEM.py similarity index 98% rename from robowaiter/scene/tasks/AEM.py rename to tasks_no_ui/AEM/AEM.py index dbbc598..e22cca2 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/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): diff --git a/robowaiter/scene/tasks/AT/__init__.py b/tasks_no_ui/AEM/__init__.py similarity index 100% rename from robowaiter/scene/tasks/AT/__init__.py rename to tasks_no_ui/AEM/__init__.py diff --git a/robowaiter/scene/tasks/AT/Auto_tasks_light.py b/tasks_no_ui/AT/Auto_tasks_light.py similarity index 100% rename from robowaiter/scene/tasks/AT/Auto_tasks_light.py rename to tasks_no_ui/AT/Auto_tasks_light.py diff --git a/robowaiter/scene/tasks/GQA/__init__.py b/tasks_no_ui/AT/__init__.py similarity index 100% rename from robowaiter/scene/tasks/GQA/__init__.py rename to tasks_no_ui/AT/__init__.py diff --git a/tasks_no_ui/CafeDailyOperations/CafeOneDay.py b/tasks_no_ui/CafeDailyOperations/CafeOneDay.py new file mode 100644 index 0000000..6b79826 --- /dev/null +++ b/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() diff --git a/tasks_no_ui/CafeDailyOperations/VLN_all.py b/tasks_no_ui/CafeDailyOperations/VLN_all.py new file mode 100644 index 0000000..0f8aa54 --- /dev/null +++ b/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() diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py b/tasks_no_ui/CafeDailyOperations/VLN_greet_and_order.py similarity index 99% rename from robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py rename to tasks_no_ui/CafeDailyOperations/VLN_greet_and_order.py index 654ab2e..246898b 100644 --- a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py +++ b/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, "谢谢！")), diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1118.py b/tasks_no_ui/CafeDailyOperations/VLN_greet_and_order_1118.py similarity index 100% rename from robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1118.py rename to tasks_no_ui/CafeDailyOperations/VLN_greet_and_order_1118.py diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1119.py b/tasks_no_ui/CafeDailyOperations/VLN_greet_and_order_1119.py similarity index 100% rename from robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1119.py rename to tasks_no_ui/CafeDailyOperations/VLN_greet_and_order_1119.py diff --git a/robowaiter/scene/tasks/OT/__init__.py b/tasks_no_ui/CafeDailyOperations/__init__.py similarity index 100% rename from robowaiter/scene/tasks/OT/__init__.py rename to tasks_no_ui/CafeDailyOperations/__init__.py diff --git a/robowaiter/scene/tasks/GQA/GQA.py b/tasks_no_ui/GQA/GQA.py similarity index 100% rename from robowaiter/scene/tasks/GQA/GQA.py rename to tasks_no_ui/GQA/GQA.py diff --git a/robowaiter/scene/tasks/GQA/GQA_1_ask_order_place.py b/tasks_no_ui/GQA/GQA_1_ask_order_place.py similarity index 81% rename from robowaiter/scene/tasks/GQA/GQA_1_ask_order_place.py rename to tasks_no_ui/GQA/GQA_1_ask_order_place.py index a0c1714..1d99758 100644 --- a/robowaiter/scene/tasks/GQA/GQA_1_ask_order_place.py +++ b/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, "你们这有什么饮料嘛？")), diff --git a/robowaiter/scene/tasks/GQA/GQA_NLP.py b/tasks_no_ui/GQA/GQA_NLP.py similarity index 100% rename from robowaiter/scene/tasks/GQA/GQA_NLP.py rename to tasks_no_ui/GQA/GQA_NLP.py diff --git a/robowaiter/scene/tasks/VLM/__init__.py b/tasks_no_ui/GQA/__init__.py similarity index 100% rename from robowaiter/scene/tasks/VLM/__init__.py rename to tasks_no_ui/GQA/__init__.py diff --git a/robowaiter/scene/tasks/__init__.py b/tasks_no_ui/Interact/__init__.py similarity index 100% rename from robowaiter/scene/tasks/__init__.py rename to tasks_no_ui/Interact/__init__.py diff --git a/robowaiter/scene/interact/system.py b/tasks_no_ui/Interact/system.py similarity index 54% rename from robowaiter/scene/interact/system.py rename to tasks_no_ui/Interact/system.py index 6cbd372..fc5517b 100644 --- a/robowaiter/scene/interact/system.py +++ b/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) diff --git a/robowaiter/scene/tasks/OT/OT_coffee_water_dessert.py b/tasks_no_ui/OT/OT_coffee_water_dessert.py similarity index 96% rename from robowaiter/scene/tasks/OT/OT_coffee_water_dessert.py rename to tasks_no_ui/OT/OT_coffee_water_dessert.py index a36779b..0934aa1 100644 --- a/robowaiter/scene/tasks/OT/OT_coffee_water_dessert.py +++ b/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, "我想来份点心和酸奶。")), diff --git a/robowaiter/scene/tasks/OT/Open_tasks.py b/tasks_no_ui/OT/Open_tasks.py similarity index 100% rename from robowaiter/scene/tasks/OT/Open_tasks.py rename to tasks_no_ui/OT/Open_tasks.py diff --git a/robowaiter/scene/tasks/OT/Open_tasks_bright_table.py b/tasks_no_ui/OT/Open_tasks_bright_table.py similarity index 100% rename from robowaiter/scene/tasks/OT/Open_tasks_bright_table.py rename to tasks_no_ui/OT/Open_tasks_bright_table.py diff --git a/robowaiter/scene/tasks/OT/Open_tasks_test.py b/tasks_no_ui/OT/Open_tasks_test.py similarity index 82% rename from robowaiter/scene/tasks/OT/Open_tasks_test.py rename to tasks_no_ui/OT/Open_tasks_test.py index 895cc98..996c28a 100644 --- a/robowaiter/scene/tasks/OT/Open_tasks_test.py +++ b/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 = [ - (3, self.set_goal("On(VacuumCup,Bar)")) - ] + # self.event_list = [ + # # (3, self.set_goal("On(VacuumCup,Bar)")) + # (3, self.set_goal("On(Yogurt,Bar)")) + # ] def _reset(self): # self.add_walkers([[0, 880], [250, 1200]]) diff --git a/robowaiter/scene/tasks/OT/Open_tasks_walkers.py b/tasks_no_ui/OT/Open_tasks_walkers.py similarity index 100% rename from robowaiter/scene/tasks/OT/Open_tasks_walkers.py rename to tasks_no_ui/OT/Open_tasks_walkers.py diff --git a/zoo/__init__.py b/tasks_no_ui/OT/__init__.py similarity index 100% rename from zoo/__init__.py rename to tasks_no_ui/OT/__init__.py diff --git a/robowaiter/scene/tasks/VLM/VLM.py b/tasks_no_ui/VLM/VLM.py similarity index 83% rename from robowaiter/scene/tasks/VLM/VLM.py rename to tasks_no_ui/VLM/VLM.py index eb42cd1..56d4747 100644 --- a/robowaiter/scene/tasks/VLM/VLM.py +++ b/tasks_no_ui/VLM/VLM.py @@ -83,18 +83,19 @@ class SceneVLM(Scene): # "抓握物体","放置物体" # 16-17 # self.gen_obj() - if op_type <=15: - self.move_task_area(op_type) - self.op_task_execute(op_type) - if op_type == 16: # 16: 抓操作需要传入物品id - self.move_task_area(op_type, obj_id=1) - self.op_task_execute(op_type, obj_id=1) - # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 100.0], 水杯桌:[-70.0, 500.0, 107] - # 桌子1:[-55.0, 0.0, 107],抹布桌:[340.0, 900.0, 99.0] # 桌子2:[-55.0, 150.0, 107], - if op_type == 17: # 17: 放操作需要传入放置位置周围的可达区域 - pos = [240.0, 40.0, 100.0] - self.move_task_area(op_type, release_pos=pos) - self.op_task_execute(op_type, release_pos=pos) # [325.0, 860.0, 100] + # if op_type <=15: + # self.move_task_area(op_type) + # self.op_task_execute(op_type) + # if op_type == 16: # 16: 抓操作需要传入物品id + # self.move_task_area(op_type, obj_id=281) + # self.op_task_execute(op_type, obj_id=281) + # op_type = 17 + # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 100.0], 水杯桌:[-70.0, 500.0, 107] + # # 桌子1:[-55.0, 0.0, 107],抹布桌:[340.0, 900.0, 99.0] # 桌子2:[-55.0, 150.0, 107], + # if op_type == 17: # 17: 放操作需要传入放置位置周围的可达区域 + # pos = [240.0, 40.0, 100.0] + # 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.move_task_area(16, obj_id=275) - # self.op_task_execute(16, obj_id=275) - # pos = [-70.0, 500.0, 107] - # self.move_task_area(17, release_pos=pos) - # self.op_task_execute(17, release_pos=pos) + # self.find_obj("Plate") + # # + id = 0 + self.move_task_area(16, obj_id=id) + self.op_task_execute(16, obj_id=id) + pos = [-70.0, 500.0, 111] # 107 98 + self.move_task_area(17, release_pos=pos) + self.op_task_execute(17, release_pos=pos) # # # 倒水:倒完的水放到旁边桌子上 # self.move_task_area(2) diff --git a/robowaiter/scene/tasks/VLM/VLM3.py b/tasks_no_ui/VLM/VLM3.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM3.py rename to tasks_no_ui/VLM/VLM3.py diff --git a/robowaiter/scene/tasks/VLM/VLM_1_order.py b/tasks_no_ui/VLM/VLM_1_order.py similarity index 87% rename from robowaiter/scene/tasks/VLM/VLM_1_order.py rename to tasks_no_ui/VLM/VLM_1_order.py index 337ccac..4e8f5e5 100644 --- a/robowaiter/scene/tasks/VLM/VLM_1_order.py +++ b/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)), ] diff --git a/robowaiter/scene/tasks/VLM/VLM_2_AC.py b/tasks_no_ui/VLM/VLM_2_AC.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_2_AC.py rename to tasks_no_ui/VLM/VLM_2_AC.py diff --git a/robowaiter/scene/tasks/VLM/VLM_AC.py b/tasks_no_ui/VLM/VLM_AC.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_AC.py rename to tasks_no_ui/VLM/VLM_AC.py diff --git a/robowaiter/scene/tasks/VLM/VLM_cafe_shutdown.py b/tasks_no_ui/VLM/VLM_cafe_shutdown.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_cafe_shutdown.py rename to tasks_no_ui/VLM/VLM_cafe_shutdown.py diff --git a/robowaiter/scene/tasks/VLM/VLM_order.py b/tasks_no_ui/VLM/VLM_order.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_order.py rename to tasks_no_ui/VLM/VLM_order.py diff --git a/robowaiter/scene/tasks/VLM/VLN_greet_lead.py b/tasks_no_ui/VLM/VLN_greet_lead.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLN_greet_lead.py rename to tasks_no_ui/VLM/VLN_greet_lead.py diff --git a/zoo/behavior_lib/__init__.py b/tasks_no_ui/VLM/__init__.py similarity index 100% rename from zoo/behavior_lib/__init__.py rename to tasks_no_ui/VLM/__init__.py diff --git a/robowaiter/scene/tasks/VLN/VLN_edge_map.py b/tasks_no_ui/VLN/VLN_edge_map.py similarity index 99% rename from robowaiter/scene/tasks/VLN/VLN_edge_map.py rename to tasks_no_ui/VLN/VLN_edge_map.py index f2897f0..0a88c3a 100644 --- a/robowaiter/scene/tasks/VLN/VLN_edge_map.py +++ b/tasks_no_ui/VLN/VLN_edge_map.py @@ -3,8 +3,6 @@ 测试导航算法的动态避障(Obstacle Detection and Avoidance, ODA)能力 - - """ import os import pickle diff --git a/robowaiter/scene/tasks/VLN/VLN_edge_obj.py b/tasks_no_ui/VLN/VLN_edge_obj.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_edge_obj.py rename to tasks_no_ui/VLN/VLN_edge_obj.py diff --git a/robowaiter/scene/tasks/VLN/VLN_greet_lead.py b/tasks_no_ui/VLN/VLN_greet_lead.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_greet_lead.py rename to tasks_no_ui/VLN/VLN_greet_lead.py diff --git a/robowaiter/scene/tasks/VLN/VLN_oda_eazy.py b/tasks_no_ui/VLN/VLN_oda_eazy.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_oda_eazy.py rename to tasks_no_ui/VLN/VLN_oda_eazy.py diff --git a/robowaiter/scene/tasks/VLN/VLN_oda_hard.py b/tasks_no_ui/VLN/VLN_oda_hard.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_oda_hard.py rename to tasks_no_ui/VLN/VLN_oda_hard.py diff --git a/robowaiter/scene/tasks/VLN/VLN_oda_normal.py b/tasks_no_ui/VLN/VLN_oda_normal.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_oda_normal.py rename to tasks_no_ui/VLN/VLN_oda_normal.py diff --git a/zoo/behavior_tree/__init__.py b/tasks_no_ui/VLN/__init__.py similarity index 100% rename from zoo/behavior_tree/__init__.py rename to tasks_no_ui/VLN/__init__.py diff --git a/zoo/demo/__init__.py b/tasks_no_ui/__init__.py similarity index 100% rename from zoo/demo/__init__.py rename to tasks_no_ui/__init__.py diff --git a/zoo/behavior_lib/Behavior.py b/zoo/behavior_lib/Behavior.py deleted file mode 100644 index 850459a..0000000 --- a/zoo/behavior_lib/Behavior.py +++ /dev/null @@ -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) diff --git a/zoo/behavior_lib/Chatting.py b/zoo/behavior_lib/Chatting.py deleted file mode 100644 index 563c23f..0000000 --- a/zoo/behavior_lib/Chatting.py +++ /dev/null @@ -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) diff --git a/zoo/behavior_lib/CoffeeCupFound.py b/zoo/behavior_lib/CoffeeCupFound.py deleted file mode 100644 index e7130ec..0000000 --- a/zoo/behavior_lib/CoffeeCupFound.py +++ /dev/null @@ -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) diff --git a/zoo/behavior_lib/CoffeeCupGrasped.py b/zoo/behavior_lib/CoffeeCupGrasped.py deleted file mode 100644 index 32c010a..0000000 --- a/zoo/behavior_lib/CoffeeCupGrasped.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/CoffeeCupPlaced.py b/zoo/behavior_lib/CoffeeCupPlaced.py deleted file mode 100644 index d4cdb35..0000000 --- a/zoo/behavior_lib/CoffeeCupPlaced.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/DealChat.py b/zoo/behavior_lib/DealChat.py deleted file mode 100644 index c1fee07..0000000 --- a/zoo/behavior_lib/DealChat.py +++ /dev/null @@ -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) diff --git a/zoo/behavior_lib/DestinationAReached.py b/zoo/behavior_lib/DestinationAReached.py deleted file mode 100644 index 9092023..0000000 --- a/zoo/behavior_lib/DestinationAReached.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/FindCoffeeCup.py b/zoo/behavior_lib/FindCoffeeCup.py deleted file mode 100644 index 6781c50..0000000 --- a/zoo/behavior_lib/FindCoffeeCup.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/Grasp.py b/zoo/behavior_lib/Grasp.py deleted file mode 100644 index 57c5708..0000000 --- a/zoo/behavior_lib/Grasp.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/GraspCoffeeCup.py b/zoo/behavior_lib/GraspCoffeeCup.py deleted file mode 100644 index 285804e..0000000 --- a/zoo/behavior_lib/GraspCoffeeCup.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/Istask.py b/zoo/behavior_lib/Istask.py deleted file mode 100644 index 4a1922c..0000000 --- a/zoo/behavior_lib/Istask.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/MoveTo.py b/zoo/behavior_lib/MoveTo.py deleted file mode 100644 index 516e9dc..0000000 --- a/zoo/behavior_lib/MoveTo.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/PlaceCoffeeCup.py b/zoo/behavior_lib/PlaceCoffeeCup.py deleted file mode 100644 index 7cf0bbd..0000000 --- a/zoo/behavior_lib/PlaceCoffeeCup.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/ReachDestinationA.py b/zoo/behavior_lib/ReachDestinationA.py deleted file mode 100644 index 18fbe03..0000000 --- a/zoo/behavior_lib/ReachDestinationA.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/SeqTest.py b/zoo/behavior_lib/SeqTest.py deleted file mode 100644 index e07ab74..0000000 --- a/zoo/behavior_lib/SeqTest.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_lib/TestTask.py b/zoo/behavior_lib/TestTask.py deleted file mode 100644 index c9c53d7..0000000 --- a/zoo/behavior_lib/TestTask.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/zoo/behavior_tree/behavior_library.py b/zoo/behavior_tree/behavior_library.py deleted file mode 100644 index 113cc49..0000000 --- a/zoo/behavior_tree/behavior_library.py +++ /dev/null @@ -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) \ No newline at end of file diff --git a/zoo/behavior_tree/main.py b/zoo/behavior_tree/main.py deleted file mode 100644 index 78cdb99..0000000 --- a/zoo/behavior_tree/main.py +++ /dev/null @@ -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() diff --git a/zoo/demo/关节控制.py b/zoo/demo/关节控制.py deleted file mode 100644 index fbbff16..0000000 --- a/zoo/demo/关节控制.py +++ /dev/null @@ -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) # 关节控制测试 \ No newline at end of file diff --git a/zoo/demo/动画控制.py b/zoo/demo/动画控制.py deleted file mode 100644 index 457bbbc..0000000 --- a/zoo/demo/动画控制.py +++ /dev/null @@ -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, "擦桌子失败") \ No newline at end of file diff --git a/zoo/demo/场景操作.py b/zoo/demo/场景操作.py deleted file mode 100644 index cc48eb7..0000000 --- a/zoo/demo/场景操作.py +++ /dev/null @@ -1,47 +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 reset(scene_id=0): - scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id)) - -def show_env_info(scene_id=0): - scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - print('------------------show_env_info----------------------') - print( - f"location:{[scene.location.X, scene.location.Y]}, rotation:{scene.rotation.Yaw}\n", - f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", f"objects number: {len(scene.objects)}\n" - f"rotation:{scene.rotation}, timestep:{scene.timestep}\n" - f"timestamp:{scene.timestamp}, collision:{scene.collision}, info:{scene.info}") - -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, "----------------------") - reset(i) # 场景重置测试 - show_env_info(i) # 场景信息测试 \ No newline at end of file diff --git a/zoo/demo/导航寻路.py b/zoo/demo/导航寻路.py deleted file mode 100644 index 21cf412..0000000 --- a/zoo/demo/导航寻路.py +++ /dev/null @@ -1,139 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -# enconding = utf8 -import sys -import time -import grpc - -sys.path.append('./') -sys.path.append('../') - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from robowaiter.proto import GrabSim_pb2_grpc,GrabSim_pb2 - - -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 Init(): - sim_client.Init(GrabSim_pb2.NUL()) - - -''' -获取当前可加载的地图信息(地图名字、地图尺寸) -''' - - -def AcquireAvailableMaps(): - AvailableMaps = sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()) - print(AvailableMaps) - - -''' -1、根据mapID加载指定地图 -2、如果scene_num>1,则根据地图尺寸偏移后加载多个相同地图 -3、这样就可以在一个关卡中训练多个地图 -''' - - -def SetWorld(map_id=0, scene_num=1): - print('------------------SetWorld----------------------') - world = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - - -''' -返回场景的状态信息 -1、返回机器人的位置和旋转 -2、返回各个关节的名字和旋转 -3、返回场景中标记的物品信息(名字、类型、位置、旋转) -4、返回场景中行人的信息(名字、位置、旋转、速度) -5、返回机器人手指和双臂的碰撞信息 -''' - - -def Observe(scene_id=0): - print('------------------show_env_info----------------------') - scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - print( - f"location:{[scene.location]}, rotation:{scene.rotation}\n", - f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", - f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" - f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" - f"collision:{scene.collision}, info:{scene.info}") - - -''' -重置场景 -1、重置桌子的宽度和高度 -2、清除生成的行人和物品 -3、重置关节角度、位置旋转 -4、清除碰撞信息 -5、重置场景中标记的物品 -''' - - -def Reset(scene_id=0): - print('------------------Reset----------------------') - scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id)) - print(scene) - - # 如果场景支持调整桌子 - # sim_client.Reset(GrabSim_pb2.ResetParams(scene = scene_id, adjust = True, height = 100.0, width = 100.0))" - - -""" -导航移动 -yaw:机器人朝向; -velocity:速度,>0代表移动,<0代表瞬移,=0代表只查询; -dis:最终达到的位置距离目标点最远距离,如果超过此距离则目标位置不可达 -""" - - -def navigation_move(scene_id=0, map_id=0): - print('------------------navigation_move----------------------') - scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] - print("position:", walk_value) - - if map_id == 11: # coffee - v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] - else: - v_list = [[0.0, 0.0]] - - for walk_v in v_list: - walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100] - print("walk_v", walk_v) - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - scene = sim_client.Do(action) - print(scene.info) - - -if __name__ == '__main__': - map_id = 11 # 地图编号 - scene_num = 1 # 场景数量 - - print('------------ 初始化加载场景 ------------') - Init() - AcquireAvailableMaps() - SetWorld(map_id, scene_num) - time.sleep(5.0) - - for i in range(scene_num): - print('------------ 场景操作 ------------') - Observe(i) - Reset(i) - - print('------------ 导航移动 ------------') - navigation_move(i, map_id) \ No newline at end of file diff --git a/zoo/demo/文字冒泡.py b/zoo/demo/文字冒泡.py deleted file mode 100644 index 370ffb8..0000000 --- a/zoo/demo/文字冒泡.py +++ /dev/null @@ -1,44 +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, "你好，欢迎光临") \ No newline at end of file diff --git a/zoo/demo/物品操作.py b/zoo/demo/物品操作.py deleted file mode 100644 index f63a4c7..0000000 --- a/zoo/demo/物品操作.py +++ /dev/null @@ -1,103 +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) - -def gen_obj(scene_id, h=80): - print('------------------gen objs----------------------') - scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z] - - obj_list = [ - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 90, y=ginger_loc[1] + 30, yaw=10, z=h, type=4), - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 80, y=ginger_loc[1] + 31, z=h, type=5), - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 33, y=ginger_loc[1] - 10.5, z=h+20, type=7), - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 70, y=ginger_loc[1] + 33, z=h, type=9), - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 60, y=ginger_loc[1] + 34, z=h, type=13) - ] - scene = sim_client.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=scene_id)) - print(scene.collision) - time.sleep(5) - -def remove_obj(scene_id=0, id_list=[1]): - print('------------------remove objs----------------------') - remove_obj_list = id_list - scene = sim_client.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_obj_list, scene=scene_id)) - print(f"remove objects {id_list}. current obj:") - time.sleep(1) - -def clean_obj(scene_id=0): - print('------------------clean objs----------------------') - scene = sim_client.CleanObjects(GrabSim_pb2.SceneID(value=scene_id)) - -def obj_test(scene_id=0): - gen_obj(scene_id) - # remove_obj(scene_id, id_list=[0]) - # clean_obj(scene_id) - -def grasp_test(hand_id, obj_scene_id, scene_id=0): - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Grasp, values=[hand_id, obj_scene_id]) - scene = sim_client.Do(action) - -def release_test(hand_id, scene_id=0): - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Release, values=[hand_id]) - scene = sim_client.Do(action) - -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) # 关节控制测试 - obj_test(i) # 物品生成测试 - grasp_test(1, 2) # 抓取物品测试 - joint_test(i) # 关节控制测试 - release_test(1) # 释放物品测试 \ No newline at end of file diff --git a/zoo/demo/相机操作.py b/zoo/demo/相机操作.py deleted file mode 100644 index 42e400c..0000000 --- a/zoo/demo/相机操作.py +++ /dev/null @@ -1,49 +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 get_camera(part, scene_id=0): - action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id) - return sim_client.Capture(action) - -def show_image(img_data): - im = img_data.images[0] - d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels)) - plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None) - plt.show() - -def camera_test(scene_id=0): - for camera_name in [GrabSim_pb2.CameraName.Head_Color, GrabSim_pb2.CameraName.Head_Depth, GrabSim_pb2.CameraName.Head_Segment]: - img_data = get_camera([camera_name], scene_id) - show_image(img_data) - -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) - - for i in range(scene_num): - print("------------------", i, "----------------------") - camera_test(i) # 相机操作测试 \ No newline at end of file diff --git a/zoo/demo/行人控制.py b/zoo/demo/行人控制.py deleted file mode 100644 index 32efcb2..0000000 --- a/zoo/demo/行人控制.py +++ /dev/null @@ -1,86 +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 add_walker(scene_id=0): - """pose:表示行人的初始位置姿态""" - print('------------------add walker----------------------') - s = sim_client.Observe(GrabSim_pb2.SceneID(value=0)) - - walker_loc = [[0, 880], [250, 1200], [-55, 750], [70, -200]] - walker_list = [] - for i in range(len(walker_loc)): - loc = walker_loc[i] + [0, 600, 100] - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc) - scene = sim_client.Do(action) - print(scene.info) - # 只有可达的位置才能成功初始化行人，显示unreachable的地方不能初始化行人 - walker_list.append(GrabSim_pb2.WalkerList.Walker(id=i, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=90))) - - scene = sim_client.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=scene_id)) - return scene - -def control_walkers(scene_id=0): - """pose:表示行人的终止位置姿态""" - s = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - - walker_loc = [[-55, 750], [70, -200], [250, 1200], [0, 880]] - controls = [] - for i in range(len(s.walkers)): - loc = walker_loc[i] - is_autowalk = False - pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180) - controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=200, pose=pose)) - scene = sim_client.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=scene_id)) - time.sleep(10) - return scene - -def remove_walkers(scene_id=0): - s = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - scene = sim_client.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=[1, 3], scene=scene_id)) - time.sleep(2) - return - -def clean_walkers(scene_id=0): - scene = sim_client.CleanWalkers(GrabSim_pb2.SceneID(value=scene_id)) - return scene - -def walker_test(scene_id=0): - add_walker(scene_id) - control_walkers(scene_id) - print(sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) - remove_walkers(scene_id) - print(sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) - clean_walkers(scene_id) - return - -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, "----------------------") - walker_test(i) # 行人控制测试 \ No newline at end of file diff --git a/zoo/install_pytorch.sh b/zoo/install_pytorch.sh deleted file mode 100644 index a2b5b5c..0000000 --- a/zoo/install_pytorch.sh +++ /dev/null @@ -1 +0,0 @@ -conda install pytorch==1.11.0 torchvision==0.12.0 torchaudio==0.11.0 cudatoolkit=11.3 -c pytorch \ No newline at end of file diff --git a/zoo/navigator_DstarLite/__init__.py b/zoo/navigator_DstarLite/__init__.py deleted file mode 100644 index 1ceb32a..0000000 --- a/zoo/navigator_DstarLite/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ - -from . import navigate -from . import dstar_lite - diff --git a/zoo/navigator_DstarLite/dstar_lite.py b/zoo/navigator_DstarLite/dstar_lite.py deleted file mode 100644 index 6a83108..0000000 --- a/zoo/navigator_DstarLite/dstar_lite.py +++ /dev/null @@ -1,510 +0,0 @@ -''' -基于两个D* lite的实现 -按照原始算法的伪代码 -自己写的D* lite -''' - -import math -import queue -from functools import partial -import numpy as np -import heapq - -from matplotlib import pyplot as plt - - -def diagonal_distance(start, end): # - return max(abs(start[0] - end[0]), abs(start[1] - end[1])) - - -def manhattan_distance(start, end): # 曼哈顿距离 - return abs(start[0] - end[0]) + abs(start[1] - end[1]) - - -def euclidean_distance(start, end): # 欧式距离 - # return np.linalg.norm(start-end) - return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2) - - -def heuristic(start, end, name='euclidean'): - if name == 'diagonal': - return diagonal_distance(start, end) - elif name == 'euclidean': - return euclidean_distance(start, end) - elif name == 'manhattan': - return manhattan_distance(start, end) - else: - raise Exception('Error heuristic name!') - - -class Priority: - ''' - 优先级类 - ''' - - def __init__(self, k1, k2): - self.k1 = k1 - self.k2 = k2 - - def __lt__(self, other): # 定义对象的 < 运算 - return self.k1 < other.k1 or (self.k1 == other.k1 and self.k2 < other.k2) - - def __le__(self, other): # 定于对象的 <= 运算 - return self.k1 < other.k1 or (self.k1 == other.k1 and self.k2 <= other.k2) - - -class Node: - ''' - 节点类 - ''' - - def __init__(self, pos: (int, int), priority: Priority): - self.pos = pos # X Y - self.priority = priority - - def __lt__(self, other): # 定义对象的 < 运算 - return self.priority < other.priority - - def __le__(self, other): # 定于对象的 <= 运算 - return self.priority <= other.priority - - -class PriorityQueue: - def __init__(self): - self.queue = [] # 节点队列 - self.nodes = [] # 节点位置列表 - - def is_empty(self): - # 队列判空 - return len(self.queue) == 0 - - def top(self): - return self.queue[0].pos - - def top_key(self): - if self.is_empty(): - return Priority(float('inf'), float('inf')) - return self.queue[0].priority - - def pop(self): - # 取出第一个节点 - node = heapq.heappop(self.queue) - self.nodes.remove(node.pos) - return node - - def insert(self, pos, priority): - # 创建并添加节点 - node = Node(pos, priority) - heapq.heappush(self.queue, node) - self.nodes.append(pos) - - def remove(self, pos): - # 删除指定节点并重新排序 - self.queue = [n for n in self.queue if n.pos != pos] - heapq.heapify(self.queue) # 重排序 - self.nodes.remove(pos) - - def update(self, pos, priority): - # 更新指定位置的priority值 - for n in self.queue: - if n.pos == pos: - n.priority = priority - break - - -class DStarLite: - def __init__(self, - map: np.array([int, int]), # [X, Y] - area_range, # [x_min, x_max, y_min, y_max] 实际坐标范围 - scale_ratio=5, # 地图缩放率 - dyna_obs_radius=20 # dyna_obs实际身位半径 - ): - - # self.area_bounds = area - self.map = map - self.background = map - self.X = map.shape[0] - self.Y = map.shape[1] - (self.x_min, self.x_max, self.y_min, self.y_max) = area_range - self.scale_ratio = scale_ratio - self.dyna_obs_list = [] # 动态障碍物位置列表( 当前地图 ) [(x, y)] - self.dyna_obs_radius = math.ceil(dyna_obs_radius/scale_ratio) # dyna_obs缩放后身位半径 - - # free:0, obs:1, dyna_obs:2 - self.idx_to_object = { - 0: "free", - 1: "obstacle", - 2: "dynamic obstacle" - } - self.object_to_idx = dict(zip(self.idx_to_object.values(), self.idx_to_object.keys())) - self.object_to_cost = { - "free": 0, - "obstacle": float('inf'), - "dynamic obstacle": 50 - } - - self.compute_cost_map() - - self.s_start = None # (int,int) 必须是元组(元组可以直接当作矩阵索引) - self.s_goal = None # (int,int) - self.s_last = None # (int,int) - self.U = PriorityQueue() - self.k_m = 0 - self.rhs = np.ones((self.X, self.Y)) * np.inf - self.g = self.rhs.copy() - self.path = [] - - def set_map(self, map_): - ''' - 设置map 和 cost_map - ''' - self.map = map_ - self.X = map_.shape[0] - self.Y = map_.shape[1] - self.compute_cost_map() - - def reset(self): - ''' - (完成一次导航后) - 重置 1.环境变量 - 2.dstar_lite变量 - ''' - # env reset - self.map = self.background.copy() - self.compute_cost_map() - self.dyna_obs_list.clear() - self.path.clear() - # dstar_lite reset - self.s_start = None - self.s_goal = None - self.s_last = None - self.U = PriorityQueue() - self.k_m = 0 - self.rhs = np.ones((self.X, self.Y)) * np.inf - self.g = self.rhs.copy() - - def calculate_key(self, s: (int, int)): - ''' - 计算 位置s 的 key1, key2 - :returns： - Priority(k1, k2): 可比较的对象 - ''' - k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start, s) + self.k_m - k2 = min(self.g[s], self.rhs[s]) - return Priority(k1, k2) - - def c(self, u: (int, int), v: (int, int), v_old=None) -> float: - ''' - 计算节点间的 路径代价 和 目标位置代价 (目标位置代价为0时采用路径代价) - (因为是终点->起点的扩展方向，因此v是node，u是v扩展的neighbor) - Args: - u: from pos - v: to pos - v_old: 指定的v的类型 - ''' - if v_old: - obj_cost = self.object_to_cost[v_old] - else: - obj_cost = self.cost_map[v] - if obj_cost > 0: - return obj_cost - return heuristic(u, v) - - def contain(self, u: (int, int)): - ''' - 判断 节点u 是否在队列中 - ''' - return u in self.U.nodes - - def update_vertex(self, u: (int, int)): - ''' - 判定节点状态, 更新队列 - 不一致且在队列 --> 更新key - 不一致且不在队列 --> 计算key并加入队列 - 一致且在队列 --> 移出队列 - ''' - if self.g[u] != self.rhs[u] and self.contain(u): - self.U.update(u, self.calculate_key(u)) - elif self.g[u] != self.rhs[u] and not self.contain(u): - self.U.insert(u, self.calculate_key(u)) - elif self.g[u] == self.rhs[u] and self.contain(u): - self.U.remove(u) - - def compute_shortest_path(self): - ''' - 计算最短路径 - ''' - while self.U.top_key() < self.calculate_key(self.s_start) or self.rhs[self.s_start] > self.g[self.s_start]: - u = self.U.top() - k_old = self.U.top_key() - k_new = self.calculate_key(u) - if k_old < k_new: - self.U.update(u, k_new) - elif self.g[u] > self.rhs[u]: # 过一致 - self.g[u] = self.rhs[u] - self.U.remove(u) - pred = self.get_neighbors(u) - for s in pred: - if s != self.s_goal: - self.rhs[s] = min(self.rhs[s], self.c(s, u) + self.g[u]) - self.update_vertex(s) - else: # 欠一致 - g_old = self.g[u] - self.g[u] = float('inf') - pred = self.get_neighbors(u) - for s in pred + [u]: - if self.rhs[s] == self.c(s, u) + g_old: - if s != self.s_goal: - succ = self.get_neighbors(s) - self.rhs[s] = min([self.c(s, s_) + self.g[s_] for s_ in succ]) - self.update_vertex(s) - - def _planning(self, s_start, s_goal, dyna_obs, step_num=None, debug=False): - ''' - 规划路径(实际实现) - Args: - dyna_obs: 动态障碍物位置列表 - step_num: 单次移动步数 - ''' - # 确保目标合法 - if not self.in_bounds_without_obstacle(s_goal): - return None - # 第一次规划需要初始化rhs并将goal加入队列，计算最短路径 - if self.s_goal is None: - self.s_start = tuple(s_start) - self.s_goal = tuple(s_goal) - self.s_last = tuple(s_start) - self.rhs[tuple(s_goal)] = 0 - self.U.insert(tuple(s_goal), Priority(k1=heuristic(tuple(s_start), tuple(s_goal)), k2=0)) - self.compute_shortest_path() # 计算最短路径 - self.path = self.get_path() - # 后续规划只更新起点，直接使用原路径(去掉已走过部分) - else: - self.s_start = tuple(s_start) - self.path = self.path[step_num:] - # 根据obs更新map, cost_map, edge_cost - changed_pos = self.update_map(dyna_obs=dyna_obs) - if changed_pos: - self.update_cost_map(changed_pos) - self.update_edge_costs(changed_pos) - # 若地图改变，重新计算最短路径 - self.compute_shortest_path() - self.path = self.get_path() - - # TODO: 误差抖动使robot没有到达路径上的点，导致新起点的rhs=∞，可能导致get_path失败 ( 当前版本没有该问题 ) - # assert (self.rhs[self.s_start] != float('inf')), "There is no known path!" - # self.path = self.get_path(step_num) - # # debug - # if debug: - # pass - return self.path - - def planning(self, s_start, s_goal, dyna_obs, step_num=None, debug=False): - ''' - 路径规划(供外部调用，处理实际坐标和地图坐标的转换) - ''' - # 实际坐标 -> 地图坐标 - s_start = self.real2map(s_start) - s_goal = self.real2map(s_goal) - for i in range(len(dyna_obs)): - dyna_obs[i] = self.real2map(dyna_obs[i]) - - self._planning(s_start, s_goal, dyna_obs, step_num, debug) - - # 地图坐标->实际坐标 - path = [self.map2real(node) for node in self.path] - return path - - def get_path(self): - ''' - 得到路径 - Args: - step_num: 路径步数 (None表示返回完整路径) - return: - path: [(x, y), ...] - ''' - if self.s_start is None or self.s_goal == self.s_start: - return [] - path = [] - cur = self.s_start - # if step_num is None: # 得到完整路径 - while cur != self.s_goal: - succ = self.get_neighbors(cur) - cur = succ[np.argmin([self.c(cur, s_) + self.g[s_] for s_ in succ])] - path.append(cur) - # else: - # for i in range(step_num): - # if cur == self.s_goal: - # break - # succ = self.get_neighbors(cur) - # cur = succ[np.argmin([self.c(cur, s_) + self.g[s_] for s_ in succ])] - # path.append(cur) - return path - - def in_bounds_without_obstacle(self, pos): - ''' - 判断位置在地图范围内 且 不是静态障碍物 - ''' - (x, y) = pos - return 0 <= x < self.X and 0 <= y < self.Y and self.map[x, y] != self.object_to_idx["obstacle"] - - def get_neighbors(self, pos, mode=8): - ''' - 获取邻居节点, 地图范围内 - ''' - (x_, y_) = pos - # results = [(x_+1,y_), (x_-1,y_), (x_, y_+1), (x_,y_-1)] - # if mode == 8: - neighbors = [(x_ + 1, y_), (x_ - 1, y_), (x_, y_ + 1), (x_, y_ - 1), (x_ + 1, y_ + 1), (x_ - 1, y_ + 1), - (x_ + 1, y_ - 1), (x_ - 1, y_ - 1)] - neighbors = filter(self.in_bounds_without_obstacle, neighbors) # 确保位置在地图范围内 且 不是静态障碍物 - return list(neighbors) - - def compute_cost_map(self): - # 计算当前地图的cost_map - self.cost_map = np.zeros(self.map.shape) - for idx, obj in self.idx_to_object.items(): - self.cost_map[self.map == idx] = self.object_to_cost[obj] - - def update_map(self, dyna_obs): - ''' - 更新地图 和 动态障碍物列表 - Args: - dyna_obs: 当前动态障碍物位置列表 [(x,y), ...] - return: - update_obj: 改变的位置列表 [(x, y, obj_idx, obj_idx_old), ...] - ''' - # dyna_obs没有变化 (集合set可以忽略元素在列表中的位置) - if set(dyna_obs) == set(self.dyna_obs_list): - return [] - - # 新旧dyna_obs占用位置列表 - old_obs_occupy = [] - new_obs_occupy = [] - for pos in self.dyna_obs_list: - old_obs_occupy.extend(self.get_occupy_pos(pos)) - for pos in dyna_obs: - new_obs_occupy.extend(self.get_occupy_pos(pos)) - old_obs_occupy = [pos for i, pos in enumerate(old_obs_occupy) if pos not in old_obs_occupy[:i]] # 去除重复位置 - new_obs_occupy = [pos for i, pos in enumerate(new_obs_occupy) if pos not in new_obs_occupy[:i]] # 去除重复位置 - - # 转变为free 和 转变为obs的位置列表 - changed_free = [pos for pos in old_obs_occupy if pos not in new_obs_occupy] - changed_obs = [pos for pos in new_obs_occupy if pos not in old_obs_occupy] - - # 更新地图，计算changed_pos - changed_pos = [] - for (x, y) in changed_free: - self.map[x, y] = self.object_to_idx['free'] - changed_pos.append((x, y, self.object_to_idx["free"], self.object_to_idx["dynamic obstacle"])) - for (x, y) in changed_obs: - self.map[x, y] = self.object_to_idx['dynamic obstacle'] - changed_pos.append((x, y, self.object_to_idx["dynamic obstacle"], self.object_to_idx["free"])) - - # 更新动态障碍物列表 - self.dyna_obs_list = dyna_obs - - return changed_pos - - def get_occupy_pos(self, obs_pos): - ''' - 根据dyna_obs中心位置，计算其占用的所有网格位置 - ''' - (x, y) = obs_pos - occupy_pos = [] - for i in range(x-self.dyna_obs_radius, x+self.dyna_obs_radius+1): - for j in range(y-self.dyna_obs_radius, y+self.dyna_obs_radius+1): - occupy_pos.append((i, j)) - occupy_pos = filter(self.in_bounds_without_obstacle, occupy_pos) # 确保位置在地图范围内 且 不是静态障碍物 - return list(occupy_pos) - - def update_cost_map(self, changed_pos): - ''' - 更新cost_map - Args: - changed_pos: 改变的位置列表 [x, y, idx] - ''' - for (x, y, idx, _) in changed_pos: - self.cost_map[x, y] = self.object_to_cost[self.idx_to_object[idx]] - - def update_edge_costs(self, changed_pos): - ''' - 重新计算edge_cost，更新受影响节点的rhs - Args: - changed_pos: 改变的位置列表 [(x, y, obj_idx_new, obj_idx_old)] - ''' - if not changed_pos: return - self.k_m += heuristic(self.s_last, self.s_start, name='euclidean') # 使用欧式距离 更新km - self.s_last = self.s_start - for pos in changed_pos: - v = (pos[0], pos[1]) # to pos - v_old = self.idx_to_object[pos[3]] # 位置v的旧类型 - pred_v = self.get_neighbors(v) - for u in pred_v: - c_old = self.c(u, v, v_old=v_old) - c_new = self.c(u, v) - if c_old > c_new and u != self.s_goal: - self.rhs[u] = min(self.rhs[u], self.c(u, v) + self.g[v]) - elif self.rhs[u] == c_old + self.g[v] and u != self.s_goal: - succ_u = self.get_neighbors(u) - self.rhs[u] = min([self.c(u, s_) + self.g[s_] for s_ in succ_u]) - self.update_vertex(u) - - def map2real(self, pos): - ''' - 地图坐标->实际坐标 - ''' - x = pos[0] * self.scale_ratio + self.x_min - y = pos[1] * self.scale_ratio + self.y_min - return tuple((x, y)) - - def real2map(self, pos, approximation='round'): - ''' - 实际坐标->地图坐标 - ''' - if approximation == 'round': # 四舍五入 - x = round((pos[0] - self.x_min) / self.scale_ratio) - y = round((pos[1] - self.y_min) / self.scale_ratio) - elif approximation == 'low': # 向下取整 - x = math.floor((pos[0] - self.x_min) / self.scale_ratio) - y = math.floor((pos[1] - self.y_min) / self.scale_ratio) - else: - raise Exception("Wrong approximation!") - return tuple((x, y)) - - def draw_graph(self, step_num): - # 清空当前figure内容，保留figure对象 - plt.clf() - # for stopping simulation with the esc key. 按esc结束 - plt.gcf().canvas.mpl_connect( - 'key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - - # 缩放坐标偏移量 - offset = (self.x_min / self.scale_ratio, self.x_max / self.scale_ratio, - self.y_min / self.scale_ratio, self.y_max / self.scale_ratio,) - - # 画地图: X行Y列，第一行在下面 - # 范围: 横向Y[-80,290] 纵向X[-70,120] - plt.imshow(self.map, cmap='binary', alpha=0.5, origin='lower', - extent=(offset[2], offset[3], - offset[0], offset[1])) - # 画起点和目标 - plt.plot(self.s_start[1] + offset[2], self.s_start[0] + offset[0], "xr") - plt.plot(self.s_goal[1] + offset[2], self.s_goal[0] + offset[0], "xr") - - # 画搜索路径 - plt.plot([y + offset[2] for (x, y) in self.path], - [x + offset[0] for (x, y) in self.path], "-g") - - # 画移动路径 - next_step = min(step_num, len(self.path)) - # plt.plot([self.s_start[1] + offset[2], self.path[next_step-1][1] + offset[2]], - # [self.s_start[0] + offset[0], self.path[next_step-1][0] + offset[0]], "-r") - plt.plot([y + offset[2] for (x, y) in self.path[:next_step]], - [x + offset[0] for (x, y) in self.path[:next_step]], "-r") - - plt.xlabel('y', loc='right') - plt.ylabel('x', loc='top') - plt.grid(color='black', linestyle='-', linewidth=0.5) - plt.pause(0.3) diff --git a/zoo/navigator_DstarLite/map_5.pkl b/zoo/navigator_DstarLite/map_5.pkl deleted file mode 100644 index 7b49370..0000000 Binary files a/zoo/navigator_DstarLite/map_5.pkl and /dev/null differ diff --git a/zoo/navigator_DstarLite/navigate.py b/zoo/navigator_DstarLite/navigate.py deleted file mode 100644 index bbb0a16..0000000 --- a/zoo/navigator_DstarLite/navigate.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -import math -import sys -import time - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable -from scene_utils import Scene - - -from dstar_lite import DStarLite - -class Navigator: - ''' - 导航类 - ''' - - def __init__(self, scene, area_range, map, scale_ratio=5): - self.scene = scene - self.area_range = area_range # 地图实际坐标范围 xmin, xmax, ymin, ymax - self.map = map # 缩放并离散化的地图 array(X,Y) - self.scale_ratio = scale_ratio # 地图缩放率 - self.step_length = 50 # 步长(单次移动) - self.step_num = self.step_length // self.scale_ratio # 单次移动地图格数 - self.v = 200 # 速度 - self.step_time = self.step_length/self.v + 0.1 # 单步移动时长 - - - self.planner = DStarLite(area_range=area_range, map=map, scale_ratio=scale_ratio) - - - @staticmethod - def is_reached(pos: np.array((float, float)), goal: np.array((float, float)), dis_limit=25): - ''' - 判断是否到达目标 - ''' - dis = np.linalg.norm(pos - goal) - return dis < dis_limit - - def reset_goal(self, goal:(float, float)): - # TODO: 使目标可达 - # 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点 - # 目标在地图外面：起点和目标连线最靠近目标的可行点 - pass - - def navigate(self, goal: (float, float), animation=True): - ''' - 单次导航，直到到达目标 - ''' - - pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) # 机器人当前: 位置 和 朝向 - yaw = self.scene.status.rotation.Yaw - print('------------------navigation_start----------------------') - while not self.is_reached(pos, goal): - dyna_obs = [(walker.pose.X, walker.pose.Y) for walker in self.scene.status.walkers] # 动态障碍物(顾客)位置列表 - path = self.planner.planning(pos, goal, dyna_obs, step_num=self.step_num) - if path: - next_step = min(self.step_num, len(path)) - (next_x, next_y) = path[next_step-1] - print('plan pos:', (next_x, next_y), end=' ') - self.scene.walk_to(next_x, next_y, yaw, velocity=self.v) - if animation: - self.planner.draw_graph(self.step_num) # 画出搜索路径 - time.sleep(self.step_time) - pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) - print('reach pos:', pos) - - self.planner.reset() # 完成一轮导航，重置变量 - - if self.is_reached(pos, goal): - print('The robot has achieved goal !!') - - - - - diff --git a/zoo/navigator_DstarLite/readme.md b/zoo/navigator_DstarLite/readme.md deleted file mode 100644 index 3a77674..0000000 --- a/zoo/navigator_DstarLite/readme.md +++ /dev/null @@ -1,4 +0,0 @@ -### dstar_lite.py ——Dstar lite算法文件 -### navigate.py ——导航类 -### test.py ——测试文件 -### map_5.pkl ——离散化地图文件 \ No newline at end of file diff --git a/zoo/navigator_DstarLite/test.py b/zoo/navigator_DstarLite/test.py deleted file mode 100644 index 66ef1e7..0000000 --- a/zoo/navigator_DstarLite/test.py +++ /dev/null @@ -1,72 +0,0 @@ -import os -import pickle -import time -import random - -import matplotlib.pyplot as plt -import numpy as np - -from scene_utils import Scene # TODO: 文件名改成Scene.py -from navigate import Navigator - - - - -if __name__ == '__main__': - - file_name = 'map_5.pkl' - if os.path.exists(file_name): - with open(file_name, 'rb') as file: - map = pickle.load(file) - - Scene.init_world(1, 3) - scene = Scene.Scene(sceneID=0) - - navigator = Navigator(scene=scene, 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到达指定目标''' - 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=False, speed=20, X=-50, Y=600, Yaw=0)]) - scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=20, X=100, Y=150, Yaw=0)]) - goal = np.array((-100, 700)) - - '''场景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) - - scene.clean_walker() - print(scene.status.collision) # TODO: 不显示碰撞信息 ??? - - - - - - - - - - - - - - diff --git a/zoo/opt_bt_expansion/BehaviorTree.py b/zoo/opt_bt_expansion/BehaviorTree.py deleted file mode 100644 index 4513a20..0000000 --- a/zoo/opt_bt_expansion/BehaviorTree.py +++ /dev/null @@ -1,95 +0,0 @@ - -#叶结点 -class Leaf: - def __init__(self,type,content,mincost): - self.type=type - self.content=content #conditionset or action - self.parent=None - self.parent_index=0 - self.mincost=mincost - - # tick 叶节点，返回返回值以及对应的条件或行动对象self.content - def tick(self,state): - if self.type=='cond': - if self.content <= state: - return 'success',self.content - else: - return 'failure',self.content - if self.type=='act': - if self.content.pre<=state: - return 'running',self.content #action - else: - return 'failure',self.content - - def __str__(self): - print( self.content) - return '' - - def print_nodes(self): - print(self.content) - - def count_size(self): - return 1 - - -#可能包含控制结点的行为树 -class ControlBT: - def __init__(self,type): - self.type=type - self.children=[] - self.parent=None - self.parent_index=0 - - - def add_child(self,subtree_list): - for subtree in subtree_list: - self.children.append(subtree) - subtree.parent=self - subtree.parent_index=len(self.children)-1 - - # tick行为树，根据不同控制结点逻辑tick子结点 - def tick(self,state): - if len(self.children) < 1: - print("error,no child") - if self.type =='?':#选择结点，即或结点 - for child in self.children: - val,obj=child.tick(state) - if val=='success': - return val,obj - if val=='running': - return val,obj - return 'failure','?fails' - if self.type =='>':#顺序结点，即与结点 - for child in self.children: - val,obj=child.tick(state) - if val=='failure': - return val,obj - if val=='running': - return val,obj - return 'success', '>success' - if self.type =='act':#行动结点 - return self.children[0].tick(state) - if self.type =='cond':#条件结点 - return self.children[0].tick(state) - - def getFirstChild(self): - return self.children[0] - - def __str__(self): - print(self.type+'\n') - for child in self.children: - print (child) - return '' - - def print_nodes(self): - print(self.type) - for child in self.children: - child.print_nodes() - - # 递归统计树中结点数 - def count_size(self): - result=1 - for child in self.children: - result+= child.count_size() - return result - diff --git a/zoo/opt_bt_expansion/MakeCoffee.ptml b/zoo/opt_bt_expansion/MakeCoffee.ptml deleted file mode 100644 index 40fc6a4..0000000 --- a/zoo/opt_bt_expansion/MakeCoffee.ptml +++ /dev/null @@ -1,59 +0,0 @@ -selector{ -cond At(Table,Coffee) -selector{ -cond At(Robot,Table), Holding(Coffee) -act PutDown(Table,Coffee) -} -selector{ -cond At(Robot,Coffee), NotHolding, At(Robot,Table) -act PickUp(Coffee) -} -selector{ -cond Available(Table), Holding(Coffee) -act MoveTo(Table) -} -selector{ -cond At(Robot,Coffee), At(Robot,Table), Holding(VacuumCup) -act PutDown(Table,VacuumCup) -} -selector{ -cond At(Robot,CoffeeMachine), NotHolding, At(Robot,Table) -act OpCoffeeMachine -} -selector{ -cond At(Robot,Coffee), Available(Table), NotHolding -act PickUp(Coffee) -} -selector{ -cond At(Robot,CoffeeMachine), At(Robot,Table), Holding(VacuumCup) -act PutDown(Table,VacuumCup) -} -selector{ -cond Available(Table), Available(Coffee), NotHolding -act MoveTo(Coffee) -} -selector{ -cond Available(Table), At(Robot,CoffeeMachine), NotHolding -act OpCoffeeMachine -} -selector{ -cond Available(Table), Available(Coffee), At(Robot,Table), Holding(VacuumCup) -act PutDown(Table,VacuumCup) -} -selector{ -cond Available(Table), NotHolding, Available(CoffeeMachine) -act MoveTo(CoffeeMachine) -} -selector{ -cond Available(Table), Available(Coffee), Holding(VacuumCup) -act MoveTo(Table) -} -selector{ -cond Available(Table), Available(CoffeeMachine), At(Robot,Table), Holding(VacuumCup) -act PutDown(Table,VacuumCup) -} -selector{ -cond Available(Table), Available(CoffeeMachine), Holding(VacuumCup) -act MoveTo(Table) -} -} diff --git a/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py b/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py deleted file mode 100644 index b886f8d..0000000 --- a/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py +++ /dev/null @@ -1,210 +0,0 @@ -import copy -import random -from robowaiter.behavior_tree.obtea.BehaviorTree import Leaf,ControlBT - -class CondActPair: - def __init__(self, cond_leaf,act_leaf): - self.cond_leaf = cond_leaf - self.act_leaf = act_leaf - -#定义行动类，行动包括前提、增加和删除影响 -class Action: - def __init__(self,name='anonymous action',pre=set(),add=set(),del_set=set(),cost=1): - self.pre=copy.deepcopy(pre) - self.add=copy.deepcopy(add) - self.del_set=copy.deepcopy(del_set) - self.name=name - self.cost=cost - - def __str__(self): - return self.name - -#生成随机状态 -def generate_random_state(num): - result = set() - for i in range(0,num): - if random.random()>0.5: - result.add(i) - return result -#从状态和行动生成后继状态 -def state_transition(state,action): - if not action.pre <= state: - print ('error: action not applicable') - return state - new_state=(state | action.add) - action.del_set - return new_state - - -#本文所提出的完备规划算法 -class OptBTExpAlgorithm: - def __init__(self,verbose=False): - self.bt = None - self.nodes=[] - self.traversed=[] - self.mounted=[] - self.conditions=[] - self.conditions_index=[] - self.verbose=verbose - - def clear(self): - self.bt = None - self.nodes = [] - self.traversed = [] - self.conditions = [] - self.conditions_index = [] - - #运行规划算法，从初始状态、目标状态和可用行动，计算行为树self.bt - def run_algorithm(self,goal,actions): - if self.verbose: - print("\n算法开始！") - - - self.bt = ControlBT(type='cond') - # 初始行为树只包含目标条件 - gc_node = Leaf(type='cond', content=goal,mincost=0) # 为了统一，都成对出现 - ga_node = Leaf(type='act', content=None, mincost=0) - subtree = ControlBT(type='?') - subtree.add_child([copy.deepcopy(gc_node)]) # 子树首先保留所扩展结 - self.bt.add_child([subtree]) - - # self.conditions.append(goal) - cond_anc_pair = CondActPair(cond_leaf=gc_node,act_leaf=ga_node) - 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 - pair_node = None - min_cost = float ('inf') - index= -1 - for i,cond_anc_pair in enumerate(self.nodes): - if cond_anc_pair.cond_leaf.mincost < min_cost: - min_cost = cond_anc_pair.cond_leaf.mincost - pair_node = copy.deepcopy(cond_anc_pair) - index = i - break - - if self.verbose: - print("选择扩展条件结点：",pair_node.cond_leaf.content) - # Update self.nodes and self.traversed - self.nodes.pop(index) # the set of explored but unexpanded conditions. self.nodes.remove(pair_node) - c = pair_node.cond_leaf.content # 子树所扩展结点对应的条件（一个文字的set） - - # Mount the action node and extend BT. T = Eapand(T,c,A(c)) - if c!=goal: - 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 - if self.verbose: - print("完成扩展 a_node= %s,对应的新条件 c_attr= %s,mincost=%d" \ - % (cond_anc_pair.act_leaf.content.name, cond_anc_pair.cond_leaf.content, - cond_anc_pair.cond_leaf.mincost)) - - if self.verbose: - print("遍历所有动作, 寻找符合条件的动作") - # 遍历所有动作, 寻找符合条件的动作 - current_mincost = pair_node.cond_leaf.mincost # 当前的最短路径是多少 - - for i in range(0, len(actions)): - if not c & ((actions[i].pre | actions[i].add) - actions[i].del_set) <= set(): - if (c - actions[i].del_set) == c: - if self.verbose: - print("———— 满足条件可以扩展") - c_attr = (actions[i].pre | c) - actions[i].add - - # 剪枝操作,现在的条件是以前扩展过的条件的超集 - valid = True - for j in self.traversed: # 剪枝操作 - if j <= c_attr: - valid = False - if self.verbose: - print("———— --被剪枝") - break - - if valid: - # 把符合条件的动作节点都放到列表里 - if self.verbose: - print("———— -- %s 符合条件放入列表" % actions[i].name) - c_attr_node = Leaf(type='cond', content=c_attr, mincost=current_mincost + actions[i].cost) - a_attr_node = Leaf(type='act', content=actions[i], mincost=current_mincost + actions[i].cost) - cond_anc_pair = CondActPair(cond_leaf=c_attr_node, act_leaf=a_attr_node) - self.nodes.append(copy.deepcopy(cond_anc_pair)) # condition node list - self.traversed.append(c_attr) # 重点 the set of expanded conditions - - if self.verbose: - print("算法结束！\n") - return True - - def print_solution(self): - print("========= BT ==========") # 树的bfs遍历 - nodes_ls = [] - nodes_ls.append(self.bt) - while len(nodes_ls) != 0: - parnode = nodes_ls[0] - print("Parrent:", parnode.type) - for child in parnode.children: - if isinstance(child, Leaf): - print("---- Leaf:", child.content) - elif isinstance(child, ControlBT): - print("---- ControlBT:", child.type) - nodes_ls.append(child) - print() - nodes_ls.pop(0) - print("========= BT ==========\n") - - # 返回所有能到达目标状态的初始状态 - def get_all_state_leafs(self): - state_leafs=[] - - nodes_ls = [] - nodes_ls.append(self.bt) - while len(nodes_ls) != 0: - parnode = nodes_ls[0] - for child in parnode.children: - if isinstance(child, Leaf): - if child.type == "cond": - state_leafs.append(child.content) - elif isinstance(child, ControlBT): - nodes_ls.append(child) - nodes_ls.pop(0) - - return state_leafs - - - # 树的dfs - def dfs_ptml(self,parnode): - for child in parnode.children: - if isinstance(child, Leaf): - if child.type == 'cond': - self.ptml_string += "cond " - c_set_str = ', '.join(map(str, child.content)) + "\n" - self.ptml_string += c_set_str - elif child.type == 'act': - self.ptml_string += 'act ' + child.content.name + "\n" - elif isinstance(child, ControlBT): - if parnode.type == '?': - self.ptml_string += "selector{\n" - self.dfs_ptml(parnode=child) - elif parnode.type == '>': - self.ptml_string += "sequence{\n" - self.dfs_ptml( parnode=child) - self.ptml_string += '}\n' - - - def get_ptml(self): - self.ptml_string = "selector{\n" - self.dfs_ptml(self.bt.children[0]) - self.ptml_string += '}\n' - return self.ptml_string - - - def save_ptml_file(self,file_name): - self.ptml_string = "selector{\n" - self.dfs_ptml(self.bt.children[0]) - self.ptml_string += '}\n' - with open(f'./{file_name}.ptml', 'w') as file: - file.write(self.ptml_string) - return self.ptml_string diff --git a/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png b/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png deleted file mode 100644 index 6453647..0000000 Binary files a/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png and /dev/null differ diff --git a/zoo/opt_bt_expansion/README.md b/zoo/opt_bt_expansion/README.md deleted file mode 100644 index 1ddd4a6..0000000 --- a/zoo/opt_bt_expansion/README.md +++ /dev/null @@ -1,125 +0,0 @@ - - -## 代码说明 - -### 1. `BehaviorTree.py` 实现行为树叶子结点和非叶子结点的定义 - -- **Leaf**：表示叶节点，可以是动作（`act`）或条件（`cond`）。 -- **ControlBT**：代表可能包含控制节点的行为树。它们可以是选择器（`?`）、序列（`>`）、动作节点（`act`）或条件节点（`cond`）。 -- 上述两个类都包含 `tick` 方法。 - -### 2. `OptimalBTExpansionAlgorithm.py` 实现最优行为树扩展算法 - -![image-20231103191141047](README.assets/image-20231103191141047.png) - -定义行动类 -```python -#定义行动类，行动包括前提、增加和删除影响 -class Action: - def __init__(self,name='anonymous action',pre=set(),add=set(),del_set=set(),cost=1): - self.pre=copy.deepcopy(pre) - self.add=copy.deepcopy(add) - self.del_set=copy.deepcopy(del_set) - self.name=name - self.cost=cost - - def __str__(self): - return self.name -``` - -调用算法 -```python -algo = OptBTExpAlgorithm(verbose=True) -algo.clear() -algo.run_algorithm(start, goal, actions) # 使用算法得到行为树在 algo.bt -algo.print_solution() # 打印行为树 -val, obj = algo.bt.tick(state) # 执行行为树 -algo.save_ptml_file("bt.ptml") # 保存行为树为 ptml 文件 -``` - -### 3. **`tools.py`** 实现打印数据、行为树测试等模块 - -使用方法 - -```python -print_action_data_table(goal,start,actions) # 打印所有变量 - -# 行为树鲁棒性测试，随机生成规划问题 -# 设置生成规划问题集的超参数：文字数、解深度、迭代次数 -seed=1 -literals_num=10 -depth = 10 -iters= 10 -BTTest(seed=seed,literals_num=literals_num,depth=depth,iters=iters) -``` - -### 4. `example.py` 中设计规划案例 goals, start, actions - -```python -def MoveBtoB (): - actions=[] - a = Action(name="Move(b,ab)") - a.pre={'Free(ab)','WayClear'} - a.add={'At(b,ab)'} - a.del_set= {'Free(ab)','At(b,pb)'} - a.cost = 1 - actions.append(a) - - a=Action(name="Move(s,ab)") - a.pre={'Free(ab)'} - a.add={'Free(ab)','WayClear'} - a.del_set={'Free(ab)','At(s,ps)'} - a.cost = 1 - actions.append(a) - - a=Action(name="Move(s,as)") - a.pre={'Free(as)'} - a.add={'At(s,ps)','WayClear'} - a.del_set={'Free(as)','At(s,ps)'} - a.cost = 1 - actions.append(a) - - start = {'Free(ab)','Free(as)','At(b,pb)','At(s,ps)'} - goal= {'At(b,ab)'} - return goal,start,actions -``` - -### 5. `opt_bt_exp_main.py` 为主函数，在此演示如何调用最优行为树扩展算法得到完全扩展最优行为树 - -初始化的时候：传入 actions (包含 pre,add,del,cost). -调用的时候，传入 goal 状态集合 (set类型)，返回完全最优扩展行为树的 ptml 形式 (string类型) - -```python -actions=[ - Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1) - ………… -] -algo = BTOptExpInterface(actions) - -goal = {'At(Table,Coffee)'} -ptml_string = algo.process(goal,start) -print(ptml_string) - -``` -两种检测方法，用于检测当前状态 `start` 能否到达目标状态 `goal` - -```python -# 判断初始状态能否到达目标状态 -start = {'At(Robot,Bar)', 'Holding(VacuumCup)', 'Available(Table)', 'Available(CoffeeMachine)','Available(FrontDesk)'} - -# 方法一：算法返回所有可能的初始状态，在里面看看有没有对应的初始状态 -right_bt = algo.find_all_leaf_states_contain_start(start) -if not right_bt: - print("ERROR1: The current state cannot reach the goal state!") -else: - print("Right1: The current state can reach the goal state!") - -# 方法二：预先跑一边行为树，看能否到达目标状态 -right_bt2 = algo.run_bt_from_start(goal,start) -if not right_bt2: - print("ERROR2: The current state cannot reach the goal state!") -else: - print("Right2: The current state can reach the goal state!") - -``` - diff --git a/zoo/opt_bt_expansion/__init__.py b/zoo/opt_bt_expansion/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/zoo/opt_bt_expansion/examples.py b/zoo/opt_bt_expansion/examples.py deleted file mode 100644 index 019af9b..0000000 --- a/zoo/opt_bt_expansion/examples.py +++ /dev/null @@ -1,174 +0,0 @@ - -from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action - - - -def MakeCoffee(): - actions=[ - Action(name='Put(Table,Coffee)', pre={'Holding(Coffee)','At(Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), - Action(name='Put(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), - - Action(name='Grasp(Coffee)', pre={'NotHolding','At(Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), - - Action(name='MoveTo(Table)', pre={'Exist(Table)'}, add={'At(Table)'}, del_set={'At(FrontDesk)','At(Coffee)','At(CoffeeMachine)'}, cost=1), - Action(name='MoveTo(Coffee)', pre={'Exist(Coffee)'}, add={'At(Coffee)'}, del_set={'At(FrontDesk)','At(Table)','At(CoffeeMachine)'}, cost=1), - Action(name='MoveTo(CoffeeMachine)', pre={'Exist(CoffeeMachine)'}, add={'At(CoffeeMachine)'}, del_set={'At(FrontDesk)','At(Coffee)','At(Table)'}, cost=1), - - Action(name='OpCoffeeMachine', pre={'At(CoffeeMachine)','NotHolding'}, add={'Exist(Coffee)','At(Coffee)'}, del_set=set(), cost=1), - ] - - start = {'At(FrontDesk)','Holding(VacuumCup)','Exist(Table)','Exist(CoffeeMachine)','Exist(FrontDesk)'} - goal = {'At(Table,Coffee)'} - return goal,start,actions - -# 本例子中，将 VacuumCup 放到 FrontDesk，比 MoveTo(Table) 再 Put(Table,VacuumCup) 的 cost 要小 -def MakeCoffeeCost(): - actions=[ - Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), - Action(name='PutDown(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Robot,Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), - - Action(name='PickUp(Coffee)', pre={'NotHolding','At(Robot,Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), - - Action(name='MoveTo(Table)', pre={'Available(Table)'}, add={'At(Robot,Table)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,CoffeeMachine)'}, cost=1), - Action(name='MoveTo(Coffee)', pre={'Available(Coffee)'}, add={'At(Robot,Coffee)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Table)','At(Robot,CoffeeMachine)'}, cost=1), - Action(name='MoveTo(CoffeeMachine)', pre={'Available(CoffeeMachine)'}, add={'At(Robot,CoffeeMachine)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,Table)'}, cost=1), - - Action(name='OpCoffeeMachine', pre={'At(Robot,CoffeeMachine)','NotHolding'}, add={'Available(Coffee)','At(Robot,Coffee)'}, del_set=set(), cost=1), - ] - - start = {'At(Robot,Bar)','Holding(VacuumCup)','Available(Table)','Available(CoffeeMachine)','Available(FrontDesk)'} - goal = {'At(Table,Coffee)'} - - return goal,start,actions - - -# test -def Test(): - actions=[ - Action(name='a1', pre={6}, add={0,2,4}, del_set={1,5}, cost=1), - Action(name='a2', pre=set(), add={0,1}, del_set=set(), cost=1), - Action(name='a3', pre={1,6}, add={0,2,3,5}, del_set={1,6}, cost=1), - Action(name='a4', pre={0,2,3}, add={4,5}, del_set={0,6}, cost=1), - Action(name='a5', pre={0,1,4}, add={2,3,6}, del_set={0}, cost=1), - ] - - start = {1,2,6} - goal={0,1,2,4,6} - return goal,start,actions - -# def Test(): -# actions=[ -# Action(name='a1', pre={2}, add={1}, del_set=set(), cost=1), -# Action(name='a2', pre=set(), add={1}, del_set={0,2}, cost=1), -# Action(name='a3', pre={1}, add=set(), del_set={0,2}, cost=1), -# Action(name='a4', pre=set(), add={0}, del_set=set(), cost=1), -# Action(name='a5', pre={1}, add={0,2}, del_set={1}, cost=1), -# Action(name='a6', pre={1}, add=set(), del_set={0,1,2}, cost=1), -# Action(name='a7', pre={1}, add={2}, del_set={0, 2}, cost=1), -# ] -# -# start = {1,2} -# goal={0,1} -# return goal,start,actions - - -# todo: 最原始的例子 -def MoveBtoB_num (): - actions=[] - a = Action(name='a1') - a.pre={1,4} - a.add={"c_goal"} - a.del_set={1,4} - a.cost = 1 - actions.append(a) - - a=Action(name='a2') - a.pre={1,2,3} - a.add={"c_goal"} - a.del_set={1,2,3} - a.cost = 1 - actions.append(a) - - a=Action(name='a3') - a.pre={1,2} - a.add={4} - a.del_set={2} - a.cost = 1 - actions.append(a) - - a=Action(name='a4') - a.pre={"c_start"} - a.add={1,2,3} - a.del_set={"c_start",4} - a.cost = 1 - actions.append(a) - - start = {"c_start"} - goal={"c_goal"} - return goal,start,actions - - -# todo: 最原始的例子 -def MoveBtoB (): - actions=[] - a = Action(name="Move(b,ab)") #'movebtob' - a.pre={'Free(ab)','WayClear'} #{1,2} - a.add={'At(b,ab)'} #{3} - a.del_set= {'Free(ab)','At(b,pb)'} #{1,4} - a.cost = 1 - actions.append(a) - - a=Action(name="Move(s,ab)") #'moveatob' - a.pre={'Free(ab)'} #{1} - a.add={'Free(ab)','WayClear'} #{5,2} - a.del_set={'Free(ab)','At(s,ps)'} #{1,6} - a.cost = 1 - actions.append(a) - - a=Action(name="Move(s,as)") #'moveatoa' - a.pre={'Free(as)'} #{7} - a.add={'At(s,ps)','WayClear'} #{8,2} - a.del_set={'Free(as)','At(s,ps)'} #{7,6} - a.cost = 1 - actions.append(a) - - start = {'Free(ab)','Free(as)','At(b,pb)','At(s,ps)'} #{1,7,4,6} - goal= {'At(b,ab)'} #{3} - return goal,start,actions - - -# 小蔡师兄论文里的例子 -def Cond2BelongsToCond3(): - actions=[] - a = Action(name='a1') - a.pre={1,4} - a.add={"c_goal"} - a.del_set={1,4} - a.cost = 1 - actions.append(a) - - a=Action(name='a2') - a.pre={1,2,3} - a.add={"c_goal"} - a.del_set={1,2,3} - a.cost = 100 - actions.append(a) - - a=Action(name='a3') - a.pre={1,2} - a.add={4} - a.del_set={2} - a.cost = 1 - actions.append(a) - - a=Action(name='a4') - a.pre={"c_start"} - a.add={1,2,3} - a.del_set={"c_start",4} - a.cost = 1 - actions.append(a) - - start = {"c_start"} - goal={"c_goal"} - return goal,start,actions - diff --git a/zoo/opt_bt_expansion/opt_bt_exp_main.py b/zoo/opt_bt_expansion/opt_bt_exp_main.py deleted file mode 100644 index 87329b6..0000000 --- a/zoo/opt_bt_expansion/opt_bt_exp_main.py +++ /dev/null @@ -1,118 +0,0 @@ -from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm,state_transition # 调用最优行为树扩展算法 -from opt_bt_expansion.examples import * - - -# 封装好的主接口 -class BTOptExpInterface: - def __init__(self, action_list): - """ - Initialize the BTOptExpansion with a list of actions. - :param action_list: A list of actions to be used in the behavior tree. - """ - # self.actions = [] - # for act in action_list: - # a = Action(name=act.name) - # a.pre=act['pre'] - # a.add=act['add'] - # a.del_set= act['del_set'] - # a.cost = 1 - # self.actions.append(a) - self.actions = action_list - self.has_processed = False - - def process(self, goal): - """ - Process the input sets and return a string result. - :param input_set: The set of goal states and the set of initial states. - :return: A PTML string representing the outcome of the behavior tree. - """ - self.goal = goal - self.algo = OptBTExpAlgorithm(verbose=False) - self.algo.clear() - self.algo.run_algorithm(self.goal, self.actions) # 调用算法得到行为树保存至 algo.bt - self.ptml_string = self.algo.get_ptml() - self.has_processed = True - # algo.print_solution() # print behavior tree - - return self.ptml_string - - # 方法一：查找所有初始状态是否包含当前状态 - def find_all_leaf_states_contain_start(self,start): - if not self.has_processed: - raise RuntimeError("The process method must be called before find_all_leaf_states_contain_start!") - # 返回所有能到达目标状态的初始状态 - state_leafs = self.algo.get_all_state_leafs() - for state in state_leafs: - if start >= state: - return True - return False - - # 方法二：模拟跑一遍行为树，看 start 能够通过执行一系列动作到达 goal - def run_bt_from_start(self,goal,start): - if not self.has_processed: - raise RuntimeError("The process method must be called before run_bt_from_start!") - # 检查是否能到达目标 - right_bt = True - state = start - steps = 0 - val, obj = self.algo.bt.tick(state) - while val != 'success' and val != 'failure': - state = state_transition(state, obj) - val, obj = self.algo.bt.tick(state) - if (val == 'failure'): - # print("bt fails at step", steps) - right_bt = False - steps += 1 - if not goal <= state: - # print("wrong solution", steps) - right_bt = False - else: - pass - # print("right solution", steps) - return right_bt - - - - -if __name__ == '__main__' : - - # todo: Example Cafe - # todo: Define goal, start, actions - actions=[ - Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), - Action(name='PutDown(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Robot,Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), - - Action(name='PickUp(Coffee)', pre={'NotHolding','At(Robot,Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), - - Action(name='MoveTo(Table)', pre={'Available(Table)'}, add={'At(Robot,Table)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,CoffeeMachine)'}, cost=1), - Action(name='MoveTo(Coffee)', pre={'Available(Coffee)'}, add={'At(Robot,Coffee)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Table)','At(Robot,CoffeeMachine)'}, cost=1), - Action(name='MoveTo(CoffeeMachine)', pre={'Available(CoffeeMachine)'}, add={'At(Robot,CoffeeMachine)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,Table)'}, cost=1), - - Action(name='OpCoffeeMachine', pre={'At(Robot,CoffeeMachine)','NotHolding'}, add={'Available(Coffee)','At(Robot,Coffee)'}, del_set=set(), cost=1), - ] - algo = BTOptExpInterface(actions) - - - goal = {'At(Table,Coffee)'} - ptml_string = algo.process(goal) - print(ptml_string) - - file_name = "MakeCoffee" - with open(f'./{file_name}.ptml', 'w') as file: - file.write(ptml_string) - - - # 判断初始状态能否到达目标状态 - start = {'At(Robot,Bar)', 'Holding(VacuumCup)', 'Available(Table)', 'Available(CoffeeMachine)','Available(FrontDesk)'} - # 方法一：算法返回所有可能的初始状态，在里面看看有没有对应的初始状态 - right_bt = algo.find_all_leaf_states_contain_start(start) - if not right_bt: - print("ERROR1: The current state cannot reach the goal state!") - else: - print("Right1: The current state can reach the goal state!") - # 方法二：预先跑一边行为树，看能否到达目标状态 - right_bt2 = algo.run_bt_from_start(goal,start) - if not right_bt2: - print("ERROR2: The current state cannot reach the goal state!") - else: - print("Right2: The current state can reach the goal state!") diff --git a/zoo/opt_bt_expansion/tools.py b/zoo/opt_bt_expansion/tools.py deleted file mode 100644 index c2e0695..0000000 --- a/zoo/opt_bt_expansion/tools.py +++ /dev/null @@ -1,167 +0,0 @@ - - -from tabulate import tabulate -import numpy as np -import random -from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm -import time - - -def print_action_data_table(goal,start,actions): - data = [] - for a in actions: - data.append([a.name , a.pre , a.add , a.del_set , a.cost]) - data.append(["Goal" ,goal ," " ,"Start" ,start]) - print(tabulate(data, headers=["Name", "Pre", "Add" ,"Del" ,"Cost"], tablefmt="fancy_grid")) # grid plain simple github fancy_grid - - -# 从状态随机生成一个行动 -def generate_from_state(act,state,num): - for i in range(0,num): - if i in state: - if random.random() >0.5: - act.pre.add(i) - if random.random() >0.5: - act.del_set.add(i) - continue - if random.random() > 0.5: - act.add.add(i) - continue - if random.random() >0.5: - act.del_set.add(i) - -def print_action(act): - print (act.pre) - print(act.add) - print(act.del_set) - - - -#行为树测试代码 -def BTTest(seed=1,literals_num=10,depth=10,iters=10,total_count=1000): - print("============= BT Test ==============") - random.seed(seed) - # 设置生成规划问题集的超参数：文字数、解深度、迭代次数 - literals_num=literals_num - depth = depth - iters= iters - total_tree_size = [] - total_action_num = [] - total_state_num = [] - total_steps_num=[] - #fail_count=0 - #danger_count=0 - success_count =0 - failure_count = 0 - planning_time_total = 0.0 - # 实验1000次 - for count in range (total_count): - - action_num = 1 - - # 生成一个规划问题，包括随机的状态和行动，以及目标状态 - states = [] - actions = [] - start = generate_random_state(literals_num) - state = start - states.append(state) - #print (state) - for i in range (0,depth): - a = Action() - generate_from_state(a,state,literals_num) - if not a in actions: - a.name = "a"+str(action_num) - action_num+=1 - actions.append(a) - state = state_transition(state,a) - if state in states: - pass - else: - states.append(state) - #print(state) - - goal = states[-1] - state = start - for i in range (0,iters): - a = Action() - generate_from_state(a,state,literals_num) - if not a in actions: - a.name = "a"+str(action_num) - action_num+=1 - actions.append(a) - state = state_transition(state,a) - if state in states: - pass - else: - states.append(state) - state = random.sample(states,1)[0] - - # 选择测试本文算法btalgorithm，或对比算法weakalgorithm - algo = OptBTExpAlgorithm() - #algo = Weakalgorithm() - start_time = time.time() - # print_action_data_table(goal, start, list(actions)) - if algo.run_algorithm(start, goal, actions):#运行算法，规划后行为树为algo.bt - total_tree_size.append( algo.bt.count_size()-1) - # algo.print_solution() # 打印行为树 - else: - print ("error") - end_time = time.time() - planning_time_total += (end_time-start_time) - - #开始从初始状态运行行为树，测试 - state=start - steps=0 - val, obj = algo.bt.tick(state)#tick行为树，obj为所运行的行动 - while val !='success' and val !='failure':#运行直到行为树成功或失败 - state = state_transition(state,obj) - val, obj = algo.bt.tick(state) - if(val == 'failure'): - print("bt fails at step",steps) - steps+=1 - if(steps>=500):#至多运行500步 - break - if not goal <= state:#错误解，目标条件不在执行后状态满足 - #print ("wrong solution",steps) - failure_count+=1 - - else:#正确解，满足目标条件 - #print ("right solution",steps) - success_count+=1 - total_steps_num.append(steps) - algo.clear() - total_action_num.append(len(actions)) - total_state_num.append(len(states)) - - print ("success:",success_count,"failure:",failure_count)#算法成功和失败次数 - print("Total Tree Size: mean=",np.mean(total_tree_size), "std=",np.std(total_tree_size, ddof=1))#1000次测试树大小 - print ("Total Steps Num: mean=",np.mean(total_steps_num),"std=",np.std(total_steps_num,ddof=1)) - print ("Average number of states:",np.mean(total_state_num))#1000次问题的平均状态数 - print ("Average number of actions",np.mean(total_action_num))#1000次问题的平均行动数 - print("Planning Time Total:",planning_time_total,planning_time_total/1000.0) - print("============ End BT Test ===========") - - # xiao cai - # success: 1000 failure: 0 - # Total Tree Size: mean= 35.303 std= 29.71336526001515 - # Total Steps Num: mean= 1.898 std= 0.970844240101644 - # Average number of states: 20.678 - # Average number of actions 20.0 - # Planning Time Total: 0.6280641555786133 0.0006280641555786133 - - # our start - # success: 1000 failure: 0 - # Total Tree Size: mean= 17.945 std= 12.841997192488865 - # Total Steps Num: mean= 1.785 std= 0.8120556843187752 - # Average number of states: 20.678 - # Average number of actions 20.0 - # Planning Time Total: 1.4748523235321045 0.0014748523235321046 - - # our - # success: 1000 failure: 0 - # Total Tree Size: mean= 48.764 std= 20.503626574406358 - # Total Steps Num: mean= 1.785 std= 0.8120556843187752 - # Average number of states: 20.678 - # Average number of actions 20.0 - # Planning Time Total: 3.3271877765655518 0.0033271877765655516 -