From b6ecd456176206314fbfcf20fb355bd129e03723 Mon Sep 17 00:00:00 2001 From: ChenXL97 <908926798@qq.com> Date: Wed, 25 Oct 2023 22:12:15 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B7=91=E9=80=9A=E7=AE=80=E5=8D=95=E7=9A=84GQ?= =?UTF-8?q?A=E9=97=AE=E7=AD=94?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/Behavior.py | 9 +- .../{IsChatting.py => Chatting.py} | 9 +- robowaiter/behavior_lib/DealChat.py | 26 +++++ robowaiter/behavior_lib/Grasp.py | 2 +- .../behavior_lib/{Move.py => MoveTo.py} | 4 +- robowaiter/robot/Default.ptml | 105 +----------------- robowaiter/robot/robot.py | 17 +++ robowaiter/scene/scene.py | 52 +++++++-- robowaiter/scene/tasks/AEM.py | 9 +- robowaiter/scene/tasks/Auto_tasks.py | 10 +- robowaiter/scene/tasks/GQA.py | 13 ++- robowaiter/scene/tasks/Open_tasks.py | 12 +- robowaiter/scene/tasks/VLM.py | 7 +- robowaiter/scene/tasks/VLN.py | 7 +- 14 files changed, 137 insertions(+), 145 deletions(-) rename robowaiter/behavior_lib/{IsChatting.py => Chatting.py} (67%) create mode 100644 robowaiter/behavior_lib/DealChat.py rename robowaiter/behavior_lib/{Move.py => MoveTo.py} (84%) diff --git a/robowaiter/behavior_lib/Behavior.py b/robowaiter/behavior_lib/Behavior.py index 9a4732f..0dc9b61 100644 --- a/robowaiter/behavior_lib/Behavior.py +++ b/robowaiter/behavior_lib/Behavior.py @@ -14,9 +14,14 @@ class Bahavior(ptree.behaviour.Behaviour): 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: - print("this is a base behavior node.") - return ptree.common.Status.SUCCESS + 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/robowaiter/behavior_lib/IsChatting.py b/robowaiter/behavior_lib/Chatting.py similarity index 67% rename from robowaiter/behavior_lib/IsChatting.py rename to robowaiter/behavior_lib/Chatting.py index 4dd0ce3..284e5c1 100644 --- a/robowaiter/behavior_lib/IsChatting.py +++ b/robowaiter/behavior_lib/Chatting.py @@ -2,7 +2,7 @@ import py_trees as ptree from typing import Any from robowaiter.behavior_lib.Behavior import Bahavior -class IsChatting(Bahavior): +class Chatting(Bahavior): def __init__(self, name: str, scene): super().__init__(name, scene) @@ -12,9 +12,12 @@ class IsChatting(Bahavior): def initialise(self) -> None: return super().initialise() - def update(self) -> ptree.common.Status: + def _update(self) -> ptree.common.Status: # if self.scene.status? - return ptree.common.Status.SUCCESS + 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/robowaiter/behavior_lib/DealChat.py b/robowaiter/behavior_lib/DealChat.py new file mode 100644 index 0000000..be8c3c2 --- /dev/null +++ b/robowaiter/behavior_lib/DealChat.py @@ -0,0 +1,26 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib.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/robowaiter/behavior_lib/Grasp.py b/robowaiter/behavior_lib/Grasp.py index 69d42fd..57c5708 100644 --- a/robowaiter/behavior_lib/Grasp.py +++ b/robowaiter/behavior_lib/Grasp.py @@ -12,7 +12,7 @@ class Grasp(ptree.behaviour.Behaviour): def initialise(self) -> None: return super().initialise() - def update(self) -> ptree.common.Status: + def _update(self) -> ptree.common.Status: print('Start checking IsChatting...') return ptree.common.Status.SUCCESS diff --git a/robowaiter/behavior_lib/Move.py b/robowaiter/behavior_lib/MoveTo.py similarity index 84% rename from robowaiter/behavior_lib/Move.py rename to robowaiter/behavior_lib/MoveTo.py index 4922b73..516e9dc 100644 --- a/robowaiter/behavior_lib/Move.py +++ b/robowaiter/behavior_lib/MoveTo.py @@ -1,7 +1,7 @@ import py_trees as ptree from typing import Any -class Move(ptree.behaviour.Behaviour): +class MoveTo(ptree.behaviour.Behaviour): def __init__(self, name: str, scene, a, b, c, d): super().__init__(name) @@ -12,7 +12,7 @@ class Move(ptree.behaviour.Behaviour): def initialise(self) -> None: return super().initialise() - def update(self) -> ptree.common.Status: + def _update(self) -> ptree.common.Status: print('Start checking IsChatting...') return ptree.common.Status.SUCCESS diff --git a/robowaiter/robot/Default.ptml b/robowaiter/robot/Default.ptml index c955552..52435e2 100644 --- a/robowaiter/robot/Default.ptml +++ b/robowaiter/robot/Default.ptml @@ -1,105 +1,4 @@ -//sequence: -// act action1() -// act action2(2, 2.3, True) -// -// parallel 2: -// act action3(int a, float b) -// act action4() - sequence{ - selector{ - cond CoffeeCupFound() - task FindCoffeeCup() - sequence{ - cond SeqTest() - task Move(1.2, 2, 2.3, True) - task Grasp() - parallel 3 { - cond Istask() - task TestTask() - } - } - } - selector{ - cond CoffeeCupGrasped() - task GraspCoffeeCup() - } - selector{ - cond DestinationAReached() - task ReachDestinationA() - } - selector{ - cond CoffeeCupPlaced() - task PlaceCoffeeCup() - } -} - - - - - - -sequence{ - cond NotAt(Coffee,Table) - selector{ - cond Holding(Coffee) - sequence{ - selector{ - cond At(Bar) - task MoveTo(Bar) - } - task Grasp(Coffee) - } - } - selector{ - cond At(Table) - task MoveTo(Table) - } - Put(Coffee) -} - - -selector{ - sequence{ - cond At(Table) - cond Holding(Coffee) - task Put(Table) - } - sequence{ - cond NotHolding(Coffee) - task MoveTo(Table) - } - sequence{ - cond At(Bar) - task Grasp(Coffee) - } - task MoveTo(Table) - - - - sequence{ - cond Coffee() - task FindCoffeeCup() - sequence{ - cond SeqTest() - task Move(1.2, 2, 2.3, True) - task Grasp() - parallel 3 { - cond Istask() - task TestTask() - } - } - } - selector{ - cond At(Table,Coffee) - task GraspCoffeeCup() - } - selector{ - cond DestinationAReached() - task ReachDestinationA() - } - selector{ - cond CoffeeCupPlaced() - task PlaceCoffeeCup() - } + cond Chatting() + task DealChat() } diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index 3cd0667..2e6d46a 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -5,16 +5,33 @@ from robowaiter.behavior_tree import load_bt_from_ptml class Robot(object): scene = None + response_frequency = 1 + def __init__(self,ptml_path,behavior_lib_path): self.ptml_path = ptml_path self.behavior_lib_path = behavior_lib_path + self.next_response_time = self.response_frequency + self.step_num = 0 + def set_scene(self,scene): self.scene = scene def load_BT(self): self.bt = load_bt_from_ptml(self.scene, self.ptml_path,self.behavior_lib_path) + def step(self): + if self.scene.time > self.next_response_time: + self.next_response_time += self.response_frequency + self.step_num += 1 + + print(f"==== step: {self.step_num}, time:{self.scene.time:f}s ======") + + self.bt.tick() + + print("\n") + + if __name__ == '__main__': pass \ No newline at end of file diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 943cfb1..f266f8c 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -62,6 +62,8 @@ class Scene: def __init__(self,robot, sceneID=0): self.sceneID = sceneID self.use_offset = True + self.start_time = time.time() + self.time = 0 # init robot robot.set_scene(self) @@ -69,9 +71,49 @@ class Scene: self.robot = robot - def run(self): + + def _reset(self): + # 场景自定义的reset pass + def _run(self): + # 场景自定义的run + pass + + def _step(self): + # 场景自定义的step + pass + + + def reset(self): + # 基类reset，默认执行仿真器初始化操作 + self.reset_sim() + + # reset state + self.state = { + "chat_list": [] + } + print("场景初始化完成") + self._reset() + + self.running = True + + def run(self): + # 基类run + + self._run() + + # 运行并由robot打印每步信息 + while True: + self.step() + + def step(self): + # 基类step，默认执行行为树tick操作 + self.time = time.time() - self.start_time + + self._step() + self.robot.step() + @property def status(self): @@ -83,16 +125,8 @@ class Scene: # reset world init_world() - # reset state - self.state = { - "chatting_list": [] - } - - def reset(self): - self.reset_sim() - def walker_control_generator(self, walkerID, autowalk, speed, X, Y, Yaw): if self.use_offset: X, Y = X + loc_offset[0], Y + loc_offset[1] diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py index 39a94c7..c70bcb0 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/robowaiter/scene/tasks/AEM.py @@ -11,9 +11,7 @@ class SceneAEM(Scene): # control.init_world(1, 3) - def reset(self): - self.reset_sim() - + def _reset(self): self.add_object(0, 570, 1600, 85.5) # type与物品编号对应，具体参考README.md self.add_object(1, 570, 1630, 85.5) self.add_object(2, 570, 1660, 85.5) @@ -22,5 +20,8 @@ class SceneAEM(Scene): # todo: 探索并获得语义地图 print(self.status.objects) # 全部的物品信息，包括名称、位置等，与获得的语义地图进行对比 - def run(self): + def _run(self): + pass + + def _step(self): pass \ No newline at end of file diff --git a/robowaiter/scene/tasks/Auto_tasks.py b/robowaiter/scene/tasks/Auto_tasks.py index dbc4c55..3a5bf43 100644 --- a/robowaiter/scene/tasks/Auto_tasks.py +++ b/robowaiter/scene/tasks/Auto_tasks.py @@ -15,12 +15,12 @@ class SceneAT(Scene): def __init__(self, robot): super().__init__(robot) - def reset(self): - self.reset_sim() - + def _reset(self): self.add_walker(1085, 2630, 220) self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) - def run(self): - self.chat_bubble("顾客说：请给我一杯咖啡") + def _run(self): + pass + def _step(self): + pass \ No newline at end of file diff --git a/robowaiter/scene/tasks/GQA.py b/robowaiter/scene/tasks/GQA.py index 0fd5ed3..288c67a 100644 --- a/robowaiter/scene/tasks/GQA.py +++ b/robowaiter/scene/tasks/GQA.py @@ -15,14 +15,17 @@ class SceneGQA(Scene): def __init__(self, robot): super().__init__(robot) - def reset(self): - self.reset_sim() - + def _reset(self): self.add_walker(1085, 2630, 220) self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) - def run(self): - self.chat_bubble("顾客说：123546567") + def _run(self): + pass + def _step(self): + if int(self.time)% 5 == 0: + print("顾客说：请问你们这里有哪些咖啡") + self.chat_bubble('顾客说：请问你们这里有哪些咖啡') + self.state['chat_list'].append('请问你们这里有哪些咖啡') diff --git a/robowaiter/scene/tasks/Open_tasks.py b/robowaiter/scene/tasks/Open_tasks.py index cdcc6c7..47a4792 100644 --- a/robowaiter/scene/tasks/Open_tasks.py +++ b/robowaiter/scene/tasks/Open_tasks.py @@ -16,16 +16,14 @@ class SceneOT(Scene): def __init__(self, robot): super().__init__(robot) - def reset(self): + def _reset(self): self.reset_sim() self.add_walker(1085, 2630, 220) self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) + def _run(self): + pass - def run(self): - self.chat_bubble("顾客说：请给我一杯咖啡") - - - - + def _step(self): + pass \ No newline at end of file diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index af5d246..bdb4871 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -11,13 +11,13 @@ class SceneVLM(Scene): def __init__(self, robot): super().__init__(robot) - def reset(self): + def _reset(self): self.reset_sim() self.add_walker(1085, 2630, 220) self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) - def run(self): + def _run(self): # 空调操作 self.walk_to(950, 1260, 90) # 没法转向？ # todo: 手臂操作 @@ -34,3 +34,6 @@ class SceneVLM(Scene): """ scene.release(1) """ + + def _step(self): + pass \ No newline at end of file diff --git a/robowaiter/scene/tasks/VLN.py b/robowaiter/scene/tasks/VLN.py index d7793d4..b2ff651 100644 --- a/robowaiter/scene/tasks/VLN.py +++ b/robowaiter/scene/tasks/VLN.py @@ -12,13 +12,13 @@ class SceneVLN(Scene): def __init__(self, robot): super().__init__(robot) - def reset(self): + def _reset(self): self.reset_sim() self.add_walker(1085, 2630, 220) self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) - def run(self): + def _run(self): # 实现单顾客领位 self.add_walker(1085, 2630, 220) self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) @@ -37,3 +37,6 @@ class SceneVLN(Scene): self.chat_bubble("请问您想喝点什么？") print(self.status.walkers) + + def _step(self): + pass \ No newline at end of file