实现顾客领路

robowaiter/behavior_lib/act/DealChat.py

@@ -16,6 +16,11 @@ class DealChat(Act):
             self.create_sub_task(sentence)
             return ptree.common.Status.RUNNING
 
+        self.scene.state["attention"]["customer"] = name
+        self.scene.state["serve_state"] = {
+            "last_chat_time": self.scene.time,
+        }
+
 
         self.chat_history += sentence + '\n'
 

robowaiter/behavior_lib/act/DelSubTree.py

@@ -9,6 +9,8 @@ class DelSubTree(Act):
         super().__init__(*args)
 
     def _update(self) -> ptree.common.Status:
+        self.scene.state["attention"] = {}
+
         sub_task_tree = self.parent
         self.scene.sub_task_seq.children.remove(sub_task_tree)
         return Status.RUNNING

robowaiter/behavior_lib/act/GreetCustomer.py

@@ -28,4 +28,6 @@ class GreetCustomer(Act):
         customer_name = self.scene.state['attention']['customer']
         self.scene.state['greeted_customers'].add(customer_name)
 
+
+
         return ptree.common.Status.RUNNING

robowaiter/behavior_lib/act/ServeCustomer.py

@@ -3,9 +3,6 @@ from robowaiter.behavior_lib._base.Act import Act
 from robowaiter.algos.navigator.navigate import Navigator
 
 class ServeCustomer(Act):
-    can_be_expanded = False
-    num_args = 0
-    valid_args = ()
 
     def __init__(self, *args):
         super().__init__(*args)
@@ -20,6 +17,8 @@ class ServeCustomer(Act):
         return info
 
     def _update(self) -> ptree.common.Status:
+        # if self.scene.time - self.scene.state["serve_state"]["last_chat_time"] > 10:
+        #     self.chat_bubble
 
         goal = Act.place_xyz_dic['Bar']
         self.scene.walk_to(goal[0]-5,goal[1], 180, 180, 0)

robowaiter/behavior_lib/cond/CustomerChatting.py

@@ -2,7 +2,7 @@ import py_trees as ptree
 from typing import Any
 from robowaiter.behavior_lib._base.Cond import Cond
 
-class Chatting(Cond):
+class CustomerChatting(Cond):
 
 
     def __init__(self):

robowaiter/behavior_lib/cond/FocusingCustomer.py

@@ -0,0 +1,16 @@
+import py_trees as ptree
+from typing import Any
+from robowaiter.behavior_lib._base.Cond import Cond
+
+class FocusingCustomer(Cond):
+
+    def __init__(self):
+        super().__init__()
+
+
+    def _update(self) -> ptree.common.Status:
+        # if self.scene.status?
+        if "customer" in self.scene.state['attention']:
+            return ptree.common.Status.SUCCESS
+        else:
+            return ptree.common.Status.FAILURE

robowaiter/behavior_lib/cond/NewCustomerComing.py

@@ -3,7 +3,7 @@ from typing import Any
 from robowaiter.behavior_lib._base.Cond import Cond
 import itertools
 
-class DetectCustomer(Cond):
+class NewCustomerComing(Cond):
     can_be_expanded = False
     num_params = 0
     valid_args = ()

robowaiter/robot/Default.ptml

@@ -2,9 +2,8 @@ selector
 {
     sequence
     {
-        cond Chatting()
+        cond CustomerChatting()
         act DealChat()
-
     }
     sequence
     {
@@ -16,7 +15,12 @@ selector
     }
     sequence
     {
-        cond DetectCustomer()
+        cond FocusingCustomer()
+        act ServeCustomer()
+    }
+    sequence
+    {
+        cond NewCustomerComing()
         selector
         {
             cond At(Robot,Bar)
@@ -24,4 +28,5 @@ selector
         }
         act GreetCustomer()
     }
+
 }

robowaiter/robot/robot.py

@@ -41,7 +41,6 @@ class Robot(object):
     def load_BT(self):
         self.bt = load_bt_from_ptml(self.scene, self.ptml_path,self.behavior_lib_path)
         sub_task_place_holder = find_node_by_name(self.bt.root,"SubTaskPlaceHolder()")
-        print(sub_task_place_holder)
         if sub_task_place_holder:
             sub_task_seq = sub_task_place_holder.parent
             sub_task_seq.children.pop()
@@ -53,7 +52,7 @@ class Robot(object):
 
     def expand_sub_task_tree(self,goal):
         if self.action_list is None:
-            print("\n--------------------")
+            print("\n\n--------------------")
             print(f"首次运行行为树扩展算法")
             self.action_list = self.collect_action_nodes()
             print(f"共收集到{len(self.action_list)}个实例化动作:")

robowaiter/scene/scene.py

@@ -69,7 +69,8 @@ class Scene:
         "customer_mem":{},
         "served_mem":{},
         "greeted_customers":set(),
-        "attention":{}
+        "attention":{},
+        "serve_state":{},
     }
     """
     status:

robowaiter/scene/tasks/VLM/VLN_greet_lead.py

@@ -32,12 +32,6 @@ class SceneVLM(Scene):
     
     def _step(self):
 
-        # self.control_walker(
-        #     [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=180)])
-        # time.sleep(3)
-        # cont = self.status.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳。"
-        # self.control_robot_action(0,3,cont)
-
 
         # 如果机器人不在 吧台
         # if "At(Robot,Bar)" not in self.state['condition_set']:
@@ -50,7 +44,7 @@ class SceneVLM(Scene):
         # if int(self.status.location.X)!=247 or  int(self.status.location.X)!=520:
             self.walker_followed = True
             self.control_walker(
-                    [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=-90)])
+                    [self.walker_control_generator(walkerID=0, autowalk=False, speed=300, X=end[0], Y=end[1], Yaw=-90)])
 
             cont = self.status.walkers[0].name+"谢谢！"
             self.control_robot_action(0,3,cont)