更新了动作节点

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.take_picture:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio,update_info_count=1)
+
+        self.scene.state["condition_set"] |= (self.info["add"])
+        self.scene.state["condition_set"] -= self.info["del_set"]
+
+
+        return Status.RUNNING

robowaiter/behavior_lib/act/Make.py

@@ -39,6 +39,9 @@ class Make(Act):
 
     def _update(self) -> ptree.common.Status:
 
+        if self.scene.take_picture:
+            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)
 

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
 
 

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
 
 

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
 
 

robowaiter/behavior_lib/act/Turn.py

@@ -74,6 +74,9 @@ class Turn(Act):
 
     def _update(self) -> ptree.common.Status:
 
+        if self.scene.take_picture:
+            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:

robowaiter/behavior_lib/cond/NewCustomer.py

@@ -16,6 +16,7 @@ class NewCustomer(Cond):
 
         if self.scene.take_picture:
             self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         # 获取customer的位置
         # bar (247.0, 520.0, 100.0)
         close_to_bar = False

robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py

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

robowaiter/scene/scene.py

@@ -78,8 +78,8 @@ class Scene:
         "sub_goal_list": [],  # 子目标列表
         "status": None,  # 仿真器中的观测信息，见下方详细解释
         "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)',
-                          'Holding(Nothing)', 'Exist(Yogurt)', 'Exist(BottledDrink)', 'On(Yogurt,Bar)',
+                          'Holding(Nothing)', 'Exist(Yogurt)', 'Exist(BottledDrink)',
-                          'On(BottledDrink,Bar)',
+                          # 'On(Yogurt,Bar)','On(BottledDrink,Bar)',
                           # 'Exist(Softdrink)', 'On(Softdrink,Table1)',
                           'Exist(VacuumCup)', 'On(VacuumCup,Table2)',
                           'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
@@ -116,7 +116,7 @@ class Scene:
 
         self.show_bubble = True
         # 图像分割
-        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))
@@ -705,10 +705,16 @@ class Scene:
             return
         print('------------------moveTo_Area----------------------')
         if op_type < 8:  # 动画控制相关任务的移动目标
+            if self.take_picture:
+                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.take_picture:
+                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.take_picture:
+                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
@@ -887,8 +893,12 @@ class Scene:
     def op_task_execute(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]):
         self.control_robot_action(0, 1, "开始" + self.op_dialog[op_type])  # 输出正在执行的任务
         if op_type < 8:
+            if self.take_picture:
+                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.take_picture:
+                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)

robowaiter/scene/tasks/CafeDailyOperations/CafeOneDay.py

@@ -35,8 +35,8 @@ class SceneVLM(Scene):
             # # # 上述准备
             (10, self.add_walker, (26, -28, -150, 90)),
             (0, self.add_walker, (10, -70, -200, -45)),
