Merge remote-tracking branch 'origin/main'

2023-11-17 20:39:55 +08:00 · 2023-11-17 20:39:55 +08:00 · da4ed7280e
parent 462287b392 68fcf72d62
commit da4ed7280e
23 changed files with 488 additions and 163 deletions
--- a/robowaiter/behavior_lib/_base/Behavior.py
+++ b/robowaiter/behavior_lib/_base/Behavior.py
@ -13,10 +13,15 @@ class Bahavior(ptree.behaviour.Behaviour):
        '''
    scene = None
    print_name_prefix = ""
-    all_place = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3'}
+    tables_for_placement = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3',"BrightTable6"}
    all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk',
                  'VacuumCup'}

+    # BrightTable5 = Table4
+    tables_for_guiding = {"QuietTable1","QuietTable2",
+                          "BrightTable1","BrightTable2","BrightTable3","BrightTable4","BrightTable5","BrightTable6"
+                          'CoffeeTable','WaterTable','Table1', 'Table2', 'Table3'}
+
    # all_place = {'Bar', 'WaterTable', 'CoffeeTable'}
    # all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'Yogurt'}
    # all_object = {'Coffee', 'Water'}
@ -29,10 +34,18 @@ class Bahavior(ptree.behaviour.Behaviour):
        'Bar2': (240.0, 40.0, 70.0),
        'WaterTable':(-70.0, 500.0, 107),
        'CoffeeTable':(250.0, 310.0, 100.0),
-        'Table1': (340.0, 900.0, 98.0),
-        # 'Table1': (345.0, 895.0, 98.0),
+        'Table1': (340.0, 900.0, 99.0),
        'Table2': (-55.0, 0.0, 107),
-        'Table3':(-55.0, 150.0, 107)
+        'Table3':(-55.0, 150.0, 107),
+        'BrightTable6': (5, -315, 116.5),
+
+        'QuietTable1':(480,1300,90),
+        'QuietTable2':(250,-240,-65),
+        'BrightTable1':(230,1200,135),
+        'BrightTable2': (65, 1000, 135),
+        'BrightTable3': (-80, 850, 135),
+        'BrightTable4': (-270, 520, 150),
+        'BrightTable5': (-270, 420, -135)
    }
    container_dic={
        'Coffee':'CoffeeCup',
--- a/robowaiter/behavior_lib/act/DealChat.py
+++ b/robowaiter/behavior_lib/act/DealChat.py
@ -20,6 +20,7 @@ class DealChat(Act):

        res_dict = ask_llm(chat)
        answer = res_dict["Answer"]
+        self.scene.chat_bubble(answer) # 机器人输出对话
        self.chat_history += answer + '\n'

        goal = res_dict["Goal"]
--- a/robowaiter/behavior_lib/act/ExploreEnv.py
+++ b/robowaiter/behavior_lib/act/ExploreEnv.py
@ -3,7 +3,8 @@ from typing import Any
 from robowaiter.behavior_lib._base.Act import Act

 class ExploreEnv(Act):
-    can_be_expanded = True
+    # can_be_expanded = True
+    can_be_expanded = False
    num_args=0
    valid_args=()

@ -20,8 +21,6 @@ class ExploreEnv(Act):

    def _update(self) -> ptree.common.Status:
        # explore algorithm
-
-
        self.scene.state["condition_set"]|= self.info["add"]

        return ptree.common.Status.RUNNING
--- a/robowaiter/behavior_lib/act/GreatCustomer.py
+++ b/robowaiter/behavior_lib/act/GreatCustomer.py
@ -0,0 +1,27 @@
+import py_trees as ptree
+from robowaiter.behavior_lib._base.Act import Act
+from robowaiter.algos.navigator.navigate import Navigator
+
+class GreatCustomer(Act):
+    can_be_expanded = False
+    num_args = 0
+    valid_args = ()
+
+    def __init__(self, *args):
+        super().__init__(*args)
+
+    @classmethod
+    def get_info(cls):
+        info = {}
+        info['pre'] = set()
+        info["add"] = set()
+        info["del_set"] = set()
+        info['cost']=0
+        return info
+
+    def _update(self) -> ptree.common.Status:
+
+        goal = Act.place_xyz_dic['Bar']
+        self.scene.walk_to(goal[0]-5,goal[1], 180, 180, 0)
+        self.scene.chat_bubble("欢迎光临！请问有什么可以帮您？")
+        return ptree.common.Status.RUNNING
--- a/robowaiter/behavior_lib/act/Make.py
+++ b/robowaiter/behavior_lib/act/Make.py
@ -34,6 +34,7 @@ class Make(Act):
            info["add"] |= {f'On({arg},WaterTable)'}
        elif arg == cls.valid_args[2]:
            info["add"] |= {f'On({arg},Bar)'}
+        info['cost'] = 10
        return info

    def _update(self) -> ptree.common.Status:
--- a/robowaiter/behavior_lib/act/MoveTo.py
+++ b/robowaiter/behavior_lib/act/MoveTo.py
@ -5,7 +5,7 @@ from robowaiter.algos.navigator.navigate import Navigator
 class MoveTo(Act):
    can_be_expanded = True
    num_args = 1
-    valid_args = Act.all_object | Act.all_place
+    valid_args = Act.all_object | Act.tables_for_placement | Act.tables_for_guiding
    valid_args.add('Customer')

    def __init__(self, target_place):
@ -21,7 +21,7 @@ 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']=10
+        info['cost']=5
        return info


@ -35,7 +35,7 @@ class MoveTo(Act):
        # 走到固定的地点
        if self.target_place in Act.place_xyz_dic:
            goal = Act.place_xyz_dic[self.target_place]
-            self.scene.walk_to(goal[0]+1,goal[1])
+            self.scene.walk_to(goal[0]+1,goal[1],goal[2])
        # 走到物品边上
        else:
            # 是否用容器装好
--- a/robowaiter/behavior_lib/act/PickUp.py
+++ b/robowaiter/behavior_lib/act/PickUp.py
@ -18,7 +18,7 @@ class PickUp(Act):
        info["pre"] = {f'At(Robot,{arg})','Holding(Nothing)'}
        info["add"] = {f'Holding({arg})'}
        info["del_set"] = {f'Holding(Nothing)'}
-        for place in Act.all_place:
+        for place in cls.valid_args:
            info["del_set"] |= {f'On({arg},{place})'}
        return info

--- a/robowaiter/behavior_lib/act/PutDown.py
+++ b/robowaiter/behavior_lib/act/PutDown.py
@ -8,7 +8,7 @@ class PutDown(Act):
    can_be_expanded = True
    num_args = 2

-    valid_args = tuple(itertools.product(Act.all_object, Act.all_place))
+    valid_args = tuple(itertools.product(Act.all_object, Act.tables_for_placement))

    def __init__(self, *args):
        super().__init__(*args)
@ -23,7 +23,7 @@ class PutDown(Act):
        info["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'}
        info["del_set"] = {f'Holding({arg[0]})'}

-        info['cost'] = 100
+        info['cost'] = 1
        return info


--- a/robowaiter/behavior_lib/cond/At.py
+++ b/robowaiter/behavior_lib/cond/At.py
@ -7,7 +7,7 @@ class At(Cond):
    can_be_expanded = True
    num_params = 2

-    valid_args = list(itertools.product(('Robot','Customer'), tuple(Cond.all_object | Cond.all_place | {'Customer'})))
+    valid_args = list(itertools.product(('Robot','Customer'), tuple(Cond.all_object | Cond.tables_for_placement | Cond.tables_for_guiding | {'Customer'})))
    valid_args.remove(('Customer','Customer'))
    valid_args = tuple(valid_args)

--- a/robowaiter/behavior_lib/cond/DetectCustomer.py
+++ b/robowaiter/behavior_lib/cond/DetectCustomer.py
@ -0,0 +1,31 @@
+import py_trees as ptree
+from typing import Any
+from robowaiter.behavior_lib._base.Cond import Cond
+import itertools
+
+class DetectCustomer(Cond):
+    can_be_expanded = False
+    num_params = 0
+    valid_args = ()
+
+    def __init__(self,*args):
+        super().__init__(*args)
+
+
+    def _update(self) -> ptree.common.Status:
+
+        # 获取customer的位置
+        # bar (247.0, 520.0, 100.0)
+        close_to_bar = False
+        scene = self.scene.status
+        for walker in scene.walkers:
+            x, y, yaw = walker.pose.X, walker.pose.Y, walker.pose.Yaw
+            # 到达一定区域就打招呼
+            if y >= 450 and y <= 620 and x >= 40 and x <= 100 and yaw>=-10 and yaw <=10:
+                close_to_bar = True
+                break
+
+        if close_to_bar:
+            return ptree.common.Status.SUCCESS
+        else:
+            return ptree.common.Status.FAILURE
--- a/robowaiter/behavior_lib/cond/On.py
+++ b/robowaiter/behavior_lib/cond/On.py
@ -6,7 +6,7 @@ class On(Cond):
    can_be_expanded = True
    num_params = 2
    valid_params = [tuple(Cond.all_object),
-            tuple(Cond.all_place)]
+            tuple(Cond.tables_for_placement)]


    def __init__(self,*args):
--- a/robowaiter/llm_client/data/test_questions.txt
+++ b/robowaiter/llm_client/data/test_questions.txt
@ -1 +1 @@
-{"做一杯咖啡": {"Answer": "OK，我这就去做一杯咖啡", "Goal": "{\"On(Coffee,CoffeeTable)\"}"}, "做一杯咖啡放到吧台上": {"Answer": "OK，我这就去做一杯咖啡放到吧台上", "Goal": "{\"On(Coffee,Bar)\"}"}, "做一杯咖啡放到水杯桌上，再倒一杯水": {"Answer": "OK，我这就去做一杯咖啡放到水杯桌上，再倒一杯水", "Goal": "{\"On(Coffee,WaterTable)\",\"On(Water,WaterTable)\"}"}, "前往2号桌": {"Answer": "OK，我这前往2号桌", "Goal": "{\"At(Robot,Table2)\"}"}, "测试AEM": {"Answer": "测试AEM", "Goal": "{\"EnvExplored()\"}"}, "倒一杯水": {"Answer": "OK，我这就去倒一杯水", "Goal": "{\"On(Water,WaterTable)\"}"}, "开空调": {"Answer": "OK，我这就去开空调", "Goal": "{\"Is(AC,On)\"}"}, "关空调": {"Answer": "OK，我这就去关空调", "Goal": "{\"Is(AC,Off)\"}"}, "关大厅灯": {"Answer": "OK，我这就去关大厅灯", "Goal": "{\"Is(HallLight,Off)\"}"}, "开大厅灯": {"Answer": "OK，我这就去开大厅灯", "Goal": "{\"Is(HallLight,On)\"}"}, "关筒灯": {"Answer": "OK，我这就去关筒灯", "Goal": "{\"Is(TubeLight,Off)\"}"}, "开筒灯": {"Answer": "OK，我这就去开筒灯", "Goal": "{\"Is(TubeLight,On)\"}"}, "关窗帘": {"Answer": "OK，我这就去关窗帘", "Goal": "{\"Is(Curtain,Off)\"}"}, "开窗帘": {"Answer": "OK，我这就去开窗帘", "Goal": "{\"Is(Curtain,On)\"}"}, "拖地": {"Answer": "OK，我这就去拖地", "Goal": "{\"Is(Floor,Clean)\"}"}, "擦桌子": {"Answer": "OK，我这就去擦桌子", "Goal": "{\"Is(Table1,Clean)\"}"}, "整理椅子": {"Answer": "OK，我这就去整理椅子", "Goal": "{\"Is(Chairs,Clean)\"}"}, "把冰红茶放到Table2": {"Answer": "OK，我这就去把冰红茶放到Table2", "Goal": "{\"On(BottledDrink,Table2)\"}"}, "我有点热，能开个空调吗？": {"Answer": "当然可以，我现在就开！", "Goal": "{\"Is(AC,On)\"}"}, "可以带我去吗": {"Answer": "当然可以，前往一号桌", "Goal": "{\"At(Robot,Table1)\"}"}, "把酸奶放到1号桌，再做一杯咖啡送到水杯桌上，再倒一杯水。": {"Answer": "明白，我这就去办！", "Goal": "{\"On(Coffee,WaterTable)\",\"On(Yogurt,Table1)\",\"On(Water,WaterTable)\"}"}, "下班啦！打扫卫生，关灯关空调关窗帘。": {"Answer": "太棒啦，下班啦！我这就去打扫卫生，关灯关空调关窗帘。", "Goal": "{\"Is(Floor,Clean)\",\"Is(Table1,Clean)\",\"Is(Chairs,Clean)\",\"Is(AC,Off)\",\"Is(HallLight,Off)\",\"Is(TubeLight,Off)\",\"Is(Curtain,Off)\"}"}}
+{"做一杯咖啡": {"Answer": "OK，我这就去做一杯咖啡", "Goal": "{\"On(Coffee,CoffeeTable)\"}"}, "做一杯咖啡放到吧台上": {"Answer": "OK，我这就去做一杯咖啡放到吧台上", "Goal": "{\"On(Coffee,Bar)\"}"}, "做一杯咖啡放到水杯桌上，再倒一杯水": {"Answer": "OK，我这就去做一杯咖啡放到水杯桌上，再倒一杯水", "Goal": "{\"On(Coffee,WaterTable)\",\"On(Water,WaterTable)\"}"}, "前往2号桌": {"Answer": "OK，我这前往2号桌", "Goal": "{\"At(Robot,Table2)\"}"}, "测试AEM": {"Answer": "测试AEM", "Goal": "{\"EnvExplored()\"}"}, "倒一杯水": {"Answer": "OK，我这就去倒一杯水", "Goal": "{\"On(Water,WaterTable)\"}"}, "开空调": {"Answer": "OK，我这就去开空调", "Goal": "{\"Is(AC,On)\"}"}, "关空调": {"Answer": "OK，我这就去关空调", "Goal": "{\"Is(AC,Off)\"}"}, "关大厅灯": {"Answer": "OK，我这就去关大厅灯", "Goal": "{\"Is(HallLight,Off)\"}"}, "开大厅灯": {"Answer": "OK，我这就去开大厅灯", "Goal": "{\"Is(HallLight,On)\"}"}, "关筒灯": {"Answer": "OK，我这就去关筒灯", "Goal": "{\"Is(TubeLight,Off)\"}"}, "开筒灯": {"Answer": "OK，我这就去开筒灯", "Goal": "{\"Is(TubeLight,On)\"}"}, "关窗帘": {"Answer": "OK，我这就去关窗帘", "Goal": "{\"Is(Curtain,Off)\"}"}, "开窗帘": {"Answer": "OK，我这就去开窗帘", "Goal": "{\"Is(Curtain,On)\"}"}, "拖地": {"Answer": "OK，我这就去拖地", "Goal": "{\"Is(Floor,Clean)\"}"}, "擦桌子": {"Answer": "OK，我这就去擦桌子", "Goal": "{\"Is(Table1,Clean)\"}"}, "整理椅子": {"Answer": "OK，我这就去整理椅子", "Goal": "{\"Is(Chairs,Clean)\"}"}, "把冰红茶放到Table2": {"Answer": "OK，我这就去把冰红茶放到Table2", "Goal": "{\"On(BottledDrink,Table2)\"}"}, "我有点热，能开个空调吗？": {"Answer": "当然可以，我现在就开！", "Goal": "{\"Is(AC,On)\"}"}, "可以带我去吗": {"Answer": "当然可以，前往一号桌", "Goal": "{\"At(Robot,Table1)\"}"}, "把酸奶放到1号桌，再做一杯咖啡送到水杯桌上，再倒一杯水。": {"Answer": "明白，我这就去办！", "Goal": "{\"On(Coffee,WaterTable)\",\"On(Yogurt,Table1)\",\"On(Water,WaterTable)\"}"}, "下班啦！打扫卫生，关灯关空调关窗帘。": {"Answer": "太棒啦，下班啦！我这就去打扫卫生，关灯关空调关窗帘。", "Goal": "{\"Is(Floor,Clean)\",\"Is(Table1,Clean)\",\"Is(Chairs,Clean)\",\"Is(AC,Off)\",\"Is(HallLight,Off)\",\"Is(TubeLight,Off)\",\"Is(Curtain,Off)\"}"}, "请问可以带我去空位上嘛？我想晒太阳。": {"Answer": "没问题！请跟我来。", "Goal": "{\"At(Robot,BrightTable1)\"}"}}
--- a/robowaiter/llm_client/data_raw/test_questions.csv
+++ b/robowaiter/llm_client/data_raw/test_questions.csv
@ -21,3 +21,4 @@ Question,Answer,Goal
 可以带我去吗,当然可以，前往一号桌,"{""At(Robot,Table1)""}"
 把酸奶放到1号桌，再做一杯咖啡送到水杯桌上，再倒一杯水。,明白，我这就去办！,"{""On(Coffee,WaterTable)"",""On(Yogurt,Table1)"",""On(Water,WaterTable)""}"
 下班啦！打扫卫生，关灯关空调关窗帘。,太棒啦，下班啦！我这就去打扫卫生，关灯关空调关窗帘。,"{""Is(Floor,Clean)"",""Is(Table1,Clean)"",""Is(Chairs,Clean)"",""Is(AC,Off)"",""Is(HallLight,Off)"",""Is(TubeLight,Off)"",""Is(Curtain,Off)""}"
+请问可以带我去空位上嘛？我想晒太阳。,没问题！请跟我来。,"{""At(Robot,BrightTable1)""}"
--- a/robowaiter/llm_client/find_obj_utils.py
+++ b/robowaiter/llm_client/find_obj_utils.py
@ -0,0 +1,122 @@
+import random
+
+# all_obj_en = ['Mug', 'Banana', 'Toothpaste', 'Bread', 'Softdrink', 'Yogurt', 'ADMilk', 'VacuumCup', 'BottledDrink', 'PencilVase',
+#               'Teacup', 'Caddy', 'Dictionary', 'Cake', 'Date', 'Stapler', 'LunchBox', 'Bracelet', 'MilkDrink', 'CocountWater', 'Walnut', 'HamSausage',
+#               'GlueStick', 'AdhensiveTape', 'Calculator', 'Chess', 'Orange', 'Glass', 'Washbowl', 'Durian', 'Gum', 'Towl', 'OrangeJuice', 'Cardcase',
+#               'RubikCube', 'StickyNotes', 'NFCJuice', 'SpringWater', 'Apple', 'Coffee', 'Gauze', 'Mangosteen', 'SesameSeedCake', 'Glove', 'Mouse',
+#               'Kettle', 'Atomize', 'Chips', 'SpongeGourd', 'Garlic', 'Potato', 'Tray', 'Hemomanometer', 'TennisBall', 'ToyDog', 'ToyBear', 'TeaTray',
+#               'Sock', 'Scarf', 'ToiletPaper', 'Milk', 'Soap', 'Novel', 'Watermelon', 'Tomato', 'CleansingFoam', 'CocountMilk', 'SugarlessGum',
+#               'MedicalAdhensiveTape', 'SourMilkDrink', 'PaperCup', 'Tissue', 'YogurtDrink', 'Newspaper', 'Box', 'PaperCupStarbucks', 'CoffeeMachine',
+#               'Straw', 'Cake', 'Tray', 'Bread', 'Glass', 'Door', 'Mug', 'Machine', 'PackagedCoffee', 'CubeSugar',
+#               'Apple', 'Spoon', 'Drinks', 'Drink', 'Take-AwayCup', 'Saucer', 'TrashBin', 'Knife', 'Ginger', 'Floor', 'Roof', 'Wall'] + \
+#              ['Broom', 'CoffeeCup', 'Door', 'Glass', 'Mug', 'ZhuZi', 'Towel', 'LaJiTong', 'CoffeeMachine', 'ZaoTai', 'Sofa', 'Cake', 'ChaTou',
+#               'Plate', 'CoffeeBag', 'TuoBu', 'KaiGuan', 'Kettle', 'Apple', 'ChaZuo', 'Sugar', 'BaTai', 'BaoJing', 'DrinkMachine', 'KongTiao',
+#               'Desk', 'Clip', 'Knife', 'TuoPan', 'BoJi', 'ZhiBeiHe', 'Bread', 'WaterCup', 'Box', 'Chair', 'Hand', 'XiGuan', 'Spoon', 'Container',
+#               'IceMachine', 'KaoXiang', 'SaoBa', 'XiangGui']
+
+# , '机器人左手', '机器人右手', 'Bernachon'
+all_obj = ['马克杯', '香蕉', '牙膏', '面包', '软饮料', '酸奶', 'AD钙奶', '真空杯', '瓶装饮料', '铅笔花瓶', '茶杯', '茶匙', '词典',
+              '蛋糕', '日期', '订书机', '午餐盒', '手镯', '牛奶饮品', '椰水', '核桃', '火腿肠', '胶棒', '胶带', '计算器', '国际象棋', '橙子',
+              '玻璃杯', '洗碗盆', '榴莲', '口香糖', '毛巾', '橙汁', '卡片盒', '魔方', '便签', 'NFC汁', '矿泉水', '苹果', '咖啡', '纱布', '芒果',
+              '芝麻蛋糕', '手套', '鼠标', '水壶', '雾化器', '薯片', '丝瓜', '大蒜', '土豆', '托盘', '血压计', '网球', '玩具狗', '玩具熊', '茶盘',
+              '袜子', '围巾', '卫生纸', '牛奶', '肥皂', '小说', '西瓜', '番茄', '洁面泡沫', '椰奶', '无糖口香糖', '医用胶带', '酸奶饮品', '纸杯',
+              '纸巾', '酸奶饮品', '报纸', '盒子', '星巴克纸杯', '咖啡机', '吸管', '蛋糕', '托盘', '面包', '玻璃杯', '门',
+              '马克杯', '机器', '包装咖啡', '方糖', '苹果', '勺子', '饮料', '饮料', '外带杯', '茶杯', '垃圾桶', '刀', '姜', '地板', '屋顶', '墙'] + \
+             ['扫帚', '咖啡杯', '门', '玻璃杯', '马克杯', '著子', '毛巾', '垃圾桶', '咖啡机', '灶台', '沙发', '蛋糕', '插头', '盘子', '咖啡袋', '拖布', '开关',
+              '水壶', '苹果', '插座', '糖', '吧台', '报警', '饮料机', '空调', '桌子', '夹子', '刀', '托盘', '簸箕', '纸杯盒', '面包', '水杯', '盒子', '椅子',
+              '手', '吸管', '勺子', '容器', '制冰机', '烤箱', '扫把', '香柜']
+
+# 储藏室
+all_loc = ['吧台', '餐桌', '沙发', '灶台', '大门', '灯开关', '空调开关', '橱柜', '卫生间', '窗户', '音响', '休闲区', '工作台', '服务台', '收银台', '墙角',
+              '蛋糕柜', '充电处', '冰箱', '书架']
+
+all_loc_en = ['bar', 'Table', 'sofa', 'stove', 'Gate', 'light switch', 'airconditioner switch', 'cabinet', 'bathroom', 'window',
+              'audio', 'lounge area', 'workstation', 'service counter', 'cashier counter', 'corner',
+              'cake display', 'ChargingStations', 'refrigerator', 'bookshelf']
+
+loc_map_en = {'bar': {'工作台', '服务台', '收银台', '蛋糕柜'},
+           'Table': {'沙发', '大门', '窗户', '休闲区', '墙角', '椅子', '书架'},
+           'sofa': {'餐桌', '窗户', '音响', '休闲区', '墙角', '书架'},
+           'stove': {'吧台', '橱柜', '工作台', '服务台', '收银台', '蛋糕柜', '冰箱'},
+           'Gate': {'吧台', '灯开关', '空调开关', '卫生间', '墙角'},
+           'light switch': {'大门', '空调开关', '卫生间', '墙角'},
+           'airconditioner switch': {'大门', '灯开关', '卫生间', '墙角'},
+           'cabinet': {'灶台', '吧台', '工作台', '服务台', '收银台', '蛋糕柜', '充电处', '冰箱'},
+           'bathroom': {'大门', '墙角'},
+           'window': {'餐桌', '沙发', '休闲区'},
+           'audio': {'餐桌', '沙发', '休闲区', '墙角', '书架'},
+           'lounge area': {'沙发', '餐桌', '墙角', '书架', '音响'},
+           'workstation': {'吧台', '服务台', '收银台'},
+           'service counter': {'吧台', '工作台', '收银台'},
+           'cashier counter': {'吧台', '工作台', '服务台'},
+           'corner': {'卫生间', '沙发', '灯开关', '空调开关', '音响', '休闲区', '书架'},
+           'cake display': {'吧台', '橱柜', '服务台', '收银台', '冰箱'},
+           'ChargingStations': {'吧台', '餐桌', '沙发', '休闲区', '工作台', '服务台', '收银台', '墙角', '书架'},
+           'refrigerator': {'吧台', '服务台', '蛋糕柜'},
+           'bookshelf': {'餐桌', '沙发', '窗户', '休闲区', '墙角'}
+           }
+
+
+loc_map = {'吧台': {'灶台', '橱柜', '工作台', '服务台', '收银台', '蛋糕柜', '充电处', '冰箱'},
+           '餐桌': {'沙发', '大门', '窗户', '休闲区', '墙角', '椅子', '书架'},
+           '沙发': {'餐桌', '窗户', '音响', '休闲区', '墙角', '书架'},
+           '灶台': {'吧台', '橱柜', '工作台', '服务台', '收银台', '蛋糕柜', '冰箱'},
+           '大门': {'吧台', '灯开关', '空调开关', '卫生间', '墙角'},
+           '灯开关': {'大门', '空调开关', '卫生间', '墙角'},
+           '空调开关': {'大门', '灯开关', '卫生间', '墙角'},
+           '橱柜': {'灶台', '吧台', '工作台', '服务台', '收银台', '蛋糕柜', '充电处', '冰箱'},
+           '卫生间': {'大门', '墙角'},
+           '窗户': {'餐桌', '沙发', '休闲区'},
+           '音响': {'餐桌', '沙发', '休闲区', '墙角', '书架'},
+           '休闲区': {'沙发', '餐桌', '墙角', '书架', '音响'},
+           '工作台': {'吧台', '服务台', '收银台'},
+           '服务台': {'吧台', '工作台', '收银台'},
+           '收银台': {'吧台', '工作台', '服务台'},
+           '墙角': {'卫生间', '沙发', '灯开关', '空调开关', '音响', '休闲区', '书架'},
+           '蛋糕柜': {'吧台', '橱柜', '服务台', '收银台', '冰箱'},
+           '充电处': {'吧台', '餐桌', '沙发', '休闲区', '工作台', '服务台', '收银台', '墙角', '书架'},
+           '冰箱': {'吧台', '灶台', '工作台', '服务台', '收银台', '蛋糕柜'},
+           '书架': {'餐桌', '沙发', '窗户', '休闲区', '墙角'}
+           }
+
+# print("looc_map:", loc_map['吧台'])
+# print("looc_map:", random.choice(list(loc_map['吧台'])))
+
+
+# import synonyms
+# def get_synonyms(word):
+#     synonyms_list = synonyms.nearby(word)[0]
+#     return synonyms_list
+#
+# target_word = "高兴"
+# synonyms_result = get_synonyms(target_word)
+# print("synonyms_result:",synonyms_result)
+
+
+# def save_variable(self):
+#     scene = self.status
+#     init_obj_name_set = set()
+#     init_obj_info_dict = {}
+#     for item in scene.objects:
+#         name = item.name
+#         location = item.location
+#         init_obj_name_set.add(name)
+#         # 如果name已经存在，则添加一个唯一标识符
+#         if name in init_obj_info_dict:
+#             count = 1
+#             new_name = f"{name}_{count}"
+#             while new_name in init_obj_info_dict:
+#                 count += 1
+#                 new_name = f"{name}_{count}"
+#             name = new_name
+#         init_obj_info_dict[name] = location
+#     # print("obj_info_dict:",obj_info_dict)
+#     # print("obj_name_set:",obj_name_set)
+#     import os
+#     import pickle
+#     absolute_path = r"E:\DATA\Projects\Robot1002 5.2\Plugins\LLM_prompt\data\init_obj_name_set.pkl"
+#     target_directory = os.path.dirname(absolute_path)
+#     os.makedirs(target_directory, exist_ok=True)
+#     # 将变量保存到二进制文件
+#     with open(absolute_path, 'wb') as file:
+#         pickle.dump(init_obj_name_set, file)
--- a/robowaiter/llm_client/tool_api.py
+++ b/robowaiter/llm_client/tool_api.py
@ -36,10 +36,9 @@ functions = get_tools()
 def run_conversation(query: str, stream=False, max_retry=5):
    params = dict(model="chatglm3", messages=[{"role": "user", "content": query}], stream=stream)
    params["functions"] = functions
-    # print(params)
    response = get_response(**params)

-    for _ in range(max_retry):
+    for retry in range(max_retry):
        if response["choices"][0]["message"].get("function_call"):
            function_call = response["choices"][0]["message"]["function_call"]
            logger.info(f"Function Call Response: {function_call}")
@ -61,6 +60,7 @@ def run_conversation(query: str, stream=False, max_retry=5):
                    "content": tool_response,  # 调用函数返回结果
                }
            )
+            # del params["functions"]
        else:
            reply = response["choices"][0]["message"]["content"]
            return {
@ -110,5 +110,18 @@ def run_conversation_for_test_only(query: str, stream=False, max_retry=5):


 if __name__ == "__main__":
-    query = "可以带我去吗"
-    print(run_conversation_for_test_only(query, stream=False))
+    # query = "可以带我去吗"
+    # print(run_conversation_for_test_only(query, stream=False))
+
+    query = "卫生间在哪里"   #
+    print(run_conversation(query, stream=False))
+
+    query = "我想看看冰箱,请问哪里可以找到冰箱"
+    print(run_conversation(query, stream=False))
+
+    query = "我想找个充电的地方，你能告诉我在哪儿吗"
+    print(run_conversation(query, stream=False))
+
+    query = "我想找张桌子"   #
+    print(run_conversation(query, stream=False))
+    # for query in
--- a/robowaiter/llm_client/tool_register.py
+++ b/robowaiter/llm_client/tool_register.py
@ -4,9 +4,13 @@ from copy import deepcopy
 from pprint import pformat
 from types import GenericAlias
 from typing import get_origin, Annotated
+import robowaiter.llm_client.find_obj_utils as find_obj_utils
+import random
+# import spacy

 _TOOL_HOOKS = {}
 _TOOL_DESCRIPTIONS = {}
+# nlp = spacy.load('en_core_web_lg')


 def register_tool(func: callable):
@ -143,26 +147,56 @@ def create_sub_task(

    return goal

-@register_tool
-def get_object_info(
-        obj: Annotated[str, '需要获取信息的物体名称', True]
-) -> str:
-    """
-    获取场景中指定物体 `object` 在哪里，不涉及到具体的执行任务
-    如果`object` 是一个地点，例如洗手间，则输出大门。
-    如果`object`是咖啡，则输出桌子，咖啡在桌子上。
-    如果`object` 是空桌子，则输出一号桌
-    """
-    near_object = None
-    if obj == "Table":
-        near_object = "Bar"
-    if obj == "洗手间":
-        near_object = "大门"
-    if obj == "空桌子":
-        near_object = "一号桌"
-    return near_object
-
+# @register_tool
+# def get_object_info(
+#         obj: Annotated[str, '需要获取信息的物体名称', True]
+# ) -> str:
+#     """
+#     获取场景中指定物体 `object` 在哪里，不涉及到具体的执行任务
+#     如果`object` 是一个地点，例如洗手间，则输出大门。
+#     如果`object`是咖啡，则输出桌子，咖啡在桌子上。
+#     如果`object` 是空桌子，则输出一号桌
+#     """
+#     near_object = None
+#     # if obj == "Table":
+#     #     near_object = "Bar"
+#     # if obj == "洗手间":
+#     #     near_object = "大门"
+#     # if obj == "空桌子":
+#     #     near_object = "一号桌"
+#     if obj in find_obj_utils.all_loc:   # object是一个地点
+#         mp = list(find_obj_utils.loc_map[obj])
+#         # near_object = random.choice(mp)
+#         near_object = mp
+#     if obj in find_obj_utils.all_obj:   # object是一个物品
+#         near_ls = find_obj_utils.all_loc + find_obj_utils.all_obj
+#         near_object = random.choices(near_ls,k=5)
+#     return near_object

+# @register_tool
+# def find_location(
+#         location: Annotated[str, '客人咨询的地点', True]
+# ) -> str:
+#     """"
+#     获取的location为英文
+#     用户想找某个地点
+#     """
+#     near_location = None
+#     query_token = nlp(location)
+#     max_similarity = 0
+#     similar_word = None
+#     for w in find_obj_utils.all_loc_en:
+#         word_token = nlp(w)
+#         similarity = query_token.similarity(word_token)
+#
+#         if similarity > max_similarity:
+#             max_similarity = similarity
+#             similar_word = w
+#     print("similarity:", max_similarity, "similar_word:", similar_word)
+#     if similar_word:
+#         mp = list(find_obj_utils.loc_map_en[similar_word])
+#         near_location = random.choice(mp)
+#     return near_location

 if __name__ == "__main__":
    print(dispatch_tool("get_weather", {"city_name": "beijing"}))
--- a/robowaiter/robot/Default.ptml
+++ b/robowaiter/robot/Default.ptml
@ -14,5 +14,14 @@ selector
            act SubTaskPlaceHolder()
        }
    }
-
+    sequence
+    {
+        cond DetectCustomer()
+        selector
+        {
+            cond At(Robot,Bar)
+            act MoveTo(Bar)
+        }
+        act GreatCustomer()
+    }
 }
--- a/robowaiter/robot/robot.py
+++ b/robowaiter/robot/robot.py
@ -37,6 +37,7 @@ class Robot(object):
    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)
        sub_task_place_holder = find_node_by_name(self.bt.root,"SubTaskPlaceHolder()")
--- a/robowaiter/scene/scene.py
+++ b/robowaiter/scene/scene.py
@ -1,3 +1,4 @@
+import pickle
 import sys
 import time
 import grpc
@ -9,8 +10,9 @@ import math
 from robowaiter.proto import GrabSim_pb2
 from robowaiter.proto import GrabSim_pb2_grpc

-
-
+import os
+from robowaiter.utils import get_root_path
+root_path = get_root_path()

 channel = grpc.insecure_channel(
    "localhost:30001",
@ -58,10 +60,9 @@ class Scene:
        "sub_goal_list": [],  # 子目标列表
        "status": None,  # 仿真器中的观测信息，见下方详细解释
        "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)',
-                                    'Holding(Nothing)',
-                                   # 'Holding(Yogurt)'
-                                   'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
-                                   'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
+         'Holding(Nothing)','Exist(Yogurt)','Exist(BottledDrink)','On(Yogurt,Bar)','On(BottledDrink,Table1)',
+         'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
+         'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
    }
    """
    status:
@ -110,6 +111,9 @@ class Scene:
        self.all_frontier_list = set()
        self.semantic_map = semantic_map
        self.auto_map = np.ones((800, 1550))
+        self.filename = os.path.join(root_path, 'robowaiter/proto/map_1.pkl')
+        with open(self.filename, 'rb') as file:
+            self.map_file = pickle.load(file)


    def reset(self):
@ -130,7 +134,6 @@ class Scene:
        # 基类run

        self._run()
-
        # 运行并由robot打印每步信息
        while True:
            self.step()
@ -175,10 +178,11 @@ class Scene:

    def reset_sim(self):
        # reset world
+        stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID))
        init_world()
-        
        stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID))

+
    def _reset(self):
        # 场景自定义的reset
        pass
@ -193,8 +197,6 @@ class Scene:



-
-
    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]
@ -230,22 +232,31 @@ class Scene:
        else:
            return True

+
+    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)
+        # print(scene.info)
+        walker_list=[]
+        if (str(scene.info).find('unreachable') > -1):
+            print('当前位置不可达,无法初始化NPC')
+        else:
+            walker_list.append(
+                GrabSim_pb2.WalkerList.Walker(id=id+5, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=loc[2])))
+        stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID))
+
    def add_walkers(self,walker_loc=[[0, 880], [250, 1200], [-55, 750], [70, -200]]):
        print('------------------add_walkers----------------------')
-        walker_list = []
-        for i in range(len(walker_loc)):
-            loc = walker_loc[i] + [0,0, 100]
-            action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc)
-            scene = stub.Do(action)
-            print(scene.info)
-            if (str(scene.info).find('unreachable') > -1):
-                print('当前位置不可达,无法初始化NPC')
-            else:
-                walker_list.append(
-                    GrabSim_pb2.WalkerList.Walker(id=i + 5, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=90)))
-
-        scene = stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID))
-        return scene
+        for id,walker in enumerate(walker_loc):
+            if len(walker)==2:
+                self.add_walker(id,walker[0],walker[1])
+            elif len(walker)==3:
+                self.add_walker(id, walker[0], walker[1],walker[2])
+            elif len(walker) == 4:
+                self.add_walker(id, walker[0], walker[1], walker[2], walker[3])
+            elif len(walker) == 5:
+                self.add_walker(id, walker[0], walker[1], walker[2], walker[3], walker[4])

    def remove_walker(self, *args):  # take single walkerID or a list of walkerIDs
        remove_list = []
@ -266,12 +277,13 @@ class Scene:
            GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID)
        )

+
    def control_walkers(self,walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True):
        """pose:表示行人的终止位置姿态"""
        scene = self.status
        walker_loc = walker_loc
        controls = []
-        for i in range(len(scene.walkers)):
+        for i in range(len(walker_loc)):
            loc = walker_loc[i]
            is_autowalk = is_autowalk
            pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180)
@ -458,7 +470,6 @@ class Scene:
            walk_v = release_pos[:-1] + [180, 180, 0]
            if release_pos == [340.0, 900.0, 99.0]:
                walk_v[2] = 130
-        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)
        print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw])
@ -507,6 +518,11 @@ class Scene:
        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 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]
+            stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
            pass
        if obj_info.name=="Glass":
            pass
@ -526,12 +542,17 @@ class Scene:
                                    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)

+    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))
+        time.sleep(1.0)
+

    def robo_stoop_parallel(self):
        # 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊
        scene = self.status
        angle = [scene.joints[i].angle for i in range(21)]
-        angle[0] = 15
+        angle[0] = 15   # 15
        angle[19] = -15
        angle[20] = -30
        action = GrabSim_pb2.Action(scene=self.sceneID,action=GrabSim_pb2.Action.ActionType.RotateJoints,    # 弯腰
@ -542,7 +563,7 @@ class Scene:
    def release_obj(self,release_pos):
        print("------------------release_obj----------------------")
        if release_pos==[340.0, 900.0, 99.0]:
-            self.ik_control_joints(2, 300.0, 935, release_pos[2])
+            self.ik_control_joints(2, release_pos[0]-40, release_pos[1]+35, release_pos[2])
            time.sleep(2.0)
        else:
            self.ik_control_joints(2, release_pos[0] - 80, release_pos[1], release_pos[2])
@ -553,6 +574,7 @@ class Scene:
        scene = stub.Do(action)
        time.sleep(2.0)
        self.robo_recover()
+        self.standard_finger()

        return True

@ -700,11 +722,14 @@ class Scene:
        ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100]
        return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2)

-    # def test_yaw(self):
-    #     walk_v = [247.0, 480.0, 180.0, 180, 0]
-    #     action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
-    #     scene = stub.Do(action)
-    #     time.sleep(4)
-    #     walk_v = [247.0, 500.0, 0.0, 180, 0]
-    #     action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
-    #     scene = stub.Do(action)
+    # 根据map文件判断是否可达
+    def reachable(self, pos):
+        x, y = self.real2map(pos[0], pos[1])
+        if self.map_file[x, y] == 0:
+            return True
+        else:
+            return False
+
+
+
+
--- a/robowaiter/scene/tasks/Open_tasks_test.py
+++ b/robowaiter/scene/tasks/Open_tasks_test.py
@ -18,10 +18,17 @@ class SceneOT(Scene):
        super().__init__(robot)
        # 在这里加入场景中发生的事件
        self.event_list = [
-            (5, self.set_goal("At(Robot,Table2)"))
+            # (5, self.set_goal("At(Robot,WaterTable)"))
+            # (5, self.set_goal("On(Yogurt,Table4)"))
+            (5, self.set_goal("At(Robot,BrightTable4)"))
        ]

    def _reset(self):
+        self.gen_obj()
+        self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)',
+         'Holding(Nothing)','Exist(Yogurt)','Exist(BottledDrink)','On(Yogurt,Bar)','On(BottledDrink,Table1)',
+         'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
+         'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
        pass


--- a/robowaiter/scene/tasks/VLM/VLM.py
+++ b/robowaiter/scene/tasks/VLM/VLM.py
@ -41,9 +41,11 @@ class SceneVLM(Scene):
         'Holding(Nothing)','Exist(Yogurt)','Exist(Softdrink)','On(Yogurt,Bar)','On(Softdrink,Table1)',
         'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
         'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
-        self.add_walkers([[0, 880], [250, 1200], [-55, 750], [70, -200]])
-        # self.add_walkers([[-500, 500]])
+        # 随机生成4个自由行走，一个在 BrightTable4,BrightTable5(-20,220)
+        self.add_walkers([[0, 880], [250, 1200], [-55, 750], [70, -200],[-290, 400, 0],[20, -150,180]])
        self.control_walkers(walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True)
+
+
        # self.control_walkers(walker_loc=[[-55, 750]],is_autowalk = False)
        # 在场景中随机增加一堆行人。
        # walker_loc = [[-55, 750], [70, -200], [250, 1200], [0, 880]]
--- a/robowaiter/scene/tasks/VLM/VLM2.py
+++ b/robowaiter/scene/tasks/VLM/VLM2.py
@ -11,28 +11,12 @@ class SceneVLM(Scene):
        super().__init__(robot)
        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
        self.event_list = [
-            # (5, self.create_chat_event("测试VLM：做一杯咖啡")),
-            # (5, self.create_chat_event("测试VLM：倒一杯水")),
-            # (5, self.create_chat_event("测试VLM：开空调")),
-            # (5, self.create_chat_event("测试VLM：关空调")),
-            # (5, self.create_chat_event("测试VLM：开大厅灯")),
-            # (5, self.create_chat_event("测试VLM：拖地")),
-            # (7, self.create_chat_event("测试VLM：擦桌子")),
-            # (5, self.create_chat_event("测试VLM：整理椅子")),
-            # (5, self.create_chat_event("测试VLM：把冰红茶放到Table2")),
-            # (5, self.create_chat_event("测试VLM：关大厅灯"))
-            # (5, self.create_chat_event("测试VLM：做一杯咖啡放到吧台上")),
-            # (5, self.create_chat_event("测试VLM：做一杯咖啡放到水杯桌上并倒水")),
-            # (8, self.create_chat_event("测试VLN：前往1号桌")),
-
+            (5, self.create_chat_event("请问可以带我去空位上嘛？我想晒太阳。")),
        ]

    def _reset(self):
-
-        # self.gen_obj(type=5)
-        # self.gen_obj(type=9)
-        # self.op_task_execute(op_type=16, obj_id=0)
-        # self.move_task_area(op_type=4)
+        self.gen_obj()
+        self.add_walkers([[47, 920]])
        pass

    def _run(self, op_type=10):
@ -42,74 +26,28 @@ class SceneVLM(Scene):
        # 带领行人去有太阳的地方
        # 行人说 有点热
        # 好的，这就去开空调
-
-        scene = self.add_walkers([[47, 920],[70,-200]])
-        self.control_walker(
-            [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=47, Y=520, Yaw=0)])
-
-        cont = scene.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳"
-        self.control_robot_action(0,3,cont)
-
-        # self.clean_walker()
-
-
-        # 共17个操作
-        # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子",    # 1-7
-        # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘",            # 8-12
-        # "调整空调开关","调高空调温度","调低空调温度",                   # 13-15
-        # "抓握物体","放置物体"                                       # 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=0)
-        #     self.op_task_execute(op_type, obj_id=0)
-        # # 原始吧台处:[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]
-
-        # 流程测试
-        # 抓握放置:抓吧台前生成的酸奶，放到抹布桌上
-        self.gen_obj()
-        # self.move_task_area(16, obj_id=0)
-        # self.op_task_execute(16, obj_id=0)
-        # pos = [340.0, 900.0, 99.0]
-        # self.move_task_area(17, release_pos=pos)
-        # self.op_task_execute(17, release_pos=pos)
-        #
-        # # 做咖啡:做完的咖啡放到水杯桌上
-        # 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.move_task_area(2)
-        # self.op_task_execute(2)
-
-        #
-        # self.move_task_area(16, obj_id=190)
-        # self.op_task_execute(16, obj_id=190)
-        # pos = [-55.0, 0.0, 107]
-        # self.move_task_area(17, release_pos=pos)
-        # self.op_task_execute(17, release_pos=pos)
-
-        # self.test_yaw()
-
        pass
    
    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']:
+        end = [self.status.location.X, self.status.location.Y]
+        # print("end:",end)
+        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.control_walker(
+                    [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=-90)])
+
+            cont = self.status.walkers[0].name+"谢谢！"
+            self.control_robot_action(0,3,cont)
        pass


--- a/robowaiter/scene/tasks/VLM/VLM3.py
+++ b/robowaiter/scene/tasks/VLM/VLM3.py
@ -0,0 +1,101 @@
+"""
+视觉语言操作
+机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
+"""
+
+import time
+from robowaiter.scene.scene import Scene
+
+class SceneVLM(Scene):
+    def __init__(self, robot):
+        super().__init__(robot)
+        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
+        self.event_list = [
+            (5, self.create_chat_event("请问可以带我去空位上嘛？我想晒太阳。")),
+        ]
+
+    def _reset(self):
+        pass
+
+    def _run(self, op_type=10):
+        # 一个行人从门口走到 吧台
+        # 打招呼需要什么
+        # 行人说 哪里有位置，想晒个太阳
+        # 带领行人去有太阳的地方
+        # 行人说 有点热
+        # 好的，这就去开空调
+
+        self.gen_obj()
+        self.add_walkers([[47, 920]])
+        self.control_walker(
+            [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=180)])
+        time.sleep(1)
+        cont = self.status.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳。"
+        self.control_robot_action(0,3,cont)
+
+        # time.sleep(3)
+        # self.event_list.append((5, self.set_goal("At(Robot,BrightTable1)"))) # "请问可以带我去空位上嘛？我想晒太阳"
+        # self.chat_bubble("没问题！请跟我来。")
+
+
+        # 跟随机器人
+        # cont = self.status.walkers[0].name + "好的！"
+        # self.control_robot_action(0, 3, cont)
+        #
+        # start = [self.status.location.X, self.status.location.Y]
+        # time.sleep(0.2)
+        # end = [self.status.location.X, self.status.location.Y]
+        # while abs(start[0]-end[0])>=1 or abs(start[1]-end[1])>=1:
+        #     self.control_walker(
+        #         [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=0)])
+        #
+        # cont = self.status.walkers[0].name+"谢谢！"
+        # self.control_robot_action(0,3,cont)
+
+        # 共17个操作
+        # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子",    # 1-7
+        # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘",            # 8-12
+        # "调整空调开关","调高空调温度","调低空调温度",                   # 13-15
+        # "抓握物体","放置物体"                                       # 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=0)
+        #     self.op_task_execute(op_type, obj_id=0)
+        # # 原始吧台处:[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]
+        pass
+    
+    def _step(self):
+
+        # 如果机器人不在 吧台
+        # if "At(Robot,Bar)" not in self.state['condition_set']:
+        end = [self.status.location.X, self.status.location.Y]
+        print("end:",end)
+        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.control_walker(
+                    [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=-90)])
+
+            cont = self.status.walkers[0].name+"谢谢！"
+            self.control_robot_action(0,3,cont)
+        pass
+
+
+if __name__ == '__main__':
+    import os
+    from robowaiter.robot.robot import Robot
+
+    robot = Robot()
+
+    # create task
+    task = SceneVLM(robot)
+    task.reset()
+    task.run()