跑通简单的GQA问答

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.")
+        re = self._update()
-        return ptree.common.Status.SUCCESS
+        print(f"{self.__class__.__name__}: {re.value}")
+        return re
 
     def terminate(self, new_status: ptree.common.Status) -> None:
         return super().terminate(new_status)

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,8 +12,11 @@ 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?
+        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:

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)

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
     

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
     

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 Chatting()
-        cond CoffeeCupFound()
+    task DealChat()
-        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()
-    }
 }

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

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]

robowaiter/scene/tasks/AEM.py

@@ -11,9 +11,7 @@ class SceneAEM(Scene):
 
         # control.init_world(1, 3)
 
-    def reset(self):
+    def _reset(self):
-        self.reset_sim()
-
         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

robowaiter/scene/tasks/Auto_tasks.py

@@ -15,12 +15,12 @@ class SceneAT(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.chat_bubble("顾客说：请给我一杯咖啡")
+        pass
 
+    def _step(self):
+        pass

robowaiter/scene/tasks/GQA.py

@@ -15,14 +15,17 @@ class SceneGQA(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.chat_bubble("顾客说：123546567")
+        pass
 
+    def _step(self):
 
+        if int(self.time)% 5 == 0:
+            print("顾客说：请问你们这里有哪些咖啡")
+            self.chat_bubble('顾客说：请问你们这里有哪些咖啡')
+            self.state['chat_list'].append('请问你们这里有哪些咖啡')

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):
+    def _step(self):
-        self.chat_bubble("顾客说：请给我一杯咖啡")
+        pass
-        
-
-
-

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

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