-            (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
+            (5, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
-            (10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])),
+            (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
@@ -47,7 +47,7 @@ class SceneVLM(Scene):
             (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, 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

robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py

@@ -13,7 +13,7 @@ class SceneVLM(Scene):
         super().__init__(robot)
         # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
 
-        self.scene_flag = 1
+        self.scene_flag = 2
         self.st1 = 3
         # self.st2 = self.st1 + 30
         # self.st3 = self.st2 + 65
@@ -24,58 +24,58 @@ class SceneVLM(Scene):
         self.signal_event_list = [
 
             # 场景1：带小女孩找阳光下的空位
-            (3, self.add_walker, (5, 230, 1200)),  # 0号"Girl02_C_3"
+            # (3, self.add_walker, (5, 230, 1200)),  # 0号"Girl02_C_3"
-            (1, self.control_walker, (0, False, 200, 60, 520, 0)),
+            # (1, self.control_walker, (0, False, 200, 60, 520, 0)),
-            (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
+            # (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
-            (1, self.customer_say, (0, "可以带我过去嘛？")),
+            # (1, self.customer_say, (0, "可以带我过去嘛？")),
-            (13, self.control_walker, (0, False, 50, 140, 1200, 180)),  # 小女孩站在了 BrightTable1 旁边就餐啦
+            # (13, self.control_walker, (0, False, 50, 140, 1200, 180)),  # 小女孩站在了 BrightTable1 旁边就餐啦
-
+            #
-            # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
+            # # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
-            # (3, self.add_walker, (5, 230, 1200)), # 小女孩
+            # # (3, self.add_walker, (5, 230, 1200)), # 小女孩
-            # # # 上述准备
+            # # # # 上述准备
-            (10, self.add_walker, (26, -28, -150, 90)),
+            # (10, self.add_walker, (26, -28, -150, 90)),
-            (0, self.add_walker, (10, -70, -200, -45)),
+            # (0, self.add_walker, (10, -70, -200, -45)),
-            (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
+            # (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
-            (10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])),
+            # (10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])),
-            # 20 胖胖男到了 BrightTable6
+            # # 20 胖胖男到了 BrightTable6
-            (2, self.customer_say, (1, "咖啡有哪些呢？")),  # 10
+            # (2, self.customer_say, (1, "咖啡有哪些呢？")),  # 10
-            (2, self.customer_say, (1, "来杯卡布奇诺吧。")),  # 15
+            # (2, self.customer_say, (1, "来杯卡布奇诺吧。")),  # 15
-
+            #
-            # # 场景3：有位女士要杯水和冰红茶
+            # # # 场景3：有位女士要杯水和冰红茶
-            (3, self.add_walkers,
+            # (3, self.add_walkers,
-             ([[[21, 65, 1000, -90], [32, -80, 850, 135], [1, 60, 420, 135], [29, -290, 400, 180]]])),
+            #  ([[[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.control_walker, (5, True, 50, 250, 1200, 180)),  # 设置id=4 的2小男孩随机游走红随机游走
-            (0, self.add_walker, (48, 60, 520, 0)),  # 生成他妈妈
+            # (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]]])),
+            # (0, self.add_walkers, ([[[48, 60, 520, 0], [31, 60, 600, -90], [20, 60, 680, -90], [9, 60, 760, -90]]])),
-            (43, self.customer_say, (7, "哎呦，今天这么多人，还有空位吗？")),  # 女士问
+            # (38, self.customer_say, (7, "哎呦，今天这么多人，还有空位吗？")),  # 女士问
-            (10, self.customer_say, (7, "我带着孩子呢，想要宽敞亮堂的地方。")),  # 女士问
+            # (10, self.customer_say, (7, "我带着孩子呢，想要宽敞亮堂的地方。")),  # 女士问
-            (8, self.customer_say, (7, "大厅的桌子好啊，快带我去呀！")),
+            # (8, self.customer_say, (7, "大厅的桌子好啊，快带我去呀！")),
-            (15, self.control_walker, (7, False, 50, -250, 480, 0)),  # #290, 400
+            # (15, self.control_walker, (7, False, 50, -250, 480, 0)),  # #290, 400
-            (3, self.customer_say, (7, "来杯酸奶吧。")), # (3, self.customer_say, (7, "我想来杯水，帮我孩子拿个酸奶吧。")),
+            # (3, self.customer_say, (7, "我想来杯水，帮我孩子拿个酸奶吧。")),
-            # # ### 9号灰色男 排队排着排着，不排了
+            # # # ### 9号灰色男 排队排着排着，不排了
-            (0, self.control_walker, (10, False, 100, 100, 760, 180)),
+            # (0, self.control_walker, (10, False, 100, 100, 760, 180)),
-            (0, self.control_walker, (10, True, 100, 0, 0, 180)),
+            # (0, self.control_walker, (10, True, 100, 0, 0, 180)),
-            (90, self.customer_say, (7, "谢谢你的水和酸奶！")),  # 倒水+取放酸奶 90s
+            # (90, self.customer_say, (7, "谢谢你的水和酸奶！")),  # 倒水+取放酸奶 90s
-
+            #
-
+            #
-            (10, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
+            # (10, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
-            (5, self.customer_say, (8,"你可以帮我拿来吗，我在前门的桌子前等你。")),
+            # (5, self.customer_say, (8,"你可以帮我拿来吗，我在前门的桌子前等你。")),
-            (1, self.control_walker,(8, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步
+            # (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, (8, False, 80, 240, 1000, -45)), # 红女士走到Table1前
-            (1, self.control_walker, (9, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
+            # (1, self.control_walker, (9, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
-            (2, self.control_walker, (10, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
+            # (2, self.control_walker, (10, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
-            (6, self.customer_say, (8,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
+            # (6, self.customer_say, (8,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
-            (5, self.customer_say, (8, "不用了。")),  # 红女士在Table1前
+            # (5, self.customer_say, (8, "不用了。")),  # 红女士在Table1前
-
+            #
-
+            #
-            (8, self.remove_walkers, ([[0, 7, 8]])),
+            # (8, self.remove_walkers, ([[0, 7, 8]])),
-            (3, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
+            # (3, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
-            (2, self.customer_say, (6, "好热呀！太阳也好大！")),
+            # (2, self.customer_say, (6, "好热呀！太阳也好大！")),
-            (20, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
+            # (20, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
 
 
             # # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
-            (24, self.clean_walkers, ()),
+            (3, self.clean_walkers, ()),
             (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
             (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
 

robowaiter/scene/tasks/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]]])),
             (-1, self.customer_say, (5, "感谢，这些够啦，你去忙吧。")),
             (10, self.customer_say, (6, "我想来份点心和酸奶。")),

robowaiter/scene/tasks/OT/Open_tasks_test.py

